1279 files changed, 87822 insertions, 39147 deletions

diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 80998958cf45..119d58db8342 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -385,8 +385,8 @@ config ACPI_CUSTOM_METHOD
 	  to override that restriction).
 
 config ACPI_BGRT
-        tristate "Boottime Graphics Resource Table support"
-        default n
+	bool "Boottime Graphics Resource Table support"
+	depends on EFI
         help
 	  This driver adds support for exposing the ACPI Boottime Graphics
 	  Resource Table, which allows the operating system to obtain
diff --git a/drivers/acpi/bgrt.c b/drivers/acpi/bgrt.c
index 6680df36b963..be6039958545 100644
--- a/drivers/acpi/bgrt.c
+++ b/drivers/acpi/bgrt.c
@@ -1,5 +1,6 @@
 /*
  * Copyright 2012 Red Hat, Inc <mjg@redhat.com>
+ * Copyright 2012 Intel Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -11,20 +12,10 @@
 #include <linux/init.h>
 #include <linux/device.h>
 #include <linux/sysfs.h>
-#include <linux/io.h>
-#include <acpi/acpi.h>
-#include <acpi/acpi_bus.h>
+#include <linux/efi-bgrt.h>
 
-static struct acpi_table_bgrt *bgrt_tab;
 static struct kobject *bgrt_kobj;
 
-struct bmp_header {
-	u16 id;
-	u32 size;
-} __attribute ((packed));
-
-static struct bmp_header bmp_header;
-
 static ssize_t show_version(struct device *dev,
 			    struct device_attribute *attr, char *buf)
 {
@@ -63,18 +54,7 @@ static DEVICE_ATTR(yoffset, S_IRUGO, show_yoffset, NULL);
 static ssize_t show_image(struct file *file, struct kobject *kobj,
 	       struct bin_attribute *attr, char *buf, loff_t off, size_t count)
 {
-	int size = attr->size;
-	void __iomem *image = attr->private;
-
-	if (off >= size) {
-		count = 0;
-	} else {
-		if (off + count > size)
-			count = size - off;
-
-		memcpy_fromio(buf, image+off, count);
-	}
-
+	memcpy(buf, attr->private + off, count);
 	return count;
 }
 
@@ -101,45 +81,18 @@ static struct attribute_group bgrt_attribute_group = {
 
 static int __init bgrt_init(void)
 {
-	acpi_status status;
 	int ret;
-	void __iomem *bgrt;
 
-	if (acpi_disabled)
-		return -ENODEV;
-
-	status = acpi_get_table("BGRT", 0,
-				(struct acpi_table_header **)&bgrt_tab);
-
-	if (ACPI_FAILURE(status))
+	if (!bgrt_image)
 		return -ENODEV;
 
 	sysfs_bin_attr_init(&image_attr);
-
-	bgrt = ioremap(bgrt_tab->image_address, sizeof(struct bmp_header));
-
-	if (!bgrt) {
-		ret = -EINVAL;
-		goto out_err;
-	}
-
-	memcpy_fromio(&bmp_header, bgrt, sizeof(bmp_header));
-	image_attr.size = bmp_header.size;
-	iounmap(bgrt);
-
-	image_attr.private = ioremap(bgrt_tab->image_address, image_attr.size);
-
-	if (!image_attr.private) {
-		ret = -EINVAL;
-		goto out_err;
-	}
-
+	image_attr.private = bgrt_image;
+	image_attr.size = bgrt_image_size;
 
 	bgrt_kobj = kobject_create_and_add("bgrt", acpi_kobj);
-	if (!bgrt_kobj) {
-		ret = -EINVAL;
-		goto out_iounmap;
-	}
+	if (!bgrt_kobj)
+		return -EINVAL;
 
 	ret = sysfs_create_group(bgrt_kobj, &bgrt_attribute_group);
 	if (ret)
@@ -155,22 +108,11 @@ out_group:
 	sysfs_remove_group(bgrt_kobj, &bgrt_attribute_group);
 out_kobject:
 	kobject_put(bgrt_kobj);
-out_iounmap:
-	iounmap(image_attr.private);
-out_err:
 	return ret;
 }
 
-static void __exit bgrt_exit(void)
-{
-	iounmap(image_attr.private);
-	sysfs_remove_group(bgrt_kobj, &bgrt_attribute_group);
-	sysfs_remove_bin_file(bgrt_kobj, &image_attr);
-}
-
 module_init(bgrt_init);
-module_exit(bgrt_exit);
 
-MODULE_AUTHOR("Matthew Garrett");
+MODULE_AUTHOR("Matthew Garrett, Josh Triplett <josh@joshtriplett.org>");
 MODULE_DESCRIPTION("BGRT boot graphic support");
 MODULE_LICENSE("GPL");
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 72a2c98bc429..bce469c0b48a 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -27,7 +27,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/types.h>
-#include <linux/spinlock.h>
+#include <linux/mutex.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
 #include <linux/pci.h>
@@ -71,9 +71,11 @@ static struct acpi_driver acpi_pci_root_driver = {
 		},
 };
 
+/* Lock to protect both acpi_pci_roots and acpi_pci_drivers lists */
+static DEFINE_MUTEX(acpi_pci_root_lock);
 static LIST_HEAD(acpi_pci_roots);
+static LIST_HEAD(acpi_pci_drivers);
 
-static struct acpi_pci_driver *sub_driver;
 static DEFINE_MUTEX(osc_lock);
 
 int acpi_pci_register_driver(struct acpi_pci_driver *driver)
@@ -81,55 +83,46 @@ int acpi_pci_register_driver(struct acpi_pci_driver *driver)
 	int n = 0;
 	struct acpi_pci_root *root;
 
-	struct acpi_pci_driver **pptr = &sub_driver;
-	while (*pptr)
-		pptr = &(*pptr)->next;
-	*pptr = driver;
-
-	if (!driver->add)
-		return 0;
-
-	list_for_each_entry(root, &acpi_pci_roots, node) {
-		driver->add(root->device->handle);
-		n++;
-	}
+	mutex_lock(&acpi_pci_root_lock);
+	list_add_tail(&driver->node, &acpi_pci_drivers);
+	if (driver->add)
+		list_for_each_entry(root, &acpi_pci_roots, node) {
+			driver->add(root);
+			n++;
+		}
+	mutex_unlock(&acpi_pci_root_lock);
 
 	return n;
 }
-
 EXPORT_SYMBOL(acpi_pci_register_driver);
 
 void acpi_pci_unregister_driver(struct acpi_pci_driver *driver)
 {
 	struct acpi_pci_root *root;
 
-	struct acpi_pci_driver **pptr = &sub_driver;
-	while (*pptr) {
-		if (*pptr == driver)
-			break;
-		pptr = &(*pptr)->next;
-	}
-	BUG_ON(!*pptr);
-	*pptr = (*pptr)->next;
-
-	if (!driver->remove)
-		return;
-
-	list_for_each_entry(root, &acpi_pci_roots, node)
-		driver->remove(root->device->handle);
+	mutex_lock(&acpi_pci_root_lock);
+	list_del(&driver->node);
+	if (driver->remove)
+		list_for_each_entry(root, &acpi_pci_roots, node)
+			driver->remove(root);
+	mutex_unlock(&acpi_pci_root_lock);
 }
-
 EXPORT_SYMBOL(acpi_pci_unregister_driver);
 
 acpi_handle acpi_get_pci_rootbridge_handle(unsigned int seg, unsigned int bus)
 {
 	struct acpi_pci_root *root;
+	acpi_handle handle = NULL;
 	
+	mutex_lock(&acpi_pci_root_lock);
 	list_for_each_entry(root, &acpi_pci_roots, node)
 		if ((root->segment == (u16) seg) &&
-		    (root->secondary.start == (u16) bus))
-			return root->device->handle;
-	return NULL;		
+		    (root->secondary.start == (u16) bus)) {
+			handle = root->device->handle;
+			break;
+		}
+	mutex_unlock(&acpi_pci_root_lock);
+	return handle;
 }
 
 EXPORT_SYMBOL_GPL(acpi_get_pci_rootbridge_handle);
@@ -277,12 +270,15 @@ static acpi_status acpi_pci_osc_support(struct acpi_pci_root *root, u32 flags)
 struct acpi_pci_root *acpi_pci_find_root(acpi_handle handle)
 {
 	struct acpi_pci_root *root;
+	struct acpi_device *device;
 
-	list_for_each_entry(root, &acpi_pci_roots, node) {
-		if (root->device->handle == handle)
-			return root;
-	}
-	return NULL;
+	if (acpi_bus_get_device(handle, &device) ||
+	    acpi_match_device_ids(device, root_device_ids))
+		return NULL;
+
+	root = acpi_driver_data(device);
+
+	return root;
 }
 EXPORT_SYMBOL_GPL(acpi_pci_find_root);
 
@@ -518,8 +514,9 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 	 * TBD: Need PCI interface for enumeration/configuration of roots.
 	 */
 
-	/* TBD: Locking */
+	mutex_lock(&acpi_pci_root_lock);
 	list_add_tail(&root->node, &acpi_pci_roots);
+	mutex_unlock(&acpi_pci_root_lock);
 
 	printk(KERN_INFO PREFIX "%s [%s] (domain %04x %pR)\n",
 	       acpi_device_name(device), acpi_device_bid(device),
@@ -538,7 +535,7 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 			    "Bus %04x:%02x not present in PCI namespace\n",
 			    root->segment, (unsigned int)root->secondary.start);
 		result = -ENODEV;
-		goto end;
+		goto out_del_root;
 	}
 
 	/*
@@ -548,7 +545,7 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 	 */
 	result = acpi_pci_bind_root(device);
 	if (result)
-		goto end;
+		goto out_del_root;
 
 	/*
 	 * PCI Routing Table
@@ -633,9 +630,11 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 
 	return 0;
 
+out_del_root:
+	mutex_lock(&acpi_pci_root_lock);
+	list_del(&root->node);
+	mutex_unlock(&acpi_pci_root_lock);
 end:
-	if (!list_empty(&root->node))
-		list_del(&root->node);
 	kfree(root);
 	return result;
 }
@@ -643,18 +642,34 @@ end:
 static int acpi_pci_root_start(struct acpi_device *device)
 {
 	struct acpi_pci_root *root = acpi_driver_data(device);
+	struct acpi_pci_driver *driver;
+
+	mutex_lock(&acpi_pci_root_lock);
+	list_for_each_entry(driver, &acpi_pci_drivers, node)
+		if (driver->add)
+			driver->add(root);
+	mutex_unlock(&acpi_pci_root_lock);
 
 	pci_bus_add_devices(root->bus);
+
 	return 0;
 }
 
 static int acpi_pci_root_remove(struct acpi_device *device, int type)
 {
 	struct acpi_pci_root *root = acpi_driver_data(device);
+	struct acpi_pci_driver *driver;
+
+	mutex_lock(&acpi_pci_root_lock);
+	list_for_each_entry(driver, &acpi_pci_drivers, node)
+		if (driver->remove)
+			driver->remove(root);
 
 	device_set_run_wake(root->bus->bridge, false);
 	pci_acpi_remove_bus_pm_notifier(device);
 
+	list_del(&root->node);
+	mutex_unlock(&acpi_pci_root_lock);
 	kfree(root);
 	return 0;
 }
diff --git a/drivers/acpi/pci_slot.c b/drivers/acpi/pci_slot.c
index e50e31a518af..d22585f21aeb 100644
--- a/drivers/acpi/pci_slot.c
+++ b/drivers/acpi/pci_slot.c
@@ -67,8 +67,8 @@ struct acpi_pci_slot {
 	struct list_head list;		/* node in the list of slots */
 };
 
-static int acpi_pci_slot_add(acpi_handle handle);
-static void acpi_pci_slot_remove(acpi_handle handle);
+static int acpi_pci_slot_add(struct acpi_pci_root *root);
+static void acpi_pci_slot_remove(struct acpi_pci_root *root);
 
 static LIST_HEAD(slot_list);
 static DEFINE_MUTEX(slot_list_lock);
@@ -233,45 +233,20 @@ out:
 
 /*
  * walk_root_bridge - generic root bridge walker
- * @handle: points to an acpi_pci_root
+ * @root: poiner of an acpi_pci_root
  * @user_function: user callback for slot objects
  *
  * Call user_function for all objects underneath this root bridge.
  * Walk p2p bridges underneath us and call user_function on those too.
  */
 static int
-walk_root_bridge(acpi_handle handle, acpi_walk_callback user_function)
+walk_root_bridge(struct acpi_pci_root *root, acpi_walk_callback user_function)
 {
-	int seg, bus;
-	unsigned long long tmp;
 	acpi_status status;
-	acpi_handle dummy_handle;
-	struct pci_bus *pci_bus;
+	acpi_handle handle = root->device->handle;
+	struct pci_bus *pci_bus = root->bus;
 	struct callback_args context;
 
-	/* If the bridge doesn't have _STA, we assume it is always there */
-	status = acpi_get_handle(handle, "_STA", &dummy_handle);
-	if (ACPI_SUCCESS(status)) {
-		status = acpi_evaluate_integer(handle, "_STA", NULL, &tmp);
-		if (ACPI_FAILURE(status)) {
-			info("%s: _STA evaluation failure\n", __func__);
-			return 0;
-		}
-		if ((tmp & ACPI_STA_DEVICE_FUNCTIONING) == 0)
-			/* don't register this object */
-			return 0;
-	}
-
-	status = acpi_evaluate_integer(handle, "_SEG", NULL, &tmp);
-	seg = ACPI_SUCCESS(status) ? tmp : 0;
-
-	status = acpi_evaluate_integer(handle, "_BBN", NULL, &tmp);
-	bus = ACPI_SUCCESS(status) ? tmp : 0;
-
-	pci_bus = pci_find_bus(seg, bus);
-	if (!pci_bus)
-		return 0;
-
 	context.pci_bus = pci_bus;
 	context.user_function = user_function;
 	context.root_handle = handle;
@@ -295,11 +270,11 @@ walk_root_bridge(acpi_handle handle, acpi_walk_callback user_function)
  * @handle: points to an acpi_pci_root
  */
 static int
-acpi_pci_slot_add(acpi_handle handle)
+acpi_pci_slot_add(struct acpi_pci_root *root)
 {
 	acpi_status status;
 
-	status = walk_root_bridge(handle, register_slot);
+	status = walk_root_bridge(root, register_slot);
 	if (ACPI_FAILURE(status))
 		err("%s: register_slot failure - %d\n", __func__, status);
 
@@ -311,10 +286,11 @@ acpi_pci_slot_add(acpi_handle handle)
  * @handle: points to an acpi_pci_root
  */
 static void
-acpi_pci_slot_remove(acpi_handle handle)
+acpi_pci_slot_remove(struct acpi_pci_root *root)
 {
 	struct acpi_pci_slot *slot, *tmp;
 	struct pci_bus *pbus;
+	acpi_handle handle = root->device->handle;
 
 	mutex_lock(&slot_list_lock);
 	list_for_each_entry_safe(slot, tmp, &slot_list, list) {
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 5e6e00bc1652..abea76c36a4b 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -184,6 +184,17 @@ static void device_release(struct kobject *kobj)
 	struct device *dev = kobj_to_dev(kobj);
 	struct device_private *p = dev->p;
 
+	/*
+	 * Some platform devices are driven without driver attached
+	 * and managed resources may have been acquired.  Make sure
+	 * all resources are released.
+	 *
+	 * Drivers still can add resources into device after device
+	 * is deleted but alive, so release devres here to avoid
+	 * possible memory leak.
+	 */
+	devres_release_all(dev);
+
 	if (dev->release)
 		dev->release(dev);
 	else if (dev->type && dev->type->release)
@@ -1196,13 +1207,6 @@ void device_del(struct device *dev)
 	bus_remove_device(dev);
 	driver_deferred_probe_del(dev);
 
-	/*
-	 * Some platform devices are driven without driver attached
-	 * and managed resources may have been acquired.  Make sure
-	 * all resources are released.
-	 */
-	devres_release_all(dev);
-
 	/* Notify the platform of the removal, in case they
 	 * need to do anything...
 	 */
@@ -1861,26 +1865,20 @@ void device_shutdown(void)
  */
 
 #ifdef CONFIG_PRINTK
-int __dev_printk(const char *level, const struct device *dev,
-		 struct va_format *vaf)
+static int
+create_syslog_header(const struct device *dev, char *hdr, size_t hdrlen)
 {
-	char dict[128];
-	const char *level_extra = "";
-	size_t dictlen = 0;
 	const char *subsys;
-
-	if (!dev)
-		return printk("%s(NULL device *): %pV", level, vaf);
+	size_t pos = 0;
 
 	if (dev->class)
 		subsys = dev->class->name;
 	else if (dev->bus)
 		subsys = dev->bus->name;
 	else
-		goto skip;
+		return 0;
 
-	dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
-			    "SUBSYSTEM=%s", subsys);
+	pos += snprintf(hdr + pos, hdrlen - pos, "SUBSYSTEM=%s", subsys);
 
 	/*
 	 * Add device identifier DEVICE=:
@@ -1896,32 +1894,63 @@ int __dev_printk(const char *level, const struct device *dev,
 			c = 'b';
 		else
 			c = 'c';
-		dictlen++;
-		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
-				   "DEVICE=%c%u:%u",
-				   c, MAJOR(dev->devt), MINOR(dev->devt));
+		pos++;
+		pos += snprintf(hdr + pos, hdrlen - pos,
+				"DEVICE=%c%u:%u",
+				c, MAJOR(dev->devt), MINOR(dev->devt));
 	} else if (strcmp(subsys, "net") == 0) {
 		struct net_device *net = to_net_dev(dev);
 
-		dictlen++;
-		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
-				    "DEVICE=n%u", net->ifindex);
+		pos++;
+		pos += snprintf(hdr + pos, hdrlen - pos,
+				"DEVICE=n%u", net->ifindex);
 	} else {
-		dictlen++;
-		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
-				    "DEVICE=+%s:%s", subsys, dev_name(dev));
+		pos++;
+		pos += snprintf(hdr + pos, hdrlen - pos,
+				"DEVICE=+%s:%s", subsys, dev_name(dev));
 	}
-skip:
-	if (level[2])
-		level_extra = &level[2]; /* skip past KERN_SOH "L" */
 
-	return printk_emit(0, level[1] - '0',
-			   dictlen ? dict : NULL, dictlen,
-			   "%s %s: %s%pV",
-			   dev_driver_string(dev), dev_name(dev),
-			   level_extra, vaf);
+	return pos;
+}
+EXPORT_SYMBOL(create_syslog_header);
+
+int dev_vprintk_emit(int level, const struct device *dev,
+		     const char *fmt, va_list args)
+{
+	char hdr[128];
+	size_t hdrlen;
+
+	hdrlen = create_syslog_header(dev, hdr, sizeof(hdr));
+
+	return vprintk_emit(0, level, hdrlen ? hdr : NULL, hdrlen, fmt, args);
+}
+EXPORT_SYMBOL(dev_vprintk_emit);
+
+int dev_printk_emit(int level, const struct device *dev, const char *fmt, ...)
+{
+	va_list args;
+	int r;
+
+	va_start(args, fmt);
+
+	r = dev_vprintk_emit(level, dev, fmt, args);
+
+	va_end(args);
+
+	return r;
+}
+EXPORT_SYMBOL(dev_printk_emit);
+
+static int __dev_printk(const char *level, const struct device *dev,
+			struct va_format *vaf)
+{
+	if (!dev)
+		return printk("%s(NULL device *): %pV", level, vaf);
+
+	return dev_printk_emit(level[1] - '0', dev,
+			       "%s %s: %pV",
+			       dev_driver_string(dev), dev_name(dev), vaf);
 }
-EXPORT_SYMBOL(__dev_printk);
 
 int dev_printk(const char *level, const struct device *dev,
 	       const char *fmt, ...)
@@ -1936,6 +1965,7 @@ int dev_printk(const char *level, const struct device *dev,
 	vaf.va = &args;
 
 	r = __dev_printk(level, dev, &vaf);
+
 	va_end(args);
 
 	return r;
@@ -1955,6 +1985,7 @@ int func(const struct device *dev, const char *fmt, ...)	\
 	vaf.va = &args;						\
 								\
 	r = __dev_printk(kern_level, dev, &vaf);		\
+								\
 	va_end(args);						\
 								\
 	return r;						\
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index 2360adb7a58f..8731979d668a 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -144,6 +144,48 @@ EXPORT_SYMBOL_GPL(devres_alloc);
 #endif
 
 /**
+ * devres_for_each_res - Resource iterator
+ * @dev: Device to iterate resource from
+ * @release: Look for resources associated with this release function
+ * @match: Match function (optional)
+ * @match_data: Data for the match function
+ * @fn: Function to be called for each matched resource.
+ * @data: Data for @fn, the 3rd parameter of @fn
+ *
+ * Call @fn for each devres of @dev which is associated with @release
+ * and for which @match returns 1.
+ *
+ * RETURNS:
+ * 	void
+ */
+void devres_for_each_res(struct device *dev, dr_release_t release,
+			dr_match_t match, void *match_data,
+			void (*fn)(struct device *, void *, void *),
+			void *data)
+{
+	struct devres_node *node;
+	struct devres_node *tmp;
+	unsigned long flags;
+
+	if (!fn)
+		return;
+
+	spin_lock_irqsave(&dev->devres_lock, flags);
+	list_for_each_entry_safe_reverse(node, tmp,
+			&dev->devres_head, entry) {
+		struct devres *dr = container_of(node, struct devres, node);
+
+		if (node->release != release)
+			continue;
+		if (match && !match(dev, dr->data, match_data))
+			continue;
+		fn(dev, dr->data, data);
+	}
+	spin_unlock_irqrestore(&dev->devres_lock, flags);
+}
+EXPORT_SYMBOL_GPL(devres_for_each_res);
+
+/**
  * devres_free - Free device resource data
  * @res: Pointer to devres data to free
  *
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 803cfc1597a9..6e210802c37b 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -21,6 +21,13 @@
 #include <linux/firmware.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <linux/list.h>
+#include <linux/async.h>
+#include <linux/pm.h>
+#include <linux/suspend.h>
+#include <linux/syscore_ops.h>
+
+#include "base.h"
 
 MODULE_AUTHOR("Manuel Estrada Sainz");
 MODULE_DESCRIPTION("Multi purpose firmware loading support");
@@ -85,23 +92,168 @@ static inline long firmware_loading_timeout(void)
 	return loading_timeout > 0 ? loading_timeout * HZ : MAX_SCHEDULE_TIMEOUT;
 }
 
-/* fw_lock could be moved to 'struct firmware_priv' but since it is just
- * guarding for corner cases a global lock should be OK */
-static DEFINE_MUTEX(fw_lock);
+struct firmware_cache {
+	/* firmware_buf instance will be added into the below list */
+	spinlock_t lock;
+	struct list_head head;
+	int state;
+
+#ifdef CONFIG_PM_SLEEP
+	/*
+	 * Names of firmware images which have been cached successfully
+	 * will be added into the below list so that device uncache
+	 * helper can trace which firmware images have been cached
+	 * before.
+	 */
+	spinlock_t name_lock;
+	struct list_head fw_names;
+
+	wait_queue_head_t wait_queue;
+	int cnt;
+	struct delayed_work work;
+
+	struct notifier_block   pm_notify;
+#endif
+};
 
-struct firmware_priv {
+struct firmware_buf {
+	struct kref ref;
+	struct list_head list;
 	struct completion completion;
-	struct firmware *fw;
+	struct firmware_cache *fwc;
 	unsigned long status;
+	void *data;
+	size_t size;
 	struct page **pages;
 	int nr_pages;
 	int page_array_size;
+	char fw_id[];
+};
+
+struct fw_cache_entry {
+	struct list_head list;
+	char name[];
+};
+
+struct firmware_priv {
 	struct timer_list timeout;
-	struct device dev;
 	bool nowait;
-	char fw_id[];
+	struct device dev;
+	struct firmware_buf *buf;
+	struct firmware *fw;
+};
+
+struct fw_name_devm {
+	unsigned long magic;
+	char name[];
 };
 
+#define to_fwbuf(d) container_of(d, struct firmware_buf, ref)
+
+#define	FW_LOADER_NO_CACHE	0
+#define	FW_LOADER_START_CACHE	1
+
+static int fw_cache_piggyback_on_request(const char *name);
+
+/* fw_lock could be moved to 'struct firmware_priv' but since it is just
+ * guarding for corner cases a global lock should be OK */
+static DEFINE_MUTEX(fw_lock);
+
+static struct firmware_cache fw_cache;
+
+static struct firmware_buf *__allocate_fw_buf(const char *fw_name,
+					      struct firmware_cache *fwc)
+{
+	struct firmware_buf *buf;
+
+	buf = kzalloc(sizeof(*buf) + strlen(fw_name) + 1 , GFP_ATOMIC);
+
+	if (!buf)
+		return buf;
+
+	kref_init(&buf->ref);
+	strcpy(buf->fw_id, fw_name);
+	buf->fwc = fwc;
+	init_completion(&buf->completion);
+
+	pr_debug("%s: fw-%s buf=%p\n", __func__, fw_name, buf);
+
+	return buf;
+}
+
+static struct firmware_buf *__fw_lookup_buf(const char *fw_name)
+{
+	struct firmware_buf *tmp;
+	struct firmware_cache *fwc = &fw_cache;
+
+	list_for_each_entry(tmp, &fwc->head, list)
+		if (!strcmp(tmp->fw_id, fw_name))
+			return tmp;
+	return NULL;
+}
+
+static int fw_lookup_and_allocate_buf(const char *fw_name,
+				      struct firmware_cache *fwc,
+				      struct firmware_buf **buf)
+{
+	struct firmware_buf *tmp;
+
+	spin_lock(&fwc->lock);
+	tmp = __fw_lookup_buf(fw_name);
+	if (tmp) {
+		kref_get(&tmp->ref);
+		spin_unlock(&fwc->lock);
+		*buf = tmp;
+		return 1;
+	}
+	tmp = __allocate_fw_buf(fw_name, fwc);
+	if (tmp)
+		list_add(&tmp->list, &fwc->head);
+	spin_unlock(&fwc->lock);
+
+	*buf = tmp;
+
+	return tmp ? 0 : -ENOMEM;
+}
+
+static struct firmware_buf *fw_lookup_buf(const char *fw_name)
+{
+	struct firmware_buf *tmp;
+	struct firmware_cache *fwc = &fw_cache;
+
+	spin_lock(&fwc->lock);
+	tmp = __fw_lookup_buf(fw_name);
+	spin_unlock(&fwc->lock);
+
+	return tmp;
+}
+
+static void __fw_free_buf(struct kref *ref)
+{
+	struct firmware_buf *buf = to_fwbuf(ref);
+	struct firmware_cache *fwc = buf->fwc;
+	int i;
+
+	pr_debug("%s: fw-%s buf=%p data=%p size=%u\n",
+		 __func__, buf->fw_id, buf, buf->data,
+		 (unsigned int)buf->size);
+
+	spin_lock(&fwc->lock);
+	list_del(&buf->list);
+	spin_unlock(&fwc->lock);
+
+	vunmap(buf->data);
+	for (i = 0; i < buf->nr_pages; i++)
+		__free_page(buf->pages[i]);
+	kfree(buf->pages);
+	kfree(buf);
+}
+
+static void fw_free_buf(struct firmware_buf *buf)
+{
+	kref_put(&buf->ref, __fw_free_buf);
+}
+
 static struct firmware_priv *to_firmware_priv(struct device *dev)
 {
 	return container_of(dev, struct firmware_priv, dev);
@@ -109,9 +261,10 @@ static struct firmware_priv *to_firmware_priv(struct device *dev)
 
 static void fw_load_abort(struct firmware_priv *fw_priv)
 {
-	set_bit(FW_STATUS_ABORT, &fw_priv->status);
-	wmb();
-	complete(&fw_priv->completion);
+	struct firmware_buf *buf = fw_priv->buf;
+
+	set_bit(FW_STATUS_ABORT, &buf->status);
+	complete_all(&buf->completion);
 }
 
 static ssize_t firmware_timeout_show(struct class *class,
@@ -154,11 +307,7 @@ static struct class_attribute firmware_class_attrs[] = {
 static void fw_dev_release(struct device *dev)
 {
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
-	int i;
 
-	for (i = 0; i < fw_priv->nr_pages; i++)
-		__free_page(fw_priv->pages[i]);
-	kfree(fw_priv->pages);
 	kfree(fw_priv);
 
 	module_put(THIS_MODULE);
@@ -168,7 +317,7 @@ static int firmware_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
 
-	if (add_uevent_var(env, "FIRMWARE=%s", fw_priv->fw_id))
+	if (add_uevent_var(env, "FIRMWARE=%s", fw_priv->buf->fw_id))
 		return -ENOMEM;
 	if (add_uevent_var(env, "TIMEOUT=%i", loading_timeout))
 		return -ENOMEM;
@@ -189,20 +338,16 @@ static ssize_t firmware_loading_show(struct device *dev,
 				     struct device_attribute *attr, char *buf)
 {
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
-	int loading = test_bit(FW_STATUS_LOADING, &fw_priv->status);
+	int loading = test_bit(FW_STATUS_LOADING, &fw_priv->buf->status);
 
 	return sprintf(buf, "%d\n", loading);
 }
 
+/* firmware holds the ownership of pages */
 static void firmware_free_data(const struct firmware *fw)
 {
-	int i;
-	vunmap(fw->data);
-	if (fw->pages) {
-		for (i = 0; i < PFN_UP(fw->size); i++)
-			__free_page(fw->pages[i]);
-		kfree(fw->pages);
-	}
+	WARN_ON(!fw->priv);
+	fw_free_buf(fw->priv);
 }
 
 /* Some architectures don't have PAGE_KERNEL_RO */
@@ -227,45 +372,33 @@ static ssize_t firmware_loading_store(struct device *dev,
 				      const char *buf, size_t count)
 {
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
+	struct firmware_buf *fw_buf = fw_priv->buf;
 	int loading = simple_strtol(buf, NULL, 10);
 	int i;
 
 	mutex_lock(&fw_lock);
 
-	if (!fw_priv->fw)
+	if (!fw_buf)
 		goto out;
 
 	switch (loading) {
 	case 1:
-		firmware_free_data(fw_priv->fw);
-		memset(fw_priv->fw, 0, sizeof(struct firmware));
-		/* If the pages are not owned by 'struct firmware' */
-		for (i = 0; i < fw_priv->nr_pages; i++)
-			__free_page(fw_priv->pages[i]);
-		kfree(fw_priv->pages);
-		fw_priv->pages = NULL;
-		fw_priv->page_array_size = 0;
-		fw_priv->nr_pages = 0;
-		set_bit(FW_STATUS_LOADING, &fw_priv->status);
+		/* discarding any previous partial load */
+		if (!test_bit(FW_STATUS_DONE, &fw_buf->status)) {
+			for (i = 0; i < fw_buf->nr_pages; i++)
+				__free_page(fw_buf->pages[i]);
+			kfree(fw_buf->pages);
+			fw_buf->pages = NULL;
+			fw_buf->page_array_size = 0;
+			fw_buf->nr_pages = 0;
+			set_bit(FW_STATUS_LOADING, &fw_buf->status);
+		}
 		break;
 	case 0:
-		if (test_bit(FW_STATUS_LOADING, &fw_priv->status)) {
-			vunmap(fw_priv->fw->data);
-			fw_priv->fw->data = vmap(fw_priv->pages,
-						 fw_priv->nr_pages,
-						 0, PAGE_KERNEL_RO);
-			if (!fw_priv->fw->data) {
-				dev_err(dev, "%s: vmap() failed\n", __func__);
-				goto err;
-			}
-			/* Pages are now owned by 'struct firmware' */
-			fw_priv->fw->pages = fw_priv->pages;
-			fw_priv->pages = NULL;
-
-			fw_priv->page_array_size = 0;
-			fw_priv->nr_pages = 0;
-			complete(&fw_priv->completion);
-			clear_bit(FW_STATUS_LOADING, &fw_priv->status);
+		if (test_bit(FW_STATUS_LOADING, &fw_buf->status)) {
+			set_bit(FW_STATUS_DONE, &fw_buf->status);
+			clear_bit(FW_STATUS_LOADING, &fw_buf->status);
+			complete_all(&fw_buf->completion);
 			break;
 		}
 		/* fallthrough */
@@ -273,7 +406,6 @@ static ssize_t firmware_loading_store(struct device *dev,
 		dev_err(dev, "%s: unexpected value (%d)\n", __func__, loading);
 		/* fallthrough */
 	case -1:
-	err:
 		fw_load_abort(fw_priv);
 		break;
 	}
@@ -290,21 +422,21 @@ static ssize_t firmware_data_read(struct file *filp, struct kobject *kobj,
 {
 	struct device *dev = kobj_to_dev(kobj);
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
-	struct firmware *fw;
+	struct firmware_buf *buf;
 	ssize_t ret_count;
 
 	mutex_lock(&fw_lock);
-	fw = fw_priv->fw;
-	if (!fw || test_bit(FW_STATUS_DONE, &fw_priv->status)) {
+	buf = fw_priv->buf;
+	if (!buf || test_bit(FW_STATUS_DONE, &buf->status)) {
 		ret_count = -ENODEV;
 		goto out;
 	}
-	if (offset > fw->size) {
+	if (offset > buf->size) {
 		ret_count = 0;
 		goto out;
 	}
-	if (count > fw->size - offset)
-		count = fw->size - offset;
+	if (count > buf->size - offset)
+		count = buf->size - offset;
 
 	ret_count = count;
 
@@ -314,11 +446,11 @@ static ssize_t firmware_data_read(struct file *filp, struct kobject *kobj,
 		int page_ofs = offset & (PAGE_SIZE-1);
 		int page_cnt = min_t(size_t, PAGE_SIZE - page_ofs, count);
 
-		page_data = kmap(fw_priv->pages[page_nr]);
+		page_data = kmap(buf->pages[page_nr]);
 
 		memcpy(buffer, page_data + page_ofs, page_cnt);
 
-		kunmap(fw_priv->pages[page_nr]);
+		kunmap(buf->pages[page_nr]);
 		buffer += page_cnt;
 		offset += page_cnt;
 		count -= page_cnt;
@@ -330,12 +462,13 @@ out:
 
 static int fw_realloc_buffer(struct firmware_priv *fw_priv, int min_size)
 {
+	struct firmware_buf *buf = fw_priv->buf;
 	int pages_needed = ALIGN(min_size, PAGE_SIZE) >> PAGE_SHIFT;
 
 	/* If the array of pages is too small, grow it... */
-	if (fw_priv->page_array_size < pages_needed) {
+	if (buf->page_array_size < pages_needed) {
 		int new_array_size = max(pages_needed,
-					 fw_priv->page_array_size * 2);
+					 buf->page_array_size * 2);
 		struct page **new_pages;
 
 		new_pages = kmalloc(new_array_size * sizeof(void *),
@@ -344,24 +477,24 @@ static int fw_realloc_buffer(struct firmware_priv *fw_priv, int min_size)
 			fw_load_abort(fw_priv);
 			return -ENOMEM;
 		}
-		memcpy(new_pages, fw_priv->pages,
-		       fw_priv->page_array_size * sizeof(void *));
-		memset(&new_pages[fw_priv->page_array_size], 0, sizeof(void *) *
-		       (new_array_size - fw_priv->page_array_size));
-		kfree(fw_priv->pages);
-		fw_priv->pages = new_pages;
-		fw_priv->page_array_size = new_array_size;
+		memcpy(new_pages, buf->pages,
+		       buf->page_array_size * sizeof(void *));
+		memset(&new_pages[buf->page_array_size], 0, sizeof(void *) *
+		       (new_array_size - buf->page_array_size));
+		kfree(buf->pages);
+		buf->pages = new_pages;
+		buf->page_array_size = new_array_size;
 	}
 
-	while (fw_priv->nr_pages < pages_needed) {
-		fw_priv->pages[fw_priv->nr_pages] =
+	while (buf->nr_pages < pages_needed) {
+		buf->pages[buf->nr_pages] =
 			alloc_page(GFP_KERNEL | __GFP_HIGHMEM);
 
-		if (!fw_priv->pages[fw_priv->nr_pages]) {
+		if (!buf->pages[buf->nr_pages]) {
 			fw_load_abort(fw_priv);
 			return -ENOMEM;
 		}
-		fw_priv->nr_pages++;
+		buf->nr_pages++;
 	}
 	return 0;
 }
@@ -384,18 +517,19 @@ static ssize_t firmware_data_write(struct file *filp, struct kobject *kobj,
 {
 	struct device *dev = kobj_to_dev(kobj);
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
-	struct firmware *fw;
+	struct firmware_buf *buf;
 	ssize_t retval;
 
 	if (!capable(CAP_SYS_RAWIO))
 		return -EPERM;
 
 	mutex_lock(&fw_lock);
-	fw = fw_priv->fw;
-	if (!fw || test_bit(FW_STATUS_DONE, &fw_priv->status)) {
+	buf = fw_priv->buf;
+	if (!buf || test_bit(FW_STATUS_DONE, &buf->status)) {
 		retval = -ENODEV;
 		goto out;
 	}
+
 	retval = fw_realloc_buffer(fw_priv, offset + count);
 	if (retval)
 		goto out;
@@ -408,17 +542,17 @@ static ssize_t firmware_data_write(struct file *filp, struct kobject *kobj,
 		int page_ofs = offset & (PAGE_SIZE - 1);
 		int page_cnt = min_t(size_t, PAGE_SIZE - page_ofs, count);
 
-		page_data = kmap(fw_priv->pages[page_nr]);
+		page_data = kmap(buf->pages[page_nr]);
 
 		memcpy(page_data + page_ofs, buffer, page_cnt);
 
-		kunmap(fw_priv->pages[page_nr]);
+		kunmap(buf->pages[page_nr]);
 		buffer += page_cnt;
 		offset += page_cnt;
 		count -= page_cnt;
 	}
 
-	fw->size = max_t(size_t, offset, fw->size);
+	buf->size = max_t(size_t, offset, buf->size);
 out:
 	mutex_unlock(&fw_lock);
 	return retval;
@@ -445,35 +579,120 @@ fw_create_instance(struct firmware *firmware, const char *fw_name,
 	struct firmware_priv *fw_priv;
 	struct device *f_dev;
 
-	fw_priv = kzalloc(sizeof(*fw_priv) + strlen(fw_name) + 1 , GFP_KERNEL);
+	fw_priv = kzalloc(sizeof(*fw_priv), GFP_KERNEL);
 	if (!fw_priv) {
 		dev_err(device, "%s: kmalloc failed\n", __func__);
-		return ERR_PTR(-ENOMEM);
+		fw_priv = ERR_PTR(-ENOMEM);
+		goto exit;
 	}
 
-	fw_priv->fw = firmware;
 	fw_priv->nowait = nowait;
-	strcpy(fw_priv->fw_id, fw_name);
-	init_completion(&fw_priv->completion);
+	fw_priv->fw = firmware;
 	setup_timer(&fw_priv->timeout,
 		    firmware_class_timeout, (u_long) fw_priv);
 
 	f_dev = &fw_priv->dev;
 
 	device_initialize(f_dev);
-	dev_set_name(f_dev, "%s", dev_name(device));
+	dev_set_name(f_dev, "%s", fw_name);
 	f_dev->parent = device;
 	f_dev->class = &firmware_class;
-
+exit:
 	return fw_priv;
 }
 
+/* one pages buffer is mapped/unmapped only once */
+static int fw_map_pages_buf(struct firmware_buf *buf)
+{
+	buf->data = vmap(buf->pages, buf->nr_pages, 0, PAGE_KERNEL_RO);
+	if (!buf->data)
+		return -ENOMEM;
+	return 0;
+}
+
+/* store the pages buffer info firmware from buf */
+static void fw_set_page_data(struct firmware_buf *buf, struct firmware *fw)
+{
+	fw->priv = buf;
+	fw->pages = buf->pages;
+	fw->size = buf->size;
+	fw->data = buf->data;
+
+	pr_debug("%s: fw-%s buf=%p data=%p size=%u\n",
+		 __func__, buf->fw_id, buf, buf->data,
+		 (unsigned int)buf->size);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static void fw_name_devm_release(struct device *dev, void *res)
+{
+	struct fw_name_devm *fwn = res;
+
+	if (fwn->magic == (unsigned long)&fw_cache)
+		pr_debug("%s: fw_name-%s devm-%p released\n",
+				__func__, fwn->name, res);
+}
+
+static int fw_devm_match(struct device *dev, void *res,
+		void *match_data)
+{
+	struct fw_name_devm *fwn = res;
+
+	return (fwn->magic == (unsigned long)&fw_cache) &&
+		!strcmp(fwn->name, match_data);
+}
+
+static struct fw_name_devm *fw_find_devm_name(struct device *dev,
+		const char *name)
+{
+	struct fw_name_devm *fwn;
+
+	fwn = devres_find(dev, fw_name_devm_release,
+			  fw_devm_match, (void *)name);
+	return fwn;
+}
+
+/* add firmware name into devres list */
+static int fw_add_devm_name(struct device *dev, const char *name)
+{
+	struct fw_name_devm *fwn;
+
+	fwn = fw_find_devm_name(dev, name);
+	if (fwn)
+		return 1;
+
+	fwn = devres_alloc(fw_name_devm_release, sizeof(struct fw_name_devm) +
+			   strlen(name) + 1, GFP_KERNEL);
+	if (!fwn)
+		return -ENOMEM;
+
+	fwn->magic = (unsigned long)&fw_cache;
+	strcpy(fwn->name, name);
+	devres_add(dev, fwn);
+
+	return 0;
+}
+#else
+static int fw_add_devm_name(struct device *dev, const char *name)
+{
+	return 0;
+}
+#endif
+
+static void _request_firmware_cleanup(const struct firmware **firmware_p)
+{
+	release_firmware(*firmware_p);
+	*firmware_p = NULL;
+}
+
 static struct firmware_priv *
 _request_firmware_prepare(const struct firmware **firmware_p, const char *name,
 			  struct device *device, bool uevent, bool nowait)
 {
 	struct firmware *firmware;
-	struct firmware_priv *fw_priv;
+	struct firmware_priv *fw_priv = NULL;
+	struct firmware_buf *buf;
+	int ret;
 
 	if (!firmware_p)
 		return ERR_PTR(-EINVAL);
@@ -490,18 +709,46 @@ _request_firmware_prepare(const struct firmware **firmware_p, const char *name,
 		return NULL;
 	}
 
-	fw_priv = fw_create_instance(firmware, name, device, uevent, nowait);
-	if (IS_ERR(fw_priv)) {
-		release_firmware(firmware);
+	ret = fw_lookup_and_allocate_buf(name, &fw_cache, &buf);
+	if (!ret)
+		fw_priv = fw_create_instance(firmware, name, device,
+				uevent, nowait);
+
+	if (IS_ERR(fw_priv) || ret < 0) {
+		kfree(firmware);
 		*firmware_p = NULL;
+		return ERR_PTR(-ENOMEM);
+	} else if (fw_priv) {
+		fw_priv->buf = buf;
+
+		/*
+		 * bind with 'buf' now to avoid warning in failure path
+		 * of requesting firmware.
+		 */
+		firmware->priv = buf;
+		return fw_priv;
 	}
-	return fw_priv;
-}
 
-static void _request_firmware_cleanup(const struct firmware **firmware_p)
-{
-	release_firmware(*firmware_p);
-	*firmware_p = NULL;
+	/* share the cached buf, which is inprogessing or completed */
+ check_status:
+	mutex_lock(&fw_lock);
+	if (test_bit(FW_STATUS_ABORT, &buf->status)) {
+		fw_priv = ERR_PTR(-ENOENT);
+		firmware->priv = buf;
+		_request_firmware_cleanup(firmware_p);
+		goto exit;
+	} else if (test_bit(FW_STATUS_DONE, &buf->status)) {
+		fw_priv = NULL;
+		fw_set_page_data(buf, firmware);
+		goto exit;
+	}
+	mutex_unlock(&fw_lock);
+	wait_for_completion(&buf->completion);
+	goto check_status;
+
+exit:
+	mutex_unlock(&fw_lock);
+	return fw_priv;
 }
 
 static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
@@ -509,6 +756,8 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
 {
 	int retval = 0;
 	struct device *f_dev = &fw_priv->dev;
+	struct firmware_buf *buf = fw_priv->buf;
+	struct firmware_cache *fwc = &fw_cache;
 
 	dev_set_uevent_suppress(f_dev, true);
 
@@ -535,7 +784,7 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
 
 	if (uevent) {
 		dev_set_uevent_suppress(f_dev, false);
-		dev_dbg(f_dev, "firmware: requesting %s\n", fw_priv->fw_id);
+		dev_dbg(f_dev, "firmware: requesting %s\n", buf->fw_id);
 		if (timeout != MAX_SCHEDULE_TIMEOUT)
 			mod_timer(&fw_priv->timeout,
 				  round_jiffies_up(jiffies + timeout));
@@ -543,15 +792,40 @@ static int _request_firmware_load(struct firmware_priv *fw_priv, bool uevent,
 		kobject_uevent(&fw_priv->dev.kobj, KOBJ_ADD);
 	}
 
-	wait_for_completion(&fw_priv->completion);
+	wait_for_completion(&buf->completion);
 
-	set_bit(FW_STATUS_DONE, &fw_priv->status);
 	del_timer_sync(&fw_priv->timeout);
 
 	mutex_lock(&fw_lock);
-	if (!fw_priv->fw->size || test_bit(FW_STATUS_ABORT, &fw_priv->status))
+	if (!buf->size || test_bit(FW_STATUS_ABORT, &buf->status))
 		retval = -ENOENT;
-	fw_priv->fw = NULL;
+
+	/*
+	 * add firmware name into devres list so that we can auto cache
+	 * and uncache firmware for device.
+	 *
+	 * f_dev->parent may has been deleted already, but the problem
+	 * should be fixed in devres or driver core.
+	 */
+	if (!retval && f_dev->parent)
+		fw_add_devm_name(f_dev->parent, buf->fw_id);
+
+	if (!retval)
+		retval = fw_map_pages_buf(buf);
+
+	/*
+	 * After caching firmware image is started, let it piggyback
+	 * on request firmware.
+	 */
+	if (!retval && fwc->state == FW_LOADER_START_CACHE) {
+		if (fw_cache_piggyback_on_request(buf->fw_id))
+			kref_get(&buf->ref);
+	}
+
+	/* pass the pages buffer to driver at the last minute */
+	fw_set_page_data(buf, fw_priv->fw);
+
+	fw_priv->buf = NULL;
 	mutex_unlock(&fw_lock);
 
 	device_remove_file(f_dev, &dev_attr_loading);
@@ -578,6 +852,8 @@ err_put_dev:
  *      @name will be used as $FIRMWARE in the uevent environment and
  *      should be distinctive enough not to be confused with any other
  *      firmware image for this or any other device.
+ *
+ *	Caller must hold the reference count of @device.
  **/
 int
 request_firmware(const struct firmware **firmware_p, const char *name,
@@ -659,6 +935,7 @@ static void request_firmware_work_func(struct work_struct *work)
 
  out:
 	fw_work->cont(fw, fw_work->context);
+	put_device(fw_work->device);
 
 	module_put(fw_work->module);
 	kfree(fw_work);
@@ -677,9 +954,15 @@ static void request_firmware_work_func(struct work_struct *work)
  * @cont: function will be called asynchronously when the firmware
  *	request is over.
  *
- *	Asynchronous variant of request_firmware() for user contexts where
- *	it is not possible to sleep for long time. It can't be called
- *	in atomic contexts.
+ *	Caller must hold the reference count of @device.
+ *
+ *	Asynchronous variant of request_firmware() for user contexts:
+ *		- sleep for as small periods as possible since it may
+ *		increase kernel boot time of built-in device drivers
+ *		requesting firmware in their ->probe() methods, if
+ *		@gfp is GFP_KERNEL.
+ *
+ *		- can't sleep at all if @gfp is GFP_ATOMIC.
  **/
 int
 request_firmware_nowait(
@@ -705,18 +988,363 @@ request_firmware_nowait(
 		return -EFAULT;
 	}
 
+	get_device(fw_work->device);
 	INIT_WORK(&fw_work->work, request_firmware_work_func);
 	schedule_work(&fw_work->work);
 	return 0;
 }
 
+/**
+ * cache_firmware - cache one firmware image in kernel memory space
+ * @fw_name: the firmware image name
+ *
+ * Cache firmware in kernel memory so that drivers can use it when
+ * system isn't ready for them to request firmware image from userspace.
+ * Once it returns successfully, driver can use request_firmware or its
+ * nowait version to get the cached firmware without any interacting
+ * with userspace
+ *
+ * Return 0 if the firmware image has been cached successfully
+ * Return !0 otherwise
+ *
+ */
+int cache_firmware(const char *fw_name)
+{
+	int ret;
+	const struct firmware *fw;
+
+	pr_debug("%s: %s\n", __func__, fw_name);
+
+	ret = request_firmware(&fw, fw_name, NULL);
+	if (!ret)
+		kfree(fw);
+
+	pr_debug("%s: %s ret=%d\n", __func__, fw_name, ret);
+
+	return ret;
+}
+
+/**
+ * uncache_firmware - remove one cached firmware image
+ * @fw_name: the firmware image name
+ *
+ * Uncache one firmware image which has been cached successfully
+ * before.
+ *
+ * Return 0 if the firmware cache has been removed successfully
+ * Return !0 otherwise
+ *
+ */
+int uncache_firmware(const char *fw_name)
+{
+	struct firmware_buf *buf;
+	struct firmware fw;
+
+	pr_debug("%s: %s\n", __func__, fw_name);
+
+	if (fw_get_builtin_firmware(&fw, fw_name))
+		return 0;
+
+	buf = fw_lookup_buf(fw_name);
+	if (buf) {
+		fw_free_buf(buf);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static struct fw_cache_entry *alloc_fw_cache_entry(const char *name)
+{
+	struct fw_cache_entry *fce;
+
+	fce = kzalloc(sizeof(*fce) + strlen(name) + 1, GFP_ATOMIC);
+	if (!fce)
+		goto exit;
+
+	strcpy(fce->name, name);
+exit:
+	return fce;
+}
+
+static int fw_cache_piggyback_on_request(const char *name)
+{
+	struct firmware_cache *fwc = &fw_cache;
+	struct fw_cache_entry *fce;
+	int ret = 0;
+
+	spin_lock(&fwc->name_lock);
+	list_for_each_entry(fce, &fwc->fw_names, list) {
+		if (!strcmp(fce->name, name))
+			goto found;
+	}
+
+	fce = alloc_fw_cache_entry(name);
+	if (fce) {
+		ret = 1;
+		list_add(&fce->list, &fwc->fw_names);
+		pr_debug("%s: fw: %s\n", __func__, name);
+	}
+found:
+	spin_unlock(&fwc->name_lock);
+	return ret;
+}
+
+static void free_fw_cache_entry(struct fw_cache_entry *fce)
+{
+	kfree(fce);
+}
+
+static void __async_dev_cache_fw_image(void *fw_entry,
+				       async_cookie_t cookie)
+{
+	struct fw_cache_entry *fce = fw_entry;
+	struct firmware_cache *fwc = &fw_cache;
+	int ret;
+
+	ret = cache_firmware(fce->name);
+	if (ret) {
+		spin_lock(&fwc->name_lock);
+		list_del(&fce->list);
+		spin_unlock(&fwc->name_lock);
+
+		free_fw_cache_entry(fce);
+	}
+
+	spin_lock(&fwc->name_lock);
+	fwc->cnt--;
+	spin_unlock(&fwc->name_lock);
+
+	wake_up(&fwc->wait_queue);
+}
+
+/* called with dev->devres_lock held */
+static void dev_create_fw_entry(struct device *dev, void *res,
+				void *data)
+{
+	struct fw_name_devm *fwn = res;
+	const char *fw_name = fwn->name;
+	struct list_head *head = data;
+	struct fw_cache_entry *fce;
+
+	fce = alloc_fw_cache_entry(fw_name);
+	if (fce)
+		list_add(&fce->list, head);
+}
+
+static int devm_name_match(struct device *dev, void *res,
+			   void *match_data)
+{
+	struct fw_name_devm *fwn = res;
+	return (fwn->magic == (unsigned long)match_data);
+}
+
+static void dev_cache_fw_image(struct device *dev, void *data)
+{
+	LIST_HEAD(todo);
+	struct fw_cache_entry *fce;
+	struct fw_cache_entry *fce_next;
+	struct firmware_cache *fwc = &fw_cache;
+
+	devres_for_each_res(dev, fw_name_devm_release,
+			    devm_name_match, &fw_cache,
+			    dev_create_fw_entry, &todo);
+
+	list_for_each_entry_safe(fce, fce_next, &todo, list) {
+		list_del(&fce->list);
+
+		spin_lock(&fwc->name_lock);
+		fwc->cnt++;
+		list_add(&fce->list, &fwc->fw_names);
+		spin_unlock(&fwc->name_lock);
+
+		async_schedule(__async_dev_cache_fw_image, (void *)fce);
+	}
+}
+
+static void __device_uncache_fw_images(void)
+{
+	struct firmware_cache *fwc = &fw_cache;
+	struct fw_cache_entry *fce;
+
+	spin_lock(&fwc->name_lock);
+	while (!list_empty(&fwc->fw_names)) {
+		fce = list_entry(fwc->fw_names.next,
+				struct fw_cache_entry, list);
+		list_del(&fce->list);
+		spin_unlock(&fwc->name_lock);
+
+		uncache_firmware(fce->name);
+		free_fw_cache_entry(fce);
+
+		spin_lock(&fwc->name_lock);
+	}
+	spin_unlock(&fwc->name_lock);
+}
+
+/**
+ * device_cache_fw_images - cache devices' firmware
+ *
+ * If one device called request_firmware or its nowait version
+ * successfully before, the firmware names are recored into the
+ * device's devres link list, so device_cache_fw_images can call
+ * cache_firmware() to cache these firmwares for the device,
+ * then the device driver can load its firmwares easily at
+ * time when system is not ready to complete loading firmware.
+ */
+static void device_cache_fw_images(void)
+{
+	struct firmware_cache *fwc = &fw_cache;
+	int old_timeout;
+	DEFINE_WAIT(wait);
+
+	pr_debug("%s\n", __func__);
+
+	/*
+	 * use small loading timeout for caching devices' firmware
+	 * because all these firmware images have been loaded
+	 * successfully at lease once, also system is ready for
+	 * completing firmware loading now. The maximum size of
+	 * firmware in current distributions is about 2M bytes,
+	 * so 10 secs should be enough.
+	 */
+	old_timeout = loading_timeout;
+	loading_timeout = 10;
+
+	mutex_lock(&fw_lock);
+	fwc->state = FW_LOADER_START_CACHE;
+	dpm_for_each_dev(NULL, dev_cache_fw_image);
+	mutex_unlock(&fw_lock);
+
+	/* wait for completion of caching firmware for all devices */
+	spin_lock(&fwc->name_lock);
+	for (;;) {
+		prepare_to_wait(&fwc->wait_queue, &wait,
+				TASK_UNINTERRUPTIBLE);
+		if (!fwc->cnt)
+			break;
+
+		spin_unlock(&fwc->name_lock);
+
+		schedule();
+
+		spin_lock(&fwc->name_lock);
+	}
+	spin_unlock(&fwc->name_lock);
+	finish_wait(&fwc->wait_queue, &wait);
+
+	loading_timeout = old_timeout;
+}
+
+/**
+ * device_uncache_fw_images - uncache devices' firmware
+ *
+ * uncache all firmwares which have been cached successfully
+ * by device_uncache_fw_images earlier
+ */
+static void device_uncache_fw_images(void)
+{
+	pr_debug("%s\n", __func__);
+	__device_uncache_fw_images();
+}
+
+static void device_uncache_fw_images_work(struct work_struct *work)
+{
+	device_uncache_fw_images();
+}
+
+/**
+ * device_uncache_fw_images_delay - uncache devices firmwares
+ * @delay: number of milliseconds to delay uncache device firmwares
+ *
+ * uncache all devices's firmwares which has been cached successfully
+ * by device_cache_fw_images after @delay milliseconds.
+ */
+static void device_uncache_fw_images_delay(unsigned long delay)
+{
+	schedule_delayed_work(&fw_cache.work,
+			msecs_to_jiffies(delay));
+}
+
+static int fw_pm_notify(struct notifier_block *notify_block,
+			unsigned long mode, void *unused)
+{
+	switch (mode) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_SUSPEND_PREPARE:
+		device_cache_fw_images();
+		break;
+
+	case PM_POST_SUSPEND:
+	case PM_POST_HIBERNATION:
+	case PM_POST_RESTORE:
+		/*
+		 * In case that system sleep failed and syscore_suspend is
+		 * not called.
+		 */
+		mutex_lock(&fw_lock);
+		fw_cache.state = FW_LOADER_NO_CACHE;
+		mutex_unlock(&fw_lock);
+
+		device_uncache_fw_images_delay(10 * MSEC_PER_SEC);
+		break;
+	}
+
+	return 0;
+}
+
+/* stop caching firmware once syscore_suspend is reached */
+static int fw_suspend(void)
+{
+	fw_cache.state = FW_LOADER_NO_CACHE;
+	return 0;
+}
+
+static struct syscore_ops fw_syscore_ops = {
+	.suspend = fw_suspend,
+};
+#else
+static int fw_cache_piggyback_on_request(const char *name)
+{
+	return 0;
+}
+#endif
+
+static void __init fw_cache_init(void)
+{
+	spin_lock_init(&fw_cache.lock);
+	INIT_LIST_HEAD(&fw_cache.head);
+	fw_cache.state = FW_LOADER_NO_CACHE;
+
+#ifdef CONFIG_PM_SLEEP
+	spin_lock_init(&fw_cache.name_lock);
+	INIT_LIST_HEAD(&fw_cache.fw_names);
+	fw_cache.cnt = 0;
+
+	init_waitqueue_head(&fw_cache.wait_queue);
+	INIT_DELAYED_WORK(&fw_cache.work,
+			  device_uncache_fw_images_work);
+
+	fw_cache.pm_notify.notifier_call = fw_pm_notify;
+	register_pm_notifier(&fw_cache.pm_notify);
+
+	register_syscore_ops(&fw_syscore_ops);
+#endif
+}
+
 static int __init firmware_class_init(void)
 {
+	fw_cache_init();
 	return class_register(&firmware_class);
 }
 
 static void __exit firmware_class_exit(void)
 {
+#ifdef CONFIG_PM_SLEEP
+	unregister_syscore_ops(&fw_syscore_ops);
+	unregister_pm_notifier(&fw_cache.pm_notify);
+#endif
 	class_unregister(&firmware_class);
 }
 
@@ -726,3 +1354,5 @@ module_exit(firmware_class_exit);
 EXPORT_SYMBOL(release_firmware);
 EXPORT_SYMBOL(request_firmware);
 EXPORT_SYMBOL(request_firmware_nowait);
+EXPORT_SYMBOL_GPL(cache_firmware);
+EXPORT_SYMBOL_GPL(uncache_firmware);
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index a1a722502587..ddeca142293c 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -20,9 +20,13 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
+#include <linux/idr.h>
 
 #include "base.h"
 
+/* For automatically allocated device IDs */
+static DEFINE_IDA(platform_devid_ida);
+
 #define to_platform_driver(drv)	(container_of((drv), struct platform_driver, \
 				 driver))
 
@@ -99,6 +103,9 @@ struct resource *platform_get_resource_byname(struct platform_device *dev,
 	for (i = 0; i < dev->num_resources; i++) {
 		struct resource *r = &dev->resource[i];
 
+		if (unlikely(!r->name))
+			continue;
+
 		if (type == resource_type(r) && !strcmp(r->name, name))
 			return r;
 	}
@@ -263,7 +270,7 @@ EXPORT_SYMBOL_GPL(platform_device_add_data);
  */
 int platform_device_add(struct platform_device *pdev)
 {
-	int i, ret = 0;
+	int i, ret;
 
 	if (!pdev)
 		return -EINVAL;
@@ -273,10 +280,27 @@ int platform_device_add(struct platform_device *pdev)
 
 	pdev->dev.bus = &platform_bus_type;
 
-	if (pdev->id != -1)
+	switch (pdev->id) {
+	default:
 		dev_set_name(&pdev->dev, "%s.%d", pdev->name,  pdev->id);
-	else
+		break;
+	case PLATFORM_DEVID_NONE:
 		dev_set_name(&pdev->dev, "%s", pdev->name);
+		break;
+	case PLATFORM_DEVID_AUTO:
+		/*
+		 * Automatically allocated device ID. We mark it as such so
+		 * that we remember it must be freed, and we append a suffix
+		 * to avoid namespace collision with explicit IDs.
+		 */
+		ret = ida_simple_get(&platform_devid_ida, 0, 0, GFP_KERNEL);
+		if (ret < 0)
+			goto err_out;
+		pdev->id = ret;
+		pdev->id_auto = true;
+		dev_set_name(&pdev->dev, "%s.%d.auto", pdev->name, pdev->id);
+		break;
+	}
 
 	for (i = 0; i < pdev->num_resources; i++) {
 		struct resource *p, *r = &pdev->resource[i];
@@ -309,6 +333,11 @@ int platform_device_add(struct platform_device *pdev)
 		return ret;
 
  failed:
+	if (pdev->id_auto) {
+		ida_simple_remove(&platform_devid_ida, pdev->id);
+		pdev->id = PLATFORM_DEVID_AUTO;
+	}
+
 	while (--i >= 0) {
 		struct resource *r = &pdev->resource[i];
 		unsigned long type = resource_type(r);
@@ -317,6 +346,7 @@ int platform_device_add(struct platform_device *pdev)
 			release_resource(r);
 	}
 
+ err_out:
 	return ret;
 }
 EXPORT_SYMBOL_GPL(platform_device_add);
@@ -336,6 +366,11 @@ void platform_device_del(struct platform_device *pdev)
 	if (pdev) {
 		device_del(&pdev->dev);
 
+		if (pdev->id_auto) {
+			ida_simple_remove(&platform_devid_ida, pdev->id);
+			pdev->id = PLATFORM_DEVID_AUTO;
+		}
+
 		for (i = 0; i < pdev->num_resources; i++) {
 			struct resource *r = &pdev->resource[i];
 			unsigned long type = resource_type(r);
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 0113adc310dc..b0b072a88f5f 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -1324,3 +1324,25 @@ int device_pm_wait_for_dev(struct device *subordinate, struct device *dev)
 	return async_error;
 }
 EXPORT_SYMBOL_GPL(device_pm_wait_for_dev);
+
+/**
+ * dpm_for_each_dev - device iterator.
+ * @data: data for the callback.
+ * @fn: function to be called for each device.
+ *
+ * Iterate over devices in dpm_list, and call @fn for each device,
+ * passing it @data.
+ */
+void dpm_for_each_dev(void *data, void (*fn)(struct device *, void *))
+{
+	struct device *dev;
+
+	if (!fn)
+		return;
+
+	device_pm_lock();
+	list_for_each_entry(dev, &dpm_list, power.entry)
+		fn(dev, data);
+	device_pm_unlock();
+}
+EXPORT_SYMBOL_GPL(dpm_for_each_dev);
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index a89734621e51..5b6b1d8e6cc0 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -16,12 +16,14 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 
 #include "internal.h"
 
 struct regmap_irq_chip_data {
 	struct mutex lock;
+	struct irq_chip irq_chip;
 
 	struct regmap *map;
 	const struct regmap_irq_chip *chip;
@@ -59,6 +61,14 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
 	struct regmap *map = d->map;
 	int i, ret;
+	u32 reg;
+
+	if (d->chip->runtime_pm) {
+		ret = pm_runtime_get_sync(map->dev);
+		if (ret < 0)
+			dev_err(map->dev, "IRQ sync failed to resume: %d\n",
+				ret);
+	}
 
 	/*
 	 * If there's been a change in the mask write it back to the
@@ -66,15 +76,22 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 	 * suppress pointless writes.
 	 */
 	for (i = 0; i < d->chip->num_regs; i++) {
-		ret = regmap_update_bits(d->map, d->chip->mask_base +
-						(i * map->reg_stride *
-						d->irq_reg_stride),
+		reg = d->chip->mask_base +
+			(i * map->reg_stride * d->irq_reg_stride);
+		if (d->chip->mask_invert)
+			ret = regmap_update_bits(d->map, reg,
+					 d->mask_buf_def[i], ~d->mask_buf[i]);
+		else
+			ret = regmap_update_bits(d->map, reg,
 					 d->mask_buf_def[i], d->mask_buf[i]);
 		if (ret != 0)
 			dev_err(d->map->dev, "Failed to sync masks in %x\n",
-				d->chip->mask_base + (i * map->reg_stride));
+				reg);
 	}
 
+	if (d->chip->runtime_pm)
+		pm_runtime_put(map->dev);
+
 	/* If we've changed our wakeup count propagate it to the parent */
 	if (d->wake_count < 0)
 		for (i = d->wake_count; i < 0; i++)
@@ -128,8 +145,7 @@ static int regmap_irq_set_wake(struct irq_data *data, unsigned int on)
 	return 0;
 }
 
-static struct irq_chip regmap_irq_chip = {
-	.name			= "regmap",
+static const struct irq_chip regmap_irq_chip = {
 	.irq_bus_lock		= regmap_irq_lock,
 	.irq_bus_sync_unlock	= regmap_irq_sync_unlock,
 	.irq_disable		= regmap_irq_disable,
@@ -144,6 +160,16 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 	struct regmap *map = data->map;
 	int ret, i;
 	bool handled = false;
+	u32 reg;
+
+	if (chip->runtime_pm) {
+		ret = pm_runtime_get_sync(map->dev);
+		if (ret < 0) {
+			dev_err(map->dev, "IRQ thread failed to resume: %d\n",
+				ret);
+			return IRQ_NONE;
+		}
+	}
 
 	/*
 	 * Ignore masked IRQs and ack if we need to; we ack early so
@@ -160,20 +186,20 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 		if (ret != 0) {
 			dev_err(map->dev, "Failed to read IRQ status: %d\n",
 					ret);
+			if (chip->runtime_pm)
+				pm_runtime_put(map->dev);
 			return IRQ_NONE;
 		}
 
 		data->status_buf[i] &= ~data->mask_buf[i];
 
 		if (data->status_buf[i] && chip->ack_base) {
-			ret = regmap_write(map, chip->ack_base +
-						(i * map->reg_stride *
-						data->irq_reg_stride),
-					   data->status_buf[i]);
+			reg = chip->ack_base +
+				(i * map->reg_stride * data->irq_reg_stride);
+			ret = regmap_write(map, reg, data->status_buf[i]);
 			if (ret != 0)
 				dev_err(map->dev, "Failed to ack 0x%x: %d\n",
-					chip->ack_base + (i * map->reg_stride),
-					ret);
+					reg, ret);
 		}
 	}
 
@@ -185,6 +211,9 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 		}
 	}
 
+	if (chip->runtime_pm)
+		pm_runtime_put(map->dev);
+
 	if (handled)
 		return IRQ_HANDLED;
 	else
@@ -197,7 +226,7 @@ static int regmap_irq_map(struct irq_domain *h, unsigned int virq,
 	struct regmap_irq_chip_data *data = h->host_data;
 
 	irq_set_chip_data(virq, data);
-	irq_set_chip_and_handler(virq, &regmap_irq_chip, handle_edge_irq);
+	irq_set_chip(virq, &data->irq_chip);
 	irq_set_nested_thread(virq, 1);
 
 	/* ARM needs us to explicitly flag the IRQ as valid
@@ -238,6 +267,7 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	struct regmap_irq_chip_data *d;
 	int i;
 	int ret = -ENOMEM;
+	u32 reg;
 
 	for (i = 0; i < chip->num_irqs; i++) {
 		if (chip->irqs[i].reg_offset % map->reg_stride)
@@ -284,6 +314,13 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 			goto err_alloc;
 	}
 
+	d->irq_chip = regmap_irq_chip;
+	d->irq_chip.name = chip->name;
+	if (!chip->wake_base) {
+		d->irq_chip.irq_set_wake = NULL;
+		d->irq_chip.flags |= IRQCHIP_MASK_ON_SUSPEND |
+				     IRQCHIP_SKIP_SET_WAKE;
+	}
 	d->irq = irq;
 	d->map = map;
 	d->chip = chip;
@@ -303,16 +340,37 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	/* Mask all the interrupts by default */
 	for (i = 0; i < chip->num_regs; i++) {
 		d->mask_buf[i] = d->mask_buf_def[i];
-		ret = regmap_write(map, chip->mask_base + (i * map->reg_stride
-				   * d->irq_reg_stride),
-				   d->mask_buf[i]);
+		reg = chip->mask_base +
+			(i * map->reg_stride * d->irq_reg_stride);
+		if (chip->mask_invert)
+			ret = regmap_update_bits(map, reg,
+					 d->mask_buf[i], ~d->mask_buf[i]);
+		else
+			ret = regmap_update_bits(map, reg,
+					 d->mask_buf[i], d->mask_buf[i]);
 		if (ret != 0) {
 			dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
-				chip->mask_base + (i * map->reg_stride), ret);
+				reg, ret);
 			goto err_alloc;
 		}
 	}
 
+	/* Wake is disabled by default */
+	if (d->wake_buf) {
+		for (i = 0; i < chip->num_regs; i++) {
+			d->wake_buf[i] = d->mask_buf_def[i];
+			reg = chip->wake_base +
+				(i * map->reg_stride * d->irq_reg_stride);
+			ret = regmap_update_bits(map, reg, d->wake_buf[i],
+						 d->wake_buf[i]);
+			if (ret != 0) {
+				dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
+					reg, ret);
+				goto err_alloc;
+			}
+		}
+	}
+
 	if (irq_base)
 		d->domain = irq_domain_add_legacy(map->dev->of_node,
 						  chip->num_irqs, irq_base, 0,
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index c241ae2f2f10..52069d29ff12 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -659,13 +659,12 @@ EXPORT_SYMBOL_GPL(devm_regmap_init);
  * new cache.  This can be used to restore the cache to defaults or to
  * update the cache configuration to reflect runtime discovery of the
  * hardware.
+ *
+ * No explicit locking is done here, the user needs to ensure that
+ * this function will not race with other calls to regmap.
  */
 int regmap_reinit_cache(struct regmap *map, const struct regmap_config *config)
 {
-	int ret;
-
-	map->lock(map);
-
 	regcache_exit(map);
 	regmap_debugfs_exit(map);
 
@@ -681,11 +680,7 @@ int regmap_reinit_cache(struct regmap *map, const struct regmap_config *config)
 	map->cache_bypass = false;
 	map->cache_only = false;
 
-	ret = regcache_init(map, config);
-
-	map->unlock(map);
-
-	return ret;
+	return regcache_init(map, config);
 }
 EXPORT_SYMBOL_GPL(regmap_reinit_cache);
 
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index de0435e63b02..887f68f6d79a 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -35,6 +35,7 @@ new_skb(ulong len)
 		skb_reset_mac_header(skb);
 		skb_reset_network_header(skb);
 		skb->protocol = __constant_htons(ETH_P_AOE);
+		skb_checksum_none_assert(skb);
 	}
 	return skb;
 }
diff --git a/drivers/block/cciss_scsi.c b/drivers/block/cciss_scsi.c
index 38aa6dda6b81..da3311129a0c 100644
--- a/drivers/block/cciss_scsi.c
+++ b/drivers/block/cciss_scsi.c
@@ -795,6 +795,7 @@ static void complete_scsi_command(CommandList_struct *c, int timeout,
 				}
 			break;
 			case CMD_PROTOCOL_ERR:
+				cmd->result = DID_ERROR << 16;
 				dev_warn(&h->pdev->dev,
 					"%p has protocol error\n", c);
                         break;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index a8fddeb3d638..f946d31d6917 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -1148,11 +1148,15 @@ static bool mtip_pause_ncq(struct mtip_port *port,
 	reply = port->rxfis + RX_FIS_D2H_REG;
 	task_file_data = readl(port->mmio+PORT_TFDATA);
 
-	if ((task_file_data & 1) || (fis->command == ATA_CMD_SEC_ERASE_UNIT))
+	if (fis->command == ATA_CMD_SEC_ERASE_UNIT)
+		clear_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
+
+	if ((task_file_data & 1))
 		return false;
 
 	if (fis->command == ATA_CMD_SEC_ERASE_PREP) {
 		set_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
+		set_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
 		port->ic_pause_timer = jiffies;
 		return true;
 	} else if ((fis->command == ATA_CMD_DOWNLOAD_MICRO) &&
@@ -1900,7 +1904,7 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 	int rv = 0, xfer_sz = command[3];
 
 	if (xfer_sz) {
-		if (user_buffer)
+		if (!user_buffer)
 			return -EFAULT;
 
 		buf = dmam_alloc_coherent(&port->dd->pdev->dev,
@@ -2043,7 +2047,7 @@ static void mtip_set_timeout(struct host_to_dev_fis *fis, unsigned int *timeout)
 		*timeout = 240000; /* 4 minutes */
 		break;
 	case ATA_CMD_STANDBYNOW1:
-		*timeout = 10000;  /* 10 seconds */
+		*timeout = 120000;  /* 2 minutes */
 		break;
 	case 0xF7:
 	case 0xFA:
@@ -2588,9 +2592,6 @@ static ssize_t mtip_hw_read_registers(struct file *f, char __user *ubuf,
 	if (!len || size)
 		return 0;
 
-	if (size < 0)
-		return -EINVAL;
-
 	size += sprintf(&buf[size], "H/ S ACTive      : [ 0x");
 
 	for (n = dd->slot_groups-1; n >= 0; n--)
@@ -2660,9 +2661,6 @@ static ssize_t mtip_hw_read_flags(struct file *f, char __user *ubuf,
 	if (!len || size)
 		return 0;
 
-	if (size < 0)
-		return -EINVAL;
-
 	size += sprintf(&buf[size], "Flag-port : [ %08lX ]\n",
 							dd->port->flags);
 	size += sprintf(&buf[size], "Flag-dd   : [ %08lX ]\n",
@@ -3214,8 +3212,8 @@ static int mtip_hw_init(struct driver_data *dd)
 				"Unable to check write protect progress\n");
 	else
 		dev_info(&dd->pdev->dev,
-				"Write protect progress: %d%% (%d blocks)\n",
-				attr242.cur, attr242.data);
+				"Write protect progress: %u%% (%u blocks)\n",
+				attr242.cur, le32_to_cpu(attr242.data));
 	return rv;
 
 out3:
@@ -3619,6 +3617,10 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 			bio_endio(bio, -ENODATA);
 			return;
 		}
+		if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))) {
+			bio_endio(bio, -ENODATA);
+			return;
+		}
 	}
 
 	if (unlikely(!bio_has_data(bio))) {
@@ -4168,7 +4170,13 @@ static void mtip_pci_shutdown(struct pci_dev *pdev)
 
 /* Table of device ids supported by this driver. */
 static DEFINE_PCI_DEVICE_TABLE(mtip_pci_tbl) = {
-	{  PCI_DEVICE(PCI_VENDOR_ID_MICRON, P320_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P320H_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P320M_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P320S_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P325M_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P420H_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P420M_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MICRON, P425M_DEVICE_ID) },
 	{ 0 }
 };
 
@@ -4199,12 +4207,12 @@ static int __init mtip_init(void)
 {
 	int error;
 
-	printk(KERN_INFO MTIP_DRV_NAME " Version " MTIP_DRV_VERSION "\n");
+	pr_info(MTIP_DRV_NAME " Version " MTIP_DRV_VERSION "\n");
 
 	/* Allocate a major block device number to use with this driver. */
 	error = register_blkdev(0, MTIP_DRV_NAME);
 	if (error <= 0) {
-		printk(KERN_ERR "Unable to register block device (%d)\n",
+		pr_err("Unable to register block device (%d)\n",
 		error);
 		return -EBUSY;
 	}
@@ -4213,7 +4221,7 @@ static int __init mtip_init(void)
 	if (!dfs_parent) {
 		dfs_parent = debugfs_create_dir("rssd", NULL);
 		if (IS_ERR_OR_NULL(dfs_parent)) {
-			printk(KERN_WARNING "Error creating debugfs parent\n");
+			pr_warn("Error creating debugfs parent\n");
 			dfs_parent = NULL;
 		}
 	}
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index f51fc23d17bb..18627a1d04c5 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -76,7 +76,13 @@
 
 /* Micron Vendor ID & P320x SSD Device ID */
 #define PCI_VENDOR_ID_MICRON    0x1344
-#define P320_DEVICE_ID		0x5150
+#define P320H_DEVICE_ID		0x5150
+#define P320M_DEVICE_ID		0x5151
+#define P320S_DEVICE_ID		0x5152
+#define P325M_DEVICE_ID		0x5153
+#define P420H_DEVICE_ID		0x5160
+#define P420M_DEVICE_ID		0x5161
+#define P425M_DEVICE_ID		0x5163
 
 /* Driver name and version strings */
 #define MTIP_DRV_NAME		"mtip32xx"
@@ -131,10 +137,12 @@ enum {
 	MTIP_PF_SVC_THD_STOP_BIT    = 8,
 
 	/* below are bit numbers in 'dd_flag' defined in driver_data */
+	MTIP_DDF_SEC_LOCK_BIT	    = 0,
 	MTIP_DDF_REMOVE_PENDING_BIT = 1,
 	MTIP_DDF_OVER_TEMP_BIT      = 2,
 	MTIP_DDF_WRITE_PROTECT_BIT  = 3,
 	MTIP_DDF_STOP_IO      = ((1 << MTIP_DDF_REMOVE_PENDING_BIT) | \
+				(1 << MTIP_DDF_SEC_LOCK_BIT) | \
 				(1 << MTIP_DDF_OVER_TEMP_BIT) | \
 				(1 << MTIP_DDF_WRITE_PROTECT_BIT)),
 
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index d07c9f7fded6..0c03411c59eb 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -449,6 +449,14 @@ static void nbd_clear_que(struct nbd_device *nbd)
 		req->errors++;
 		nbd_end_request(req);
 	}
+
+	while (!list_empty(&nbd->waiting_queue)) {
+		req = list_entry(nbd->waiting_queue.next, struct request,
+				 queuelist);
+		list_del_init(&req->queuelist);
+		req->errors++;
+		nbd_end_request(req);
+	}
 }
 
 
@@ -598,6 +606,7 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		nbd->file = NULL;
 		nbd_clear_que(nbd);
 		BUG_ON(!list_empty(&nbd->queue_head));
+		BUG_ON(!list_empty(&nbd->waiting_queue));
 		if (file)
 			fput(file);
 		return 0;
diff --git a/drivers/block/nvme.c b/drivers/block/nvme.c
index 38a2d0631882..931769e133e5 100644
--- a/drivers/block/nvme.c
+++ b/drivers/block/nvme.c
@@ -79,6 +79,7 @@ struct nvme_dev {
 	char serial[20];
 	char model[40];
 	char firmware_rev[8];
+	u32 max_hw_sectors;
 };
 
 /*
@@ -835,15 +836,15 @@ static int nvme_identify(struct nvme_dev *dev, unsigned nsid, unsigned cns,
 }
 
 static int nvme_get_features(struct nvme_dev *dev, unsigned fid,
-				unsigned dword11, dma_addr_t dma_addr)
+				unsigned nsid, dma_addr_t dma_addr)
 {
 	struct nvme_command c;
 
 	memset(&c, 0, sizeof(c));
 	c.features.opcode = nvme_admin_get_features;
+	c.features.nsid = cpu_to_le32(nsid);
 	c.features.prp1 = cpu_to_le64(dma_addr);
 	c.features.fid = cpu_to_le32(fid);
-	c.features.dword11 = cpu_to_le32(dword11);
 
 	return nvme_submit_admin_cmd(dev, &c, NULL);
 }
@@ -862,11 +863,51 @@ static int nvme_set_features(struct nvme_dev *dev, unsigned fid,
 	return nvme_submit_admin_cmd(dev, &c, result);
 }
 
+/**
+ * nvme_cancel_ios - Cancel outstanding I/Os
+ * @queue: The queue to cancel I/Os on
+ * @timeout: True to only cancel I/Os which have timed out
+ */
+static void nvme_cancel_ios(struct nvme_queue *nvmeq, bool timeout)
+{
+	int depth = nvmeq->q_depth - 1;
+	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
+	unsigned long now = jiffies;
+	int cmdid;
+
+	for_each_set_bit(cmdid, nvmeq->cmdid_data, depth) {
+		void *ctx;
+		nvme_completion_fn fn;
+		static struct nvme_completion cqe = {
+			.status = cpu_to_le16(NVME_SC_ABORT_REQ) << 1,
+		};
+
+		if (timeout && !time_after(now, info[cmdid].timeout))
+			continue;
+		dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d\n", cmdid);
+		ctx = cancel_cmdid(nvmeq, cmdid, &fn);
+		fn(nvmeq->dev, ctx, &cqe);
+	}
+}
+
+static void nvme_free_queue_mem(struct nvme_queue *nvmeq)
+{
+	dma_free_coherent(nvmeq->q_dmadev, CQ_SIZE(nvmeq->q_depth),
+				(void *)nvmeq->cqes, nvmeq->cq_dma_addr);
+	dma_free_coherent(nvmeq->q_dmadev, SQ_SIZE(nvmeq->q_depth),
+					nvmeq->sq_cmds, nvmeq->sq_dma_addr);
+	kfree(nvmeq);
+}
+
 static void nvme_free_queue(struct nvme_dev *dev, int qid)
 {
 	struct nvme_queue *nvmeq = dev->queues[qid];
 	int vector = dev->entry[nvmeq->cq_vector].vector;
 
+	spin_lock_irq(&nvmeq->q_lock);
+	nvme_cancel_ios(nvmeq, false);
+	spin_unlock_irq(&nvmeq->q_lock);
+
 	irq_set_affinity_hint(vector, NULL);
 	free_irq(vector, nvmeq);
 
@@ -876,18 +917,15 @@ static void nvme_free_queue(struct nvme_dev *dev, int qid)
 		adapter_delete_cq(dev, qid);
 	}
 
-	dma_free_coherent(nvmeq->q_dmadev, CQ_SIZE(nvmeq->q_depth),
-				(void *)nvmeq->cqes, nvmeq->cq_dma_addr);
-	dma_free_coherent(nvmeq->q_dmadev, SQ_SIZE(nvmeq->q_depth),
-					nvmeq->sq_cmds, nvmeq->sq_dma_addr);
-	kfree(nvmeq);
+	nvme_free_queue_mem(nvmeq);
 }
 
 static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 							int depth, int vector)
 {
 	struct device *dmadev = &dev->pci_dev->dev;
-	unsigned extra = (depth / 8) + (depth * sizeof(struct nvme_cmd_info));
+	unsigned extra = DIV_ROUND_UP(depth, 8) + (depth *
+						sizeof(struct nvme_cmd_info));
 	struct nvme_queue *nvmeq = kzalloc(sizeof(*nvmeq) + extra, GFP_KERNEL);
 	if (!nvmeq)
 		return NULL;
@@ -975,7 +1013,7 @@ static __devinit struct nvme_queue *nvme_create_queue(struct nvme_dev *dev,
 
 static int __devinit nvme_configure_admin_queue(struct nvme_dev *dev)
 {
-	int result;
+	int result = 0;
 	u32 aqa;
 	u64 cap;
 	unsigned long timeout;
@@ -1005,17 +1043,22 @@ static int __devinit nvme_configure_admin_queue(struct nvme_dev *dev)
 	timeout = ((NVME_CAP_TIMEOUT(cap) + 1) * HZ / 2) + jiffies;
 	dev->db_stride = NVME_CAP_STRIDE(cap);
 
-	while (!(readl(&dev->bar->csts) & NVME_CSTS_RDY)) {
+	while (!result && !(readl(&dev->bar->csts) & NVME_CSTS_RDY)) {
 		msleep(100);
 		if (fatal_signal_pending(current))
-			return -EINTR;
+			result = -EINTR;
 		if (time_after(jiffies, timeout)) {
 			dev_err(&dev->pci_dev->dev,
 				"Device not ready; aborting initialisation\n");
-			return -ENODEV;
+			result = -ENODEV;
 		}
 	}
 
+	if (result) {
+		nvme_free_queue_mem(nvmeq);
+		return result;
+	}
+
 	result = queue_request_irq(dev, nvmeq, "nvme admin");
 	dev->queues[0] = nvmeq;
 	return result;
@@ -1037,6 +1080,8 @@ static struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
 	offset = offset_in_page(addr);
 	count = DIV_ROUND_UP(offset + length, PAGE_SIZE);
 	pages = kcalloc(count, sizeof(*pages), GFP_KERNEL);
+	if (!pages)
+		return ERR_PTR(-ENOMEM);
 
 	err = get_user_pages_fast(addr, count, 1, pages);
 	if (err < count) {
@@ -1146,14 +1191,13 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	return status;
 }
 
-static int nvme_user_admin_cmd(struct nvme_ns *ns,
+static int nvme_user_admin_cmd(struct nvme_dev *dev,
 					struct nvme_admin_cmd __user *ucmd)
 {
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_admin_cmd cmd;
 	struct nvme_command c;
 	int status, length;
-	struct nvme_iod *iod;
+	struct nvme_iod *uninitialized_var(iod);
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -1204,7 +1248,7 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
 	case NVME_IOCTL_ID:
 		return ns->ns_id;
 	case NVME_IOCTL_ADMIN_CMD:
-		return nvme_user_admin_cmd(ns, (void __user *)arg);
+		return nvme_user_admin_cmd(ns->dev, (void __user *)arg);
 	case NVME_IOCTL_SUBMIT_IO:
 		return nvme_submit_io(ns, (void __user *)arg);
 	default:
@@ -1218,26 +1262,6 @@ static const struct block_device_operations nvme_fops = {
 	.compat_ioctl	= nvme_ioctl,
 };
 
-static void nvme_timeout_ios(struct nvme_queue *nvmeq)
-{
-	int depth = nvmeq->q_depth - 1;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
-	unsigned long now = jiffies;
-	int cmdid;
-
-	for_each_set_bit(cmdid, nvmeq->cmdid_data, depth) {
-		void *ctx;
-		nvme_completion_fn fn;
-		static struct nvme_completion cqe = { .status = cpu_to_le16(NVME_SC_ABORT_REQ) << 1, };
-
-		if (!time_after(now, info[cmdid].timeout))
-			continue;
-		dev_warn(nvmeq->q_dmadev, "Timing out I/O %d\n", cmdid);
-		ctx = cancel_cmdid(nvmeq, cmdid, &fn);
-		fn(nvmeq->dev, ctx, &cqe);
-	}
-}
-
 static void nvme_resubmit_bios(struct nvme_queue *nvmeq)
 {
 	while (bio_list_peek(&nvmeq->sq_cong)) {
@@ -1269,7 +1293,7 @@ static int nvme_kthread(void *data)
 				spin_lock_irq(&nvmeq->q_lock);
 				if (nvme_process_cq(nvmeq))
 					printk("process_cq did something\n");
-				nvme_timeout_ios(nvmeq);
+				nvme_cancel_ios(nvmeq, true);
 				nvme_resubmit_bios(nvmeq);
 				spin_unlock_irq(&nvmeq->q_lock);
 			}
@@ -1339,6 +1363,9 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, int nsid,
 	ns->disk = disk;
 	lbaf = id->flbas & 0xf;
 	ns->lba_shift = id->lbaf[lbaf].ds;
+	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
+	if (dev->max_hw_sectors)
+		blk_queue_max_hw_sectors(ns->queue, dev->max_hw_sectors);
 
 	disk->major = nvme_major;
 	disk->minors = NVME_MINORS;
@@ -1383,7 +1410,7 @@ static int set_queue_count(struct nvme_dev *dev, int count)
 
 static int __devinit nvme_setup_io_queues(struct nvme_dev *dev)
 {
-	int result, cpu, i, nr_io_queues, db_bar_size;
+	int result, cpu, i, nr_io_queues, db_bar_size, q_depth;
 
 	nr_io_queues = num_online_cpus();
 	result = set_queue_count(dev, nr_io_queues);
@@ -1429,9 +1456,10 @@ static int __devinit nvme_setup_io_queues(struct nvme_dev *dev)
 		cpu = cpumask_next(cpu, cpu_online_mask);
 	}
 
+	q_depth = min_t(int, NVME_CAP_MQES(readq(&dev->bar->cap)) + 1,
+								NVME_Q_DEPTH);
 	for (i = 0; i < nr_io_queues; i++) {
-		dev->queues[i + 1] = nvme_create_queue(dev, i + 1,
-							NVME_Q_DEPTH, i);
+		dev->queues[i + 1] = nvme_create_queue(dev, i + 1, q_depth, i);
 		if (IS_ERR(dev->queues[i + 1]))
 			return PTR_ERR(dev->queues[i + 1]);
 		dev->queue_count++;
@@ -1480,6 +1508,10 @@ static int __devinit nvme_dev_add(struct nvme_dev *dev)
 	memcpy(dev->serial, ctrl->sn, sizeof(ctrl->sn));
 	memcpy(dev->model, ctrl->mn, sizeof(ctrl->mn));
 	memcpy(dev->firmware_rev, ctrl->fr, sizeof(ctrl->fr));
+	if (ctrl->mdts) {
+		int shift = NVME_CAP_MPSMIN(readq(&dev->bar->cap)) + 12;
+		dev->max_hw_sectors = 1 << (ctrl->mdts + shift - 9);
+	}
 
 	id_ns = mem;
 	for (i = 1; i <= nn; i++) {
@@ -1523,8 +1555,6 @@ static int nvme_dev_remove(struct nvme_dev *dev)
 	list_del(&dev->node);
 	spin_unlock(&dev_list_lock);
 
-	/* TODO: wait all I/O finished or cancel them */
-
 	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
 		list_del(&ns->list);
 		del_gendisk(ns->disk);
@@ -1560,15 +1590,33 @@ static void nvme_release_prp_pools(struct nvme_dev *dev)
 	dma_pool_destroy(dev->prp_small_pool);
 }
 
-/* XXX: Use an ida or something to let remove / add work correctly */
-static void nvme_set_instance(struct nvme_dev *dev)
+static DEFINE_IDA(nvme_instance_ida);
+
+static int nvme_set_instance(struct nvme_dev *dev)
 {
-	static int instance;
-	dev->instance = instance++;
+	int instance, error;
+
+	do {
+		if (!ida_pre_get(&nvme_instance_ida, GFP_KERNEL))
+			return -ENODEV;
+
+		spin_lock(&dev_list_lock);
+		error = ida_get_new(&nvme_instance_ida, &instance);
+		spin_unlock(&dev_list_lock);
+	} while (error == -EAGAIN);
+
+	if (error)
+		return -ENODEV;
+
+	dev->instance = instance;
+	return 0;
 }
 
 static void nvme_release_instance(struct nvme_dev *dev)
 {
+	spin_lock(&dev_list_lock);
+	ida_remove(&nvme_instance_ida, dev->instance);
+	spin_unlock(&dev_list_lock);
 }
 
 static int __devinit nvme_probe(struct pci_dev *pdev,
@@ -1601,7 +1649,10 @@ static int __devinit nvme_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, dev);
 	dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
 	dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
-	nvme_set_instance(dev);
+	result = nvme_set_instance(dev);
+	if (result)
+		goto disable;
+
 	dev->entry[0].vector = pdev->irq;
 
 	result = nvme_setup_prp_pools(dev);
@@ -1675,7 +1726,7 @@ static void __devexit nvme_remove(struct pci_dev *pdev)
 #define nvme_suspend NULL
 #define nvme_resume NULL
 
-static struct pci_error_handlers nvme_err_handler = {
+static const struct pci_error_handlers nvme_err_handler = {
 	.error_detected	= nvme_error_detected,
 	.mmio_enabled	= nvme_dump_registers,
 	.link_reset	= nvme_link_reset,
@@ -1704,15 +1755,17 @@ static struct pci_driver nvme_driver = {
 
 static int __init nvme_init(void)
 {
-	int result = -EBUSY;
+	int result;
 
 	nvme_thread = kthread_run(nvme_kthread, NULL, "nvme");
 	if (IS_ERR(nvme_thread))
 		return PTR_ERR(nvme_thread);
 
-	nvme_major = register_blkdev(nvme_major, "nvme");
-	if (nvme_major <= 0)
+	result = register_blkdev(nvme_major, "nvme");
+	if (result < 0)
 		goto kill_kthread;
+	else if (result > 0)
+		nvme_major = result;
 
 	result = pci_register_driver(&nvme_driver);
 	if (result)
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 9917943a3572..54a55f03115d 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -246,13 +246,12 @@ static int rbd_open(struct block_device *bdev, fmode_t mode)
 {
 	struct rbd_device *rbd_dev = bdev->bd_disk->private_data;
 
-	rbd_get_dev(rbd_dev);
-
-	set_device_ro(bdev, rbd_dev->read_only);
-
 	if ((mode & FMODE_WRITE) && rbd_dev->read_only)
 		return -EROFS;
 
+	rbd_get_dev(rbd_dev);
+	set_device_ro(bdev, rbd_dev->read_only);
+
 	return 0;
 }
 
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 73f196ca713f..c6decb901e5e 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -337,7 +337,7 @@ static void xen_blkbk_unmap(struct pending_req *req)
 		invcount++;
 	}
 
-	ret = gnttab_unmap_refs(unmap, pages, invcount, false);
+	ret = gnttab_unmap_refs(unmap, NULL, pages, invcount);
 	BUG_ON(ret);
 }
 
diff --git a/drivers/bluetooth/hci_ath.c b/drivers/bluetooth/hci_ath.c
index 12172a6a95c4..0bc8a6a6a148 100644
--- a/drivers/bluetooth/hci_ath.c
+++ b/drivers/bluetooth/hci_ath.c
@@ -58,7 +58,7 @@ static int ath_wakeup_ar3k(struct tty_struct *tty)
 		return status;
 
 	/* Disable Automatic RTSCTS */
-	memcpy(&ktermios, tty->termios, sizeof(ktermios));
+	ktermios = tty->termios;
 	ktermios.c_cflag &= ~CRTSCTS;
 	tty_set_termios(tty, &ktermios);
 
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index ea6f6325f9ba..72bedad6bf8c 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -418,8 +418,8 @@ config APPLICOM
 	  If unsure, say N.
 
 config SONYPI
-	tristate "Sony Vaio Programmable I/O Control Device support (EXPERIMENTAL)"
-	depends on EXPERIMENTAL && X86 && PCI && INPUT && !64BIT
+	tristate "Sony Vaio Programmable I/O Control Device support"
+	depends on X86 && PCI && INPUT && !64BIT
 	---help---
 	  This driver enables access to the Sony Programmable I/O Control
 	  Device which can be found in many (all ?) Sony Vaio laptops.
@@ -566,7 +566,7 @@ source "drivers/char/tpm/Kconfig"
 
 config TELCLOCK
 	tristate "Telecom clock driver for ATCA SBC"
-	depends on EXPERIMENTAL && X86
+	depends on X86
 	default n
 	help
 	  The telecom clock device is specific to the MPCBL0010 and MPCBL0050
diff --git a/drivers/char/agp/sgi-agp.c b/drivers/char/agp/sgi-agp.c
index 192000377737..3a5af2f9b015 100644
--- a/drivers/char/agp/sgi-agp.c
+++ b/drivers/char/agp/sgi-agp.c
@@ -289,12 +289,11 @@ static int __devinit agp_sgi_init(void)
 
 	j = 0;
 	list_for_each_entry(info, &tioca_list, ca_list) {
-		struct list_head *tmp;
 		if (list_empty(info->ca_devices))
 			continue;
-		list_for_each(tmp, info->ca_devices) {
+		list_for_each_entry(pdev, info->ca_devices, bus_list) {
 			u8 cap_ptr;
-			pdev = pci_dev_b(tmp);
+
 			if (pdev->class != (PCI_CLASS_DISPLAY_VGA << 8))
 				continue;
 			cap_ptr = pci_find_capability(pdev, PCI_CAP_ID_AGP);
diff --git a/drivers/char/mmtimer.c b/drivers/char/mmtimer.c
index 33dc2298af73..3d6c0671e996 100644
--- a/drivers/char/mmtimer.c
+++ b/drivers/char/mmtimer.c
@@ -826,7 +826,7 @@ static int __init mmtimer_init(void)
 
 	/* Allocate list of node ptrs to mmtimer_t's */
 	timers = kzalloc(sizeof(struct mmtimer_node)*maxn, GFP_KERNEL);
-	if (timers == NULL) {
+	if (!timers) {
 		printk(KERN_ERR "%s: failed to allocate memory for device\n",
 				MMTIMER_NAME);
 		goto out3;
@@ -848,7 +848,6 @@ static int __init mmtimer_init(void)
 	return 0;
 
 out3:
-	kfree(timers);
 	misc_deregister(&mmtimer_miscdev);
 out2:
 	free_irq(SGI_MMTIMER_VECTOR, NULL);
diff --git a/drivers/char/mwave/mwavedd.c b/drivers/char/mwave/mwavedd.c
index 1d82d5838f0c..164544afd680 100644
--- a/drivers/char/mwave/mwavedd.c
+++ b/drivers/char/mwave/mwavedd.c
@@ -430,7 +430,7 @@ static ssize_t mwave_write(struct file *file, const char __user *buf,
 
 static int register_serial_portandirq(unsigned int port, int irq)
 {
-	struct uart_port uart;
+	struct uart_8250_port uart;
 	
 	switch ( port ) {
 		case 0x3f8:
@@ -462,14 +462,14 @@ static int register_serial_portandirq(unsigned int port, int irq)
 	} /* switch */
 	/* irq is okay */
 
-	memset(&uart, 0, sizeof(struct uart_port));
+	memset(&uart, 0, sizeof(uart));
 	
-	uart.uartclk =  1843200;
-	uart.iobase = port;
-	uart.irq = irq;
-	uart.iotype = UPIO_PORT;
-	uart.flags =  UPF_SHARE_IRQ;
-	return serial8250_register_port(&uart);
+	uart.port.uartclk =  1843200;
+	uart.port.iobase = port;
+	uart.port.irq = irq;
+	uart.port.iotype = UPIO_PORT;
+	uart.port.flags =  UPF_SHARE_IRQ;
+	return serial8250_register_8250_port(&uart);
 }
 
 
diff --git a/drivers/char/nwbutton.c b/drivers/char/nwbutton.c
index 04a480f86c6c..cfdfe493c6af 100644
--- a/drivers/char/nwbutton.c
+++ b/drivers/char/nwbutton.c
@@ -93,9 +93,9 @@ int button_del_callback (void (*callback) (void))
 			button_callback_list [lp].count = 0;
 			callback_count--;
 			return 0;
-		};
+		}
 		lp--;
-	};
+	}
 	return -EINVAL;
 }
 
diff --git a/drivers/char/pcmcia/synclink_cs.c b/drivers/char/pcmcia/synclink_cs.c
index 0a484b4a1b02..21721d25e388 100644
--- a/drivers/char/pcmcia/synclink_cs.c
+++ b/drivers/char/pcmcia/synclink_cs.c
@@ -891,6 +891,14 @@ static void rx_ready_async(MGSLPC_INFO *info, int tcd, struct tty_struct *tty)
 	int work = 0;
  	struct mgsl_icount *icount = &info->icount;
 
+	if (!tty) {
+		/* tty is not available anymore */
+		issue_command(info, CHA, CMD_RXRESET);
+		if (debug_level >= DEBUG_LEVEL_ISR)
+			printk("%s(%d):rx_ready_async(tty=NULL)\n",__FILE__,__LINE__);
+		return;
+	}
+
 	if (tcd) {
 		/* early termination, get FIFO count from RBCL register */
 		fifo_count = (unsigned char)(read_reg(info, CHA+RBCL) & 0x1f);
@@ -980,7 +988,7 @@ static void tx_done(MGSLPC_INFO *info, struct tty_struct *tty)
 	else
 #endif
 	{
-		if (tty->stopped || tty->hw_stopped) {
+		if (tty && (tty->stopped || tty->hw_stopped)) {
 			tx_stop(info);
 			return;
 		}
@@ -1000,7 +1008,7 @@ static void tx_ready(MGSLPC_INFO *info, struct tty_struct *tty)
 		if (!info->tx_active)
 			return;
 	} else {
-		if (tty->stopped || tty->hw_stopped) {
+		if (tty && (tty->stopped || tty->hw_stopped)) {
 			tx_stop(info);
 			return;
 		}
@@ -1050,13 +1058,12 @@ static void cts_change(MGSLPC_INFO *info, struct tty_struct *tty)
 	wake_up_interruptible(&info->status_event_wait_q);
 	wake_up_interruptible(&info->event_wait_q);
 
-	if (info->port.flags & ASYNC_CTS_FLOW) {
+	if (tty && tty_port_cts_enabled(&info->port)) {
 		if (tty->hw_stopped) {
 			if (info->serial_signals & SerialSignal_CTS) {
 				if (debug_level >= DEBUG_LEVEL_ISR)
 					printk("CTS tx start...");
-				if (tty)
-					tty->hw_stopped = 0;
+				tty->hw_stopped = 0;
 				tx_start(info, tty);
 				info->pending_bh |= BH_TRANSMIT;
 				return;
@@ -1065,8 +1072,7 @@ static void cts_change(MGSLPC_INFO *info, struct tty_struct *tty)
 			if (!(info->serial_signals & SerialSignal_CTS)) {
 				if (debug_level >= DEBUG_LEVEL_ISR)
 					printk("CTS tx stop...");
-				if (tty)
-					tty->hw_stopped = 1;
+				tty->hw_stopped = 1;
 				tx_stop(info);
 			}
 		}
@@ -1344,7 +1350,7 @@ static void shutdown(MGSLPC_INFO * info, struct tty_struct *tty)
 	/* TODO:disable interrupts instead of reset to preserve signal states */
 	reset_device(info);
 
- 	if (!tty || tty->termios->c_cflag & HUPCL) {
+ 	if (!tty || tty->termios.c_cflag & HUPCL) {
  		info->serial_signals &= ~(SerialSignal_DTR + SerialSignal_RTS);
 		set_signals(info);
 	}
@@ -1385,7 +1391,7 @@ static void mgslpc_program_hw(MGSLPC_INFO *info, struct tty_struct *tty)
 	port_irq_enable(info, (unsigned char) PVR_DSR | PVR_RI);
 	get_signals(info);
 
-	if (info->netcount || (tty && (tty->termios->c_cflag & CREAD)))
+	if (info->netcount || (tty && (tty->termios.c_cflag & CREAD)))
 		rx_start(info);
 
 	spin_unlock_irqrestore(&info->lock,flags);
@@ -1398,14 +1404,14 @@ static void mgslpc_change_params(MGSLPC_INFO *info, struct tty_struct *tty)
 	unsigned cflag;
 	int bits_per_char;
 
-	if (!tty || !tty->termios)
+	if (!tty)
 		return;
 
 	if (debug_level >= DEBUG_LEVEL_INFO)
 		printk("%s(%d):mgslpc_change_params(%s)\n",
 			 __FILE__,__LINE__, info->device_name );
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	/* if B0 rate (hangup) specified then negate DTR and RTS */
 	/* otherwise assert DTR and RTS */
@@ -1728,7 +1734,7 @@ static void mgslpc_throttle(struct tty_struct * tty)
 	if (I_IXOFF(tty))
 		mgslpc_send_xchar(tty, STOP_CHAR(tty));
 
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->serial_signals &= ~SerialSignal_RTS;
 	 	set_signals(info);
@@ -1757,7 +1763,7 @@ static void mgslpc_unthrottle(struct tty_struct * tty)
 			mgslpc_send_xchar(tty, START_CHAR(tty));
 	}
 
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->serial_signals |= SerialSignal_RTS;
 	 	set_signals(info);
@@ -2293,8 +2299,8 @@ static void mgslpc_set_termios(struct tty_struct *tty, struct ktermios *old_term
 			tty->driver->name );
 
 	/* just return if nothing has changed */
-	if ((tty->termios->c_cflag == old_termios->c_cflag)
-	    && (RELEVANT_IFLAG(tty->termios->c_iflag)
+	if ((tty->termios.c_cflag == old_termios->c_cflag)
+	    && (RELEVANT_IFLAG(tty->termios.c_iflag)
 		== RELEVANT_IFLAG(old_termios->c_iflag)))
 	  return;
 
@@ -2302,7 +2308,7 @@ static void mgslpc_set_termios(struct tty_struct *tty, struct ktermios *old_term
 
 	/* Handle transition to B0 status */
 	if (old_termios->c_cflag & CBAUD &&
-	    !(tty->termios->c_cflag & CBAUD)) {
+	    !(tty->termios.c_cflag & CBAUD)) {
 		info->serial_signals &= ~(SerialSignal_RTS + SerialSignal_DTR);
 		spin_lock_irqsave(&info->lock,flags);
 	 	set_signals(info);
@@ -2311,9 +2317,9 @@ static void mgslpc_set_termios(struct tty_struct *tty, struct ktermios *old_term
 
 	/* Handle transition away from B0 status */
 	if (!(old_termios->c_cflag & CBAUD) &&
-	    tty->termios->c_cflag & CBAUD) {
+	    tty->termios.c_cflag & CBAUD) {
 		info->serial_signals |= SerialSignal_DTR;
- 		if (!(tty->termios->c_cflag & CRTSCTS) ||
+ 		if (!(tty->termios.c_cflag & CRTSCTS) ||
  		    !test_bit(TTY_THROTTLED, &tty->flags)) {
 			info->serial_signals |= SerialSignal_RTS;
  		}
@@ -2324,7 +2330,7 @@ static void mgslpc_set_termios(struct tty_struct *tty, struct ktermios *old_term
 
 	/* Handle turning off CRTSCTS */
 	if (old_termios->c_cflag & CRTSCTS &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		tx_release(tty);
 	}
@@ -2731,6 +2737,8 @@ static void mgslpc_add_device(MGSLPC_INFO *info)
 #if SYNCLINK_GENERIC_HDLC
 	hdlcdev_init(info);
 #endif
+	tty_port_register_device(&info->port, serial_driver, info->line,
+			&info->p_dev->dev);
 }
 
 static void mgslpc_remove_device(MGSLPC_INFO *remove_info)
@@ -2744,6 +2752,7 @@ static void mgslpc_remove_device(MGSLPC_INFO *remove_info)
 				last->next_device = info->next_device;
 			else
 				mgslpc_device_list = info->next_device;
+			tty_unregister_device(serial_driver, info->line);
 #if SYNCLINK_GENERIC_HDLC
 			hdlcdev_exit(info);
 #endif
@@ -2798,77 +2807,63 @@ static const struct tty_operations mgslpc_ops = {
 	.proc_fops = &mgslpc_proc_fops,
 };
 
-static void synclink_cs_cleanup(void)
+static int __init synclink_cs_init(void)
 {
 	int rc;
 
-	while(mgslpc_device_list)
-		mgslpc_remove_device(mgslpc_device_list);
-
-	if (serial_driver) {
-		if ((rc = tty_unregister_driver(serial_driver)))
-			printk("%s(%d) failed to unregister tty driver err=%d\n",
-			       __FILE__,__LINE__,rc);
-		put_tty_driver(serial_driver);
+	if (break_on_load) {
+		mgslpc_get_text_ptr();
+		BREAKPOINT();
 	}
 
-	pcmcia_unregister_driver(&mgslpc_driver);
-}
-
-static int __init synclink_cs_init(void)
-{
-    int rc;
-
-    if (break_on_load) {
-	    mgslpc_get_text_ptr();
-	    BREAKPOINT();
-    }
-
-    if ((rc = pcmcia_register_driver(&mgslpc_driver)) < 0)
-	    return rc;
-
-    serial_driver = alloc_tty_driver(MAX_DEVICE_COUNT);
-    if (!serial_driver) {
-	    rc = -ENOMEM;
-	    goto error;
-    }
+	serial_driver = tty_alloc_driver(MAX_DEVICE_COUNT,
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_DEV);
+	if (IS_ERR(serial_driver)) {
+		rc = PTR_ERR(serial_driver);
+		goto err;
+	}
 
-    /* Initialize the tty_driver structure */
-
-    serial_driver->driver_name = "synclink_cs";
-    serial_driver->name = "ttySLP";
-    serial_driver->major = ttymajor;
-    serial_driver->minor_start = 64;
-    serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
-    serial_driver->subtype = SERIAL_TYPE_NORMAL;
-    serial_driver->init_termios = tty_std_termios;
-    serial_driver->init_termios.c_cflag =
-	    B9600 | CS8 | CREAD | HUPCL | CLOCAL;
-    serial_driver->flags = TTY_DRIVER_REAL_RAW;
-    tty_set_operations(serial_driver, &mgslpc_ops);
-
-    if ((rc = tty_register_driver(serial_driver)) < 0) {
-	    printk("%s(%d):Couldn't register serial driver\n",
-		   __FILE__,__LINE__);
-	    put_tty_driver(serial_driver);
-	    serial_driver = NULL;
-	    goto error;
-    }
+	/* Initialize the tty_driver structure */
+	serial_driver->driver_name = "synclink_cs";
+	serial_driver->name = "ttySLP";
+	serial_driver->major = ttymajor;
+	serial_driver->minor_start = 64;
+	serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	serial_driver->subtype = SERIAL_TYPE_NORMAL;
+	serial_driver->init_termios = tty_std_termios;
+	serial_driver->init_termios.c_cflag =
+	B9600 | CS8 | CREAD | HUPCL | CLOCAL;
+	tty_set_operations(serial_driver, &mgslpc_ops);
+
+	rc = tty_register_driver(serial_driver);
+	if (rc < 0) {
+		printk(KERN_ERR "%s(%d):Couldn't register serial driver\n",
+				__FILE__, __LINE__);
+		goto err_put_tty;
+	}
 
-    printk("%s %s, tty major#%d\n",
-	   driver_name, driver_version,
-	   serial_driver->major);
+	rc = pcmcia_register_driver(&mgslpc_driver);
+	if (rc < 0)
+		goto err_unreg_tty;
 
-    return 0;
+	printk(KERN_INFO "%s %s, tty major#%d\n", driver_name, driver_version,
+			serial_driver->major);
 
-error:
-    synclink_cs_cleanup();
-    return rc;
+	return 0;
+err_unreg_tty:
+	tty_unregister_driver(serial_driver);
+err_put_tty:
+	put_tty_driver(serial_driver);
+err:
+	return rc;
 }
 
 static void __exit synclink_cs_exit(void)
 {
-	synclink_cs_cleanup();
+	pcmcia_unregister_driver(&mgslpc_driver);
+	tty_unregister_driver(serial_driver);
+	put_tty_driver(serial_driver);
 }
 
 module_init(synclink_cs_init);
diff --git a/drivers/char/ppdev.c b/drivers/char/ppdev.c
index 3fcf80ff12f2..d0d824ebf2c1 100644
--- a/drivers/char/ppdev.c
+++ b/drivers/char/ppdev.c
@@ -783,7 +783,8 @@ static int __init ppdev_init (void)
 		err = PTR_ERR(ppdev_class);
 		goto out_chrdev;
 	}
-	if (parport_register_driver(&pp_driver)) {
+	err = parport_register_driver(&pp_driver);
+	if (err < 0) {
 		printk (KERN_WARNING CHRDEV ": unable to register with parport\n");
 		goto out_class;
 	}
diff --git a/drivers/char/rtc.c b/drivers/char/rtc.c
index af9437488b6c..91470fdbab2a 100644
--- a/drivers/char/rtc.c
+++ b/drivers/char/rtc.c
@@ -411,7 +411,7 @@ static int rtc_do_ioctl(unsigned int cmd, unsigned long arg, int kernel)
 		case RTC_IRQP_READ:
 		case RTC_IRQP_SET:
 			return -EINVAL;
-		};
+		}
 	}
 #endif
 
diff --git a/drivers/char/tlclk.c b/drivers/char/tlclk.c
index ce29e7cce528..e95e0ab0bd87 100644
--- a/drivers/char/tlclk.c
+++ b/drivers/char/tlclk.c
@@ -784,8 +784,10 @@ static int __init tlclk_init(void)
 	}
 	tlclk_major = ret;
 	alarm_events = kzalloc( sizeof(struct tlclk_alarms), GFP_KERNEL);
-	if (!alarm_events)
+	if (!alarm_events) {
+		ret = -ENOMEM;
 		goto out1;
+	}
 
 	/* Read telecom clock IRQ number (Set by BIOS) */
 	if (!request_region(TLCLK_BASE, 8, "telco_clock")) {
diff --git a/drivers/char/ttyprintk.c b/drivers/char/ttyprintk.c
index 46b77ede84c0..af98f6d6509b 100644
--- a/drivers/char/ttyprintk.c
+++ b/drivers/char/ttyprintk.c
@@ -67,7 +67,7 @@ static int tpk_printk(const unsigned char *buf, int count)
 				tmp[tpk_curr + 1] = '\0';
 				printk(KERN_INFO "%s%s", tpk_tag, tmp);
 				tpk_curr = 0;
-				if (buf[i + 1] == '\n')
+				if ((i + 1) < count && buf[i + 1] == '\n')
 					i++;
 				break;
 			case '\n':
@@ -178,11 +178,17 @@ static struct tty_driver *ttyprintk_driver;
 static int __init ttyprintk_init(void)
 {
 	int ret = -ENOMEM;
-	void *rp;
 
-	ttyprintk_driver = alloc_tty_driver(1);
-	if (!ttyprintk_driver)
-		return ret;
+	tty_port_init(&tpk_port.port);
+	tpk_port.port.ops = &null_ops;
+	mutex_init(&tpk_port.port_write_mutex);
+
+	ttyprintk_driver = tty_alloc_driver(1,
+			TTY_DRIVER_RESET_TERMIOS |
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_UNNUMBERED_NODE);
+	if (IS_ERR(ttyprintk_driver))
+		return PTR_ERR(ttyprintk_driver);
 
 	ttyprintk_driver->driver_name = "ttyprintk";
 	ttyprintk_driver->name = "ttyprintk";
@@ -191,9 +197,8 @@ static int __init ttyprintk_init(void)
 	ttyprintk_driver->type = TTY_DRIVER_TYPE_CONSOLE;
 	ttyprintk_driver->init_termios = tty_std_termios;
 	ttyprintk_driver->init_termios.c_oflag = OPOST | OCRNL | ONOCR | ONLRET;
-	ttyprintk_driver->flags = TTY_DRIVER_RESET_TERMIOS |
-		TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
 	tty_set_operations(ttyprintk_driver, &ttyprintk_ops);
+	tty_port_link_device(&tpk_port.port, ttyprintk_driver, 0);
 
 	ret = tty_register_driver(ttyprintk_driver);
 	if (ret < 0) {
@@ -201,22 +206,10 @@ static int __init ttyprintk_init(void)
 		goto error;
 	}
 
-	/* create our unnumbered device */
-	rp = device_create(tty_class, NULL, MKDEV(TTYAUX_MAJOR, 3), NULL,
-				ttyprintk_driver->name);
-	if (IS_ERR(rp)) {
-		printk(KERN_ERR "Couldn't create ttyprintk device\n");
-		ret = PTR_ERR(rp);
-		goto error;
-	}
-
-	tty_port_init(&tpk_port.port);
-	tpk_port.port.ops = &null_ops;
-	mutex_init(&tpk_port.port_write_mutex);
-
 	return 0;
 
 error:
+	tty_unregister_driver(ttyprintk_driver);
 	put_tty_driver(ttyprintk_driver);
 	ttyprintk_driver = NULL;
 	return ret;
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index cdf2f5451c76..060a672ebb7b 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -1941,7 +1941,17 @@ static int __init init(void)
 	INIT_LIST_HEAD(&pdrvdata.consoles);
 	INIT_LIST_HEAD(&pdrvdata.portdevs);
 
-	return register_virtio_driver(&virtio_console);
+	err = register_virtio_driver(&virtio_console);
+	if (err < 0) {
+		pr_err("Error %d registering virtio driver\n", err);
+		goto free;
+	}
+	return 0;
+free:
+	if (pdrvdata.debugfs_dir)
+		debugfs_remove_recursive(pdrvdata.debugfs_dir);
+	class_destroy(pdrvdata.class);
+	return err;
 }
 
 static void __exit fini(void)
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 7f0b5ca78516..bace9e98f75d 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -40,4 +40,17 @@ config COMMON_CLK_WM831X
           Supports the clocking subsystem of the WM831x/2x series of
 	  PMICs from Wolfson Microlectronics.
 
+config COMMON_CLK_VERSATILE
+	bool "Clock driver for ARM Reference designs"
+	depends on ARCH_INTEGRATOR || ARCH_REALVIEW
+	---help---
+          Supports clocking on ARM Reference designs Integrator/AP,
+	  Integrator/CP, RealView PB1176, EB, PB11MP and PBX.
+
+config COMMON_CLK_MAX77686
+	tristate "Clock driver for Maxim 77686 MFD"
+	depends on MFD_MAX77686
+	---help---
+	  This driver supports Maxim 77686 crystal oscillator clock. 
+
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 5869ea387054..b7b862077d88 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -1,4 +1,5 @@
 # common clock types
+obj-$(CONFIG_HAVE_CLK)		+= clk-devres.o
 obj-$(CONFIG_CLKDEV_LOOKUP)	+= clkdev.o
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-fixed-rate.o clk-gate.o \
 				   clk-mux.o clk-divider.o clk-fixed-factor.o
@@ -9,7 +10,14 @@ obj-$(CONFIG_ARCH_MXS)		+= mxs/
 obj-$(CONFIG_ARCH_SOCFPGA)	+= socfpga/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_U300)		+= clk-u300.o
-obj-$(CONFIG_ARCH_INTEGRATOR)	+= versatile/
+obj-$(CONFIG_COMMON_CLK_VERSATILE) += versatile/
+obj-$(CONFIG_ARCH_PRIMA2)	+= clk-prima2.o
+ifeq ($(CONFIG_COMMON_CLK), y)
+obj-$(CONFIG_ARCH_MMP)		+= mmp/
+endif
+obj-$(CONFIG_MACH_LOONGSON1)	+= clk-ls1x.o
+obj-$(CONFIG_ARCH_U8500)	+= ux500/
 
 # Chip specific
 obj-$(CONFIG_COMMON_CLK_WM831X) += clk-wm831x.o
+obj-$(CONFIG_COMMON_CLK_MAX77686) += clk-max77686.o
diff --git a/drivers/clk/clk-devres.c b/drivers/clk/clk-devres.c
new file mode 100644
index 000000000000..8f571548870f
--- /dev/null
+++ b/drivers/clk/clk-devres.c
@@ -0,0 +1,55 @@
+/*
+ * 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/device.h>
+#include <linux/export.h>
+#include <linux/gfp.h>
+
+static void devm_clk_release(struct device *dev, void *res)
+{
+	clk_put(*(struct clk **)res);
+}
+
+struct clk *devm_clk_get(struct device *dev, const char *id)
+{
+	struct clk **ptr, *clk;
+
+	ptr = devres_alloc(devm_clk_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	clk = clk_get(dev, id);
+	if (!IS_ERR(clk)) {
+		*ptr = clk;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return clk;
+}
+EXPORT_SYMBOL(devm_clk_get);
+
+static int devm_clk_match(struct device *dev, void *res, void *data)
+{
+	struct clk **c = res;
+	if (!c || !*c) {
+		WARN_ON(!c || !*c);
+		return 0;
+	}
+	return *c == data;
+}
+
+void devm_clk_put(struct device *dev, struct clk *clk)
+{
+	int ret;
+
+	ret = devres_release(dev, devm_clk_release, devm_clk_match, clk);
+
+	WARN_ON(ret);
+}
+EXPORT_SYMBOL(devm_clk_put);
diff --git a/drivers/clk/clk-ls1x.c b/drivers/clk/clk-ls1x.c
new file mode 100644
index 000000000000..f20b750235f6
--- /dev/null
+++ b/drivers/clk/clk-ls1x.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2012 Zhang, Keguang <keguang.zhang@gmail.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include <loongson1.h>
+
+#define OSC	33
+
+static DEFINE_SPINLOCK(_lock);
+
+static int ls1x_pll_clk_enable(struct clk_hw *hw)
+{
+	return 0;
+}
+
+static void ls1x_pll_clk_disable(struct clk_hw *hw)
+{
+}
+
+static unsigned long ls1x_pll_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	u32 pll, rate;
+
+	pll = __raw_readl(LS1X_CLK_PLL_FREQ);
+	rate = ((12 + (pll & 0x3f)) * 1000000) +
+		((((pll >> 8) & 0x3ff) * 1000000) >> 10);
+	rate *= OSC;
+	rate >>= 1;
+
+	return rate;
+}
+
+static const struct clk_ops ls1x_pll_clk_ops = {
+	.enable = ls1x_pll_clk_enable,
+	.disable = ls1x_pll_clk_disable,
+	.recalc_rate = ls1x_pll_recalc_rate,
+};
+
+static struct clk * __init clk_register_pll(struct device *dev,
+	 const char *name, const char *parent_name, unsigned long flags)
+{
+	struct clk_hw *hw;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	/* allocate the divider */
+	hw = kzalloc(sizeof(struct clk_hw), GFP_KERNEL);
+	if (!hw) {
+		pr_err("%s: could not allocate clk_hw\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.ops = &ls1x_pll_clk_ops;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	hw->init = &init;
+
+	/* register the clock */
+	clk = clk_register(dev, hw);
+
+	if (IS_ERR(clk))
+		kfree(hw);
+
+	return clk;
+}
+
+void __init ls1x_clk_init(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_pll(NULL, "pll_clk", NULL, CLK_IS_ROOT);
+	clk_prepare_enable(clk);
+
+	clk = clk_register_divider(NULL, "cpu_clk", "pll_clk",
+			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_CPU_SHIFT,
+			DIV_CPU_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_prepare_enable(clk);
+	clk_register_clkdev(clk, "cpu", NULL);
+
+	clk = clk_register_divider(NULL, "dc_clk", "pll_clk",
+			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
+			DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_prepare_enable(clk);
+	clk_register_clkdev(clk, "dc", NULL);
+
+	clk = clk_register_divider(NULL, "ahb_clk", "pll_clk",
+			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_DDR_SHIFT,
+			DIV_DDR_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_prepare_enable(clk);
+	clk_register_clkdev(clk, "ahb", NULL);
+	clk_register_clkdev(clk, "stmmaceth", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1, 2);
+	clk_prepare_enable(clk);
+	clk_register_clkdev(clk, "apb", NULL);
+	clk_register_clkdev(clk, "serial8250", NULL);
+}
diff --git a/drivers/clk/clk-max77686.c b/drivers/clk/clk-max77686.c
new file mode 100644
index 000000000000..ac5f5434cb9a
--- /dev/null
+++ b/drivers/clk/clk-max77686.c
@@ -0,0 +1,244 @@
+/*
+ * clk-max77686.c - Clock driver for Maxim 77686
+ *
+ * Copyright (C) 2012 Samsung Electornics
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/max77686.h>
+#include <linux/mfd/max77686-private.h>
+#include <linux/clk-provider.h>
+#include <linux/mutex.h>
+#include <linux/clkdev.h>
+
+enum {
+	MAX77686_CLK_AP = 0,
+	MAX77686_CLK_CP,
+	MAX77686_CLK_PMIC,
+	MAX77686_CLKS_NUM,
+};
+
+struct max77686_clk {
+	struct max77686_dev *iodev;
+	u32 mask;
+	struct clk_hw hw;
+	struct clk_lookup *lookup;
+};
+
+static struct max77686_clk *get_max77686_clk(struct clk_hw *hw)
+{
+	return container_of(hw, struct max77686_clk, hw);
+}
+
+static int max77686_clk_prepare(struct clk_hw *hw)
+{
+	struct max77686_clk *max77686;
+	int ret;
+
+	max77686 = get_max77686_clk(hw);
+	if (!max77686)
+		return -ENOMEM;
+
+	ret = regmap_update_bits(max77686->iodev->regmap,
+		MAX77686_REG_32KHZ, max77686->mask, max77686->mask);
+
+	return ret;
+}
+
+static void max77686_clk_unprepare(struct clk_hw *hw)
+{
+	struct max77686_clk *max77686;
+
+	max77686 = get_max77686_clk(hw);
+	if (!max77686)
+		return;
+
+	regmap_update_bits(max77686->iodev->regmap,
+		MAX77686_REG_32KHZ, max77686->mask, ~max77686->mask);
+}
+
+static int max77686_clk_is_enabled(struct clk_hw *hw)
+{
+	struct max77686_clk *max77686;
+	int ret;
+	u32 val;
+
+	max77686 = get_max77686_clk(hw);
+	if (!max77686)
+		return -ENOMEM;
+
+	ret = regmap_read(max77686->iodev->regmap,
+				MAX77686_REG_32KHZ, &val);
+
+	if (ret < 0)
+		return -EINVAL;
+
+	return val & max77686->mask;
+}
+
+static struct clk_ops max77686_clk_ops = {
+	.prepare	= max77686_clk_prepare,
+	.unprepare	= max77686_clk_unprepare,
+	.is_enabled	= max77686_clk_is_enabled,
+};
+
+static struct clk_init_data max77686_clks_init[MAX77686_CLKS_NUM] = {
+	[MAX77686_CLK_AP] = {
+		.name = "32khz_ap",
+		.ops = &max77686_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+	[MAX77686_CLK_CP] = {
+		.name = "32khz_cp",
+		.ops = &max77686_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+	[MAX77686_CLK_PMIC] = {
+		.name = "32khz_pmic",
+		.ops = &max77686_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+};
+
+static int max77686_clk_register(struct device *dev,
+				struct max77686_clk *max77686)
+{
+	struct clk *clk;
+	struct clk_hw *hw = &max77686->hw;
+
+	clk = clk_register(dev, hw);
+
+	if (IS_ERR(clk))
+		return -ENOMEM;
+
+	max77686->lookup = devm_kzalloc(dev, sizeof(struct clk_lookup),
+					GFP_KERNEL);
+	if (IS_ERR(max77686->lookup))
+		return -ENOMEM;
+
+	max77686->lookup->con_id = hw->init->name;
+	max77686->lookup->clk = clk;
+
+	clkdev_add(max77686->lookup);
+
+	return 0;
+}
+
+static __devinit int max77686_clk_probe(struct platform_device *pdev)
+{
+	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct max77686_clk **max77686_clks;
+	int i, ret;
+
+	max77686_clks = devm_kzalloc(&pdev->dev, sizeof(struct max77686_clk *)
+					* MAX77686_CLKS_NUM, GFP_KERNEL);
+	if (IS_ERR(max77686_clks))
+		return -ENOMEM;
+
+	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
+		max77686_clks[i] = devm_kzalloc(&pdev->dev,
+					sizeof(struct max77686_clk), GFP_KERNEL);
+		if (IS_ERR(max77686_clks[i]))
+			return -ENOMEM;
+	}
+
+	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
+		max77686_clks[i]->iodev = iodev;
+		max77686_clks[i]->mask = 1 << i;
+		max77686_clks[i]->hw.init = &max77686_clks_init[i];
+
+		ret = max77686_clk_register(&pdev->dev, max77686_clks[i]);
+		if (ret) {
+			switch (i) {
+			case MAX77686_CLK_AP:
+				dev_err(&pdev->dev, "Fail to register CLK_AP\n");
+				goto err_clk_ap;
+				break;
+			case MAX77686_CLK_CP:
+				dev_err(&pdev->dev, "Fail to register CLK_CP\n");
+				goto err_clk_cp;
+				break;
+			case MAX77686_CLK_PMIC:
+				dev_err(&pdev->dev, "Fail to register CLK_PMIC\n");
+				goto err_clk_pmic;
+			}
+		}
+	}
+
+	platform_set_drvdata(pdev, max77686_clks);
+
+	goto out;
+
+err_clk_pmic:
+	clkdev_drop(max77686_clks[MAX77686_CLK_CP]->lookup);
+	kfree(max77686_clks[MAX77686_CLK_CP]->hw.clk);
+err_clk_cp:
+	clkdev_drop(max77686_clks[MAX77686_CLK_AP]->lookup);
+	kfree(max77686_clks[MAX77686_CLK_AP]->hw.clk);
+err_clk_ap:
+out:
+	return ret;
+}
+
+static int __devexit max77686_clk_remove(struct platform_device *pdev)
+{
+	struct max77686_clk **max77686_clks = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
+		clkdev_drop(max77686_clks[i]->lookup);
+		kfree(max77686_clks[i]->hw.clk);
+	}
+	return 0;
+}
+
+static const struct platform_device_id max77686_clk_id[] = {
+	{ "max77686-clk", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, max77686_clk_id);
+
+static struct platform_driver max77686_clk_driver = {
+	.driver = {
+		.name  = "max77686-clk",
+		.owner = THIS_MODULE,
+	},
+	.probe = max77686_clk_probe,
+	.remove = __devexit_p(max77686_clk_remove),
+	.id_table = max77686_clk_id,
+};
+
+static int __init max77686_clk_init(void)
+{
+	return platform_driver_register(&max77686_clk_driver);
+}
+subsys_initcall(max77686_clk_init);
+
+static void __init max77686_clk_cleanup(void)
+{
+	platform_driver_unregister(&max77686_clk_driver);
+}
+module_exit(max77686_clk_cleanup);
+
+MODULE_DESCRIPTION("MAXIM 77686 Clock Driver");
+MODULE_AUTHOR("Jonghwa Lee <jonghwa3.lee@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-prima2.c b/drivers/clk/clk-prima2.c
new file mode 100644
index 000000000000..517874fa6858
--- /dev/null
+++ b/drivers/clk/clk-prima2.c
@@ -0,0 +1,1171 @@
+/*
+ * Clock tree for CSR SiRFprimaII
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#define SIRFSOC_CLKC_CLK_EN0    0x0000
+#define SIRFSOC_CLKC_CLK_EN1    0x0004
+#define SIRFSOC_CLKC_REF_CFG    0x0014
+#define SIRFSOC_CLKC_CPU_CFG    0x0018
+#define SIRFSOC_CLKC_MEM_CFG    0x001c
+#define SIRFSOC_CLKC_SYS_CFG    0x0020
+#define SIRFSOC_CLKC_IO_CFG     0x0024
+#define SIRFSOC_CLKC_DSP_CFG    0x0028
+#define SIRFSOC_CLKC_GFX_CFG    0x002c
+#define SIRFSOC_CLKC_MM_CFG     0x0030
+#define SIRFSOC_CLKC_LCD_CFG     0x0034
+#define SIRFSOC_CLKC_MMC_CFG    0x0038
+#define SIRFSOC_CLKC_PLL1_CFG0  0x0040
+#define SIRFSOC_CLKC_PLL2_CFG0  0x0044
+#define SIRFSOC_CLKC_PLL3_CFG0  0x0048
+#define SIRFSOC_CLKC_PLL1_CFG1  0x004c
+#define SIRFSOC_CLKC_PLL2_CFG1  0x0050
+#define SIRFSOC_CLKC_PLL3_CFG1  0x0054
+#define SIRFSOC_CLKC_PLL1_CFG2  0x0058
+#define SIRFSOC_CLKC_PLL2_CFG2  0x005c
+#define SIRFSOC_CLKC_PLL3_CFG2  0x0060
+#define SIRFSOC_USBPHY_PLL_CTRL 0x0008
+#define SIRFSOC_USBPHY_PLL_POWERDOWN  BIT(1)
+#define SIRFSOC_USBPHY_PLL_BYPASS     BIT(2)
+#define SIRFSOC_USBPHY_PLL_LOCK       BIT(3)
+
+static void *sirfsoc_clk_vbase, *sirfsoc_rsc_vbase;
+
+#define KHZ     1000
+#define MHZ     (KHZ * KHZ)
+
+/*
+ * SiRFprimaII clock controller
+ * - 2 oscillators: osc-26MHz, rtc-32.768KHz
+ * - 3 standard configurable plls: pll1, pll2 & pll3
+ * - 2 exclusive plls: usb phy pll and sata phy pll
+ * - 8 clock domains: cpu/cpudiv, mem/memdiv, sys/io, dsp, graphic, multimedia,
+ *     display and sdphy.
+ *     Each clock domain can select its own clock source from five clock sources,
+ *     X_XIN, X_XINW, PLL1, PLL2 and PLL3. The domain clock is used as the source
+ *     clock of the group clock.
+ *     - dsp domain: gps, mf
+ *     - io domain: dmac, nand, audio, uart, i2c, spi, usp, pwm, pulse
+ *     - sys domain: security
+ */
+
+struct clk_pll {
+	struct clk_hw hw;
+	unsigned short regofs;  /* register offset */
+};
+
+#define to_pllclk(_hw) container_of(_hw, struct clk_pll, hw)
+
+struct clk_dmn {
+	struct clk_hw hw;
+	signed char enable_bit; /* enable bit: 0 ~ 63 */
+	unsigned short regofs;  /* register offset */
+};
+
+#define to_dmnclk(_hw) container_of(_hw, struct clk_dmn, hw)
+
+struct clk_std {
+	struct clk_hw hw;
+	signed char enable_bit; /* enable bit: 0 ~ 63 */
+};
+
+#define to_stdclk(_hw) container_of(_hw, struct clk_std, hw)
+
+static int std_clk_is_enabled(struct clk_hw *hw);
+static int std_clk_enable(struct clk_hw *hw);
+static void std_clk_disable(struct clk_hw *hw);
+
+static inline unsigned long clkc_readl(unsigned reg)
+{
+	return readl(sirfsoc_clk_vbase + reg);
+}
+
+static inline void clkc_writel(u32 val, unsigned reg)
+{
+	writel(val, sirfsoc_clk_vbase + reg);
+}
+
+/*
+ * std pll
+ */
+
+static unsigned long pll_clk_recalc_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+{
+	unsigned long fin = parent_rate;
+	struct clk_pll *clk = to_pllclk(hw);
+	u32 regcfg2 = clk->regofs + SIRFSOC_CLKC_PLL1_CFG2 -
+		SIRFSOC_CLKC_PLL1_CFG0;
+
+	if (clkc_readl(regcfg2) & BIT(2)) {
+		/* pll bypass mode */
+		return fin;
+	} else {
+		/* fout = fin * nf / nr / od */
+		u32 cfg0 = clkc_readl(clk->regofs);
+		u32 nf = (cfg0 & (BIT(13) - 1)) + 1;
+		u32 nr = ((cfg0 >> 13) & (BIT(6) - 1)) + 1;
+		u32 od = ((cfg0 >> 19) & (BIT(4) - 1)) + 1;
+		WARN_ON(fin % MHZ);
+		return fin / MHZ * nf / nr / od * MHZ;
+	}
+}
+
+static long pll_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long *parent_rate)
+{
+	unsigned long fin, nf, nr, od;
+
+	/*
+	 * fout = fin * nf / (nr * od);
+	 * set od = 1, nr = fin/MHz, so fout = nf * MHz
+	 */
+	rate = rate - rate % MHZ;
+
+	nf = rate / MHZ;
+	if (nf > BIT(13))
+		nf = BIT(13);
+	if (nf < 1)
+		nf = 1;
+
+	fin = *parent_rate;
+
+	nr = fin / MHZ;
+	if (nr > BIT(6))
+		nr = BIT(6);
+	od = 1;
+
+	return fin * nf / (nr * od);
+}
+
+static int pll_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long parent_rate)
+{
+	struct clk_pll *clk = to_pllclk(hw);
+	unsigned long fin, nf, nr, od, reg;
+
+	/*
+	 * fout = fin * nf / (nr * od);
+	 * set od = 1, nr = fin/MHz, so fout = nf * MHz
+	 */
+
+	nf = rate / MHZ;
+	if (unlikely((rate % MHZ) || nf > BIT(13) || nf < 1))
+		return -EINVAL;
+
+	fin = parent_rate;
+	BUG_ON(fin < MHZ);
+
+	nr = fin / MHZ;
+	BUG_ON((fin % MHZ) || nr > BIT(6));
+
+	od = 1;
+
+	reg = (nf - 1) | ((nr - 1) << 13) | ((od - 1) << 19);
+	clkc_writel(reg, clk->regofs);
+
+	reg = clk->regofs + SIRFSOC_CLKC_PLL1_CFG1 - SIRFSOC_CLKC_PLL1_CFG0;
+	clkc_writel((nf >> 1) - 1, reg);
+
+	reg = clk->regofs + SIRFSOC_CLKC_PLL1_CFG2 - SIRFSOC_CLKC_PLL1_CFG0;
+	while (!(clkc_readl(reg) & BIT(6)))
+		cpu_relax();
+
+	return 0;
+}
+
+static struct clk_ops std_pll_ops = {
+	.recalc_rate = pll_clk_recalc_rate,
+	.round_rate = pll_clk_round_rate,
+	.set_rate = pll_clk_set_rate,
+};
+
+static const char *pll_clk_parents[] = {
+	"osc",
+};
+
+static struct clk_init_data clk_pll1_init = {
+	.name = "pll1",
+	.ops = &std_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_init_data clk_pll2_init = {
+	.name = "pll2",
+	.ops = &std_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_init_data clk_pll3_init = {
+	.name = "pll3",
+	.ops = &std_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_pll1 = {
+	.regofs = SIRFSOC_CLKC_PLL1_CFG0,
+	.hw = {
+		.init = &clk_pll1_init,
+	},
+};
+
+static struct clk_pll clk_pll2 = {
+	.regofs = SIRFSOC_CLKC_PLL2_CFG0,
+	.hw = {
+		.init = &clk_pll2_init,
+	},
+};
+
+static struct clk_pll clk_pll3 = {
+	.regofs = SIRFSOC_CLKC_PLL3_CFG0,
+	.hw = {
+		.init = &clk_pll3_init,
+	},
+};
+
+/*
+ * usb uses specified pll
+ */
+
+static int usb_pll_clk_enable(struct clk_hw *hw)
+{
+	u32 reg = readl(sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL);
+	reg &= ~(SIRFSOC_USBPHY_PLL_POWERDOWN | SIRFSOC_USBPHY_PLL_BYPASS);
+	writel(reg, sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL);
+	while (!(readl(sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL) &
+			SIRFSOC_USBPHY_PLL_LOCK))
+		cpu_relax();
+
+	return 0;
+}
+
+static void usb_pll_clk_disable(struct clk_hw *clk)
+{
+	u32 reg = readl(sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL);
+	reg |= (SIRFSOC_USBPHY_PLL_POWERDOWN | SIRFSOC_USBPHY_PLL_BYPASS);
+	writel(reg, sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL);
+}
+
+static unsigned long usb_pll_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	u32 reg = readl(sirfsoc_rsc_vbase + SIRFSOC_USBPHY_PLL_CTRL);
+	return (reg & SIRFSOC_USBPHY_PLL_BYPASS) ? parent_rate : 48*MHZ;
+}
+
+static struct clk_ops usb_pll_ops = {
+	.enable = usb_pll_clk_enable,
+	.disable = usb_pll_clk_disable,
+	.recalc_rate = usb_pll_clk_recalc_rate,
+};
+
+static struct clk_init_data clk_usb_pll_init = {
+	.name = "usb_pll",
+	.ops = &usb_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_hw usb_pll_clk_hw = {
+	.init = &clk_usb_pll_init,
+};
+
+/*
+ * clock domains - cpu, mem, sys/io, dsp, gfx
+ */
+
+static const char *dmn_clk_parents[] = {
+	"rtc",
+	"osc",
+	"pll1",
+	"pll2",
+	"pll3",
+};
+
+static u8 dmn_clk_get_parent(struct clk_hw *hw)
+{
+	struct clk_dmn *clk = to_dmnclk(hw);
+	u32 cfg = clkc_readl(clk->regofs);
+
+	/* parent of io domain can only be pll3 */
+	if (strcmp(hw->init->name, "io") == 0)
+		return 4;
+
+	WARN_ON((cfg & (BIT(3) - 1)) > 4);
+
+	return cfg & (BIT(3) - 1);
+}
+
+static int dmn_clk_set_parent(struct clk_hw *hw, u8 parent)
+{
+	struct clk_dmn *clk = to_dmnclk(hw);
+	u32 cfg = clkc_readl(clk->regofs);
+
+	/* parent of io domain can only be pll3 */
+	if (strcmp(hw->init->name, "io") == 0)
+		return -EINVAL;
+
+	cfg &= ~(BIT(3) - 1);
+	clkc_writel(cfg | parent, clk->regofs);
+	/* BIT(3) - switching status: 1 - busy, 0 - done */
+	while (clkc_readl(clk->regofs) & BIT(3))
+		cpu_relax();
+
+	return 0;
+}
+
+static unsigned long dmn_clk_recalc_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+
+{
+	unsigned long fin = parent_rate;
+	struct clk_dmn *clk = to_dmnclk(hw);
+
+	u32 cfg = clkc_readl(clk->regofs);
+
+	if (cfg & BIT(24)) {
+		/* fcd bypass mode */
+		return fin;
+	} else {
+		/*
+		 * wait count: bit[19:16], hold count: bit[23:20]
+		 */
+		u32 wait = (cfg >> 16) & (BIT(4) - 1);
+		u32 hold = (cfg >> 20) & (BIT(4) - 1);
+
+		return fin / (wait + hold + 2);
+	}
+}
+
+static long dmn_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long *parent_rate)
+{
+	unsigned long fin;
+	unsigned ratio, wait, hold;
+	unsigned bits = (strcmp(hw->init->name, "mem") == 0) ? 3 : 4;
+
+	fin = *parent_rate;
+	ratio = fin / rate;
+
+	if (ratio < 2)
+		ratio = 2;
+	if (ratio > BIT(bits + 1))
+		ratio = BIT(bits + 1);
+
+	wait = (ratio >> 1) - 1;
+	hold = ratio - wait - 2;
+
+	return fin / (wait + hold + 2);
+}
+
+static int dmn_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long parent_rate)
+{
+	struct clk_dmn *clk = to_dmnclk(hw);
+	unsigned long fin;
+	unsigned ratio, wait, hold, reg;
+	unsigned bits = (strcmp(hw->init->name, "mem") == 0) ? 3 : 4;
+
+	fin = parent_rate;
+	ratio = fin / rate;
+
+	if (unlikely(ratio < 2 || ratio > BIT(bits + 1)))
+		return -EINVAL;
+
+	WARN_ON(fin % rate);
+
+	wait = (ratio >> 1) - 1;
+	hold = ratio - wait - 2;
+
+	reg = clkc_readl(clk->regofs);
+	reg &= ~(((BIT(bits) - 1) << 16) | ((BIT(bits) - 1) << 20));
+	reg |= (wait << 16) | (hold << 20) | BIT(25);
+	clkc_writel(reg, clk->regofs);
+
+	/* waiting FCD been effective */
+	while (clkc_readl(clk->regofs) & BIT(25))
+		cpu_relax();
+
+	return 0;
+}
+
+static struct clk_ops msi_ops = {
+	.set_rate = dmn_clk_set_rate,
+	.round_rate = dmn_clk_round_rate,
+	.recalc_rate = dmn_clk_recalc_rate,
+	.set_parent = dmn_clk_set_parent,
+	.get_parent = dmn_clk_get_parent,
+};
+
+static struct clk_init_data clk_mem_init = {
+	.name = "mem",
+	.ops = &msi_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_mem = {
+	.regofs = SIRFSOC_CLKC_MEM_CFG,
+	.hw = {
+		.init = &clk_mem_init,
+	},
+};
+
+static struct clk_init_data clk_sys_init = {
+	.name = "sys",
+	.ops = &msi_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+	.flags = CLK_SET_RATE_GATE,
+};
+
+static struct clk_dmn clk_sys = {
+	.regofs = SIRFSOC_CLKC_SYS_CFG,
+	.hw = {
+		.init = &clk_sys_init,
+	},
+};
+
+static struct clk_init_data clk_io_init = {
+	.name = "io",
+	.ops = &msi_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_io = {
+	.regofs = SIRFSOC_CLKC_IO_CFG,
+	.hw = {
+		.init = &clk_io_init,
+	},
+};
+
+static struct clk_ops cpu_ops = {
+	.set_parent = dmn_clk_set_parent,
+	.get_parent = dmn_clk_get_parent,
+};
+
+static struct clk_init_data clk_cpu_init = {
+	.name = "cpu",
+	.ops = &cpu_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+	.flags = CLK_SET_RATE_PARENT,
+};
+
+static struct clk_dmn clk_cpu = {
+	.regofs = SIRFSOC_CLKC_CPU_CFG,
+	.hw = {
+		.init = &clk_cpu_init,
+	},
+};
+
+static struct clk_ops dmn_ops = {
+	.is_enabled = std_clk_is_enabled,
+	.enable = std_clk_enable,
+	.disable = std_clk_disable,
+	.set_rate = dmn_clk_set_rate,
+	.round_rate = dmn_clk_round_rate,
+	.recalc_rate = dmn_clk_recalc_rate,
+	.set_parent = dmn_clk_set_parent,
+	.get_parent = dmn_clk_get_parent,
+};
+
+/* dsp, gfx, mm, lcd and vpp domain */
+
+static struct clk_init_data clk_dsp_init = {
+	.name = "dsp",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_dsp = {
+	.regofs = SIRFSOC_CLKC_DSP_CFG,
+	.enable_bit = 0,
+	.hw = {
+		.init = &clk_dsp_init,
+	},
+};
+
+static struct clk_init_data clk_gfx_init = {
+	.name = "gfx",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_gfx = {
+	.regofs = SIRFSOC_CLKC_GFX_CFG,
+	.enable_bit = 8,
+	.hw = {
+		.init = &clk_gfx_init,
+	},
+};
+
+static struct clk_init_data clk_mm_init = {
+	.name = "mm",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_mm = {
+	.regofs = SIRFSOC_CLKC_MM_CFG,
+	.enable_bit = 9,
+	.hw = {
+		.init = &clk_mm_init,
+	},
+};
+
+static struct clk_init_data clk_lcd_init = {
+	.name = "lcd",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_lcd = {
+	.regofs = SIRFSOC_CLKC_LCD_CFG,
+	.enable_bit = 10,
+	.hw = {
+		.init = &clk_lcd_init,
+	},
+};
+
+static struct clk_init_data clk_vpp_init = {
+	.name = "vpp",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_vpp = {
+	.regofs = SIRFSOC_CLKC_LCD_CFG,
+	.enable_bit = 11,
+	.hw = {
+		.init = &clk_vpp_init,
+	},
+};
+
+static struct clk_init_data clk_mmc01_init = {
+	.name = "mmc01",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_mmc01 = {
+	.regofs = SIRFSOC_CLKC_MMC_CFG,
+	.enable_bit = 59,
+	.hw = {
+		.init = &clk_mmc01_init,
+	},
+};
+
+static struct clk_init_data clk_mmc23_init = {
+	.name = "mmc23",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_mmc23 = {
+	.regofs = SIRFSOC_CLKC_MMC_CFG,
+	.enable_bit = 60,
+	.hw = {
+		.init = &clk_mmc23_init,
+	},
+};
+
+static struct clk_init_data clk_mmc45_init = {
+	.name = "mmc45",
+	.ops = &dmn_ops,
+	.parent_names = dmn_clk_parents,
+	.num_parents = ARRAY_SIZE(dmn_clk_parents),
+};
+
+static struct clk_dmn clk_mmc45 = {
+	.regofs = SIRFSOC_CLKC_MMC_CFG,
+	.enable_bit = 61,
+	.hw = {
+		.init = &clk_mmc45_init,
+	},
+};
+
+/*
+ * peripheral controllers in io domain
+ */
+
+static int std_clk_is_enabled(struct clk_hw *hw)
+{
+	u32 reg;
+	int bit;
+	struct clk_std *clk = to_stdclk(hw);
+
+	bit = clk->enable_bit % 32;
+	reg = clk->enable_bit / 32;
+	reg = SIRFSOC_CLKC_CLK_EN0 + reg * sizeof(reg);
+
+	return !!(clkc_readl(reg) & BIT(bit));
+}
+
+static int std_clk_enable(struct clk_hw *hw)
+{
+	u32 val, reg;
+	int bit;
+	struct clk_std *clk = to_stdclk(hw);
+
+	BUG_ON(clk->enable_bit < 0 || clk->enable_bit > 63);
+
+	bit = clk->enable_bit % 32;
+	reg = clk->enable_bit / 32;
+	reg = SIRFSOC_CLKC_CLK_EN0 + reg * sizeof(reg);
+
+	val = clkc_readl(reg) | BIT(bit);
+	clkc_writel(val, reg);
+	return 0;
+}
+
+static void std_clk_disable(struct clk_hw *hw)
+{
+	u32 val, reg;
+	int bit;
+	struct clk_std *clk = to_stdclk(hw);
+
+	BUG_ON(clk->enable_bit < 0 || clk->enable_bit > 63);
+
+	bit = clk->enable_bit % 32;
+	reg = clk->enable_bit / 32;
+	reg = SIRFSOC_CLKC_CLK_EN0 + reg * sizeof(reg);
+
+	val = clkc_readl(reg) & ~BIT(bit);
+	clkc_writel(val, reg);
+}
+
+static const char *std_clk_io_parents[] = {
+	"io",
+};
+
+static struct clk_ops ios_ops = {
+	.is_enabled = std_clk_is_enabled,
+	.enable = std_clk_enable,
+	.disable = std_clk_disable,
+};
+
+static struct clk_init_data clk_dmac0_init = {
+	.name = "dmac0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_dmac0 = {
+	.enable_bit = 32,
+	.hw = {
+		.init = &clk_dmac0_init,
+	},
+};
+
+static struct clk_init_data clk_dmac1_init = {
+	.name = "dmac1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_dmac1 = {
+	.enable_bit = 33,
+	.hw = {
+		.init = &clk_dmac1_init,
+	},
+};
+
+static struct clk_init_data clk_nand_init = {
+	.name = "nand",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_nand = {
+	.enable_bit = 34,
+	.hw = {
+		.init = &clk_nand_init,
+	},
+};
+
+static struct clk_init_data clk_audio_init = {
+	.name = "audio",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_audio = {
+	.enable_bit = 35,
+	.hw = {
+		.init = &clk_audio_init,
+	},
+};
+
+static struct clk_init_data clk_uart0_init = {
+	.name = "uart0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_uart0 = {
+	.enable_bit = 36,
+	.hw = {
+		.init = &clk_uart0_init,
+	},
+};
+
+static struct clk_init_data clk_uart1_init = {
+	.name = "uart1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_uart1 = {
+	.enable_bit = 37,
+	.hw = {
+		.init = &clk_uart1_init,
+	},
+};
+
+static struct clk_init_data clk_uart2_init = {
+	.name = "uart2",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_uart2 = {
+	.enable_bit = 38,
+	.hw = {
+		.init = &clk_uart2_init,
+	},
+};
+
+static struct clk_init_data clk_usp0_init = {
+	.name = "usp0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_usp0 = {
+	.enable_bit = 39,
+	.hw = {
+		.init = &clk_usp0_init,
+	},
+};
+
+static struct clk_init_data clk_usp1_init = {
+	.name = "usp1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_usp1 = {
+	.enable_bit = 40,
+	.hw = {
+		.init = &clk_usp1_init,
+	},
+};
+
+static struct clk_init_data clk_usp2_init = {
+	.name = "usp2",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_usp2 = {
+	.enable_bit = 41,
+	.hw = {
+		.init = &clk_usp2_init,
+	},
+};
+
+static struct clk_init_data clk_vip_init = {
+	.name = "vip",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_vip = {
+	.enable_bit = 42,
+	.hw = {
+		.init = &clk_vip_init,
+	},
+};
+
+static struct clk_init_data clk_spi0_init = {
+	.name = "spi0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_spi0 = {
+	.enable_bit = 43,
+	.hw = {
+		.init = &clk_spi0_init,
+	},
+};
+
+static struct clk_init_data clk_spi1_init = {
+	.name = "spi1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_spi1 = {
+	.enable_bit = 44,
+	.hw = {
+		.init = &clk_spi1_init,
+	},
+};
+
+static struct clk_init_data clk_tsc_init = {
+	.name = "tsc",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_tsc = {
+	.enable_bit = 45,
+	.hw = {
+		.init = &clk_tsc_init,
+	},
+};
+
+static struct clk_init_data clk_i2c0_init = {
+	.name = "i2c0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_i2c0 = {
+	.enable_bit = 46,
+	.hw = {
+		.init = &clk_i2c0_init,
+	},
+};
+
+static struct clk_init_data clk_i2c1_init = {
+	.name = "i2c1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_i2c1 = {
+	.enable_bit = 47,
+	.hw = {
+		.init = &clk_i2c1_init,
+	},
+};
+
+static struct clk_init_data clk_pwmc_init = {
+	.name = "pwmc",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_pwmc = {
+	.enable_bit = 48,
+	.hw = {
+		.init = &clk_pwmc_init,
+	},
+};
+
+static struct clk_init_data clk_efuse_init = {
+	.name = "efuse",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_efuse = {
+	.enable_bit = 49,
+	.hw = {
+		.init = &clk_efuse_init,
+	},
+};
+
+static struct clk_init_data clk_pulse_init = {
+	.name = "pulse",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_pulse = {
+	.enable_bit = 50,
+	.hw = {
+		.init = &clk_pulse_init,
+	},
+};
+
+static const char *std_clk_dsp_parents[] = {
+	"dsp",
+};
+
+static struct clk_init_data clk_gps_init = {
+	.name = "gps",
+	.ops = &ios_ops,
+	.parent_names = std_clk_dsp_parents,
+	.num_parents = ARRAY_SIZE(std_clk_dsp_parents),
+};
+
+static struct clk_std clk_gps = {
+	.enable_bit = 1,
+	.hw = {
+		.init = &clk_gps_init,
+	},
+};
+
+static struct clk_init_data clk_mf_init = {
+	.name = "mf",
+	.ops = &ios_ops,
+	.parent_names = std_clk_io_parents,
+	.num_parents = ARRAY_SIZE(std_clk_io_parents),
+};
+
+static struct clk_std clk_mf = {
+	.enable_bit = 2,
+	.hw = {
+		.init = &clk_mf_init,
+	},
+};
+
+static const char *std_clk_sys_parents[] = {
+	"sys",
+};
+
+static struct clk_init_data clk_security_init = {
+	.name = "mf",
+	.ops = &ios_ops,
+	.parent_names = std_clk_sys_parents,
+	.num_parents = ARRAY_SIZE(std_clk_sys_parents),
+};
+
+static struct clk_std clk_security = {
+	.enable_bit = 19,
+	.hw = {
+		.init = &clk_security_init,
+	},
+};
+
+static const char *std_clk_usb_parents[] = {
+	"usb_pll",
+};
+
+static struct clk_init_data clk_usb0_init = {
+	.name = "usb0",
+	.ops = &ios_ops,
+	.parent_names = std_clk_usb_parents,
+	.num_parents = ARRAY_SIZE(std_clk_usb_parents),
+};
+
+static struct clk_std clk_usb0 = {
+	.enable_bit = 16,
+	.hw = {
+		.init = &clk_usb0_init,
+	},
+};
+
+static struct clk_init_data clk_usb1_init = {
+	.name = "usb1",
+	.ops = &ios_ops,
+	.parent_names = std_clk_usb_parents,
+	.num_parents = ARRAY_SIZE(std_clk_usb_parents),
+};
+
+static struct clk_std clk_usb1 = {
+	.enable_bit = 17,
+	.hw = {
+		.init = &clk_usb1_init,
+	},
+};
+
+static struct of_device_id clkc_ids[] = {
+	{ .compatible = "sirf,prima2-clkc" },
+	{},
+};
+
+static struct of_device_id rsc_ids[] = {
+	{ .compatible = "sirf,prima2-rsc" },
+	{},
+};
+
+void __init sirfsoc_of_clk_init(void)
+{
+	struct clk *clk;
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, clkc_ids);
+	if (!np)
+		panic("unable to find compatible clkc node in dtb\n");
+
+	sirfsoc_clk_vbase = of_iomap(np, 0);
+	if (!sirfsoc_clk_vbase)
+		panic("unable to map clkc registers\n");
+
+	of_node_put(np);
+
+	np = of_find_matching_node(NULL, rsc_ids);
+	if (!np)
+		panic("unable to find compatible rsc node in dtb\n");
+
+	sirfsoc_rsc_vbase = of_iomap(np, 0);
+	if (!sirfsoc_rsc_vbase)
+		panic("unable to map rsc registers\n");
+
+	of_node_put(np);
+
+
+	/* These are always available (RTC and 26MHz OSC)*/
+	clk = clk_register_fixed_rate(NULL, "rtc", NULL,
+		CLK_IS_ROOT, 32768);
+	BUG_ON(!clk);
+	clk = clk_register_fixed_rate(NULL, "osc", NULL,
+		CLK_IS_ROOT, 26000000);
+	BUG_ON(!clk);
+
+	clk = clk_register(NULL, &clk_pll1.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_pll2.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_pll3.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_mem.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_sys.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_security.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b8030000.security");
+	clk = clk_register(NULL, &clk_dsp.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_gps.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "a8010000.gps");
+	clk = clk_register(NULL, &clk_mf.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_io.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "io");
+	clk = clk_register(NULL, &clk_cpu.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "cpu");
+	clk = clk_register(NULL, &clk_uart0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0050000.uart");
+	clk = clk_register(NULL, &clk_uart1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0060000.uart");
+	clk = clk_register(NULL, &clk_uart2.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0070000.uart");
+	clk = clk_register(NULL, &clk_tsc.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0110000.tsc");
+	clk = clk_register(NULL, &clk_i2c0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00e0000.i2c");
+	clk = clk_register(NULL, &clk_i2c1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00f0000.i2c");
+	clk = clk_register(NULL, &clk_spi0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00d0000.spi");
+	clk = clk_register(NULL, &clk_spi1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0170000.spi");
+	clk = clk_register(NULL, &clk_pwmc.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0130000.pwm");
+	clk = clk_register(NULL, &clk_efuse.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0140000.efusesys");
+	clk = clk_register(NULL, &clk_pulse.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0150000.pulsec");
+	clk = clk_register(NULL, &clk_dmac0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00b0000.dma-controller");
+	clk = clk_register(NULL, &clk_dmac1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0160000.dma-controller");
+	clk = clk_register(NULL, &clk_nand.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0030000.nand");
+	clk = clk_register(NULL, &clk_audio.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0040000.audio");
+	clk = clk_register(NULL, &clk_usp0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0080000.usp");
+	clk = clk_register(NULL, &clk_usp1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b0090000.usp");
+	clk = clk_register(NULL, &clk_usp2.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00a0000.usp");
+	clk = clk_register(NULL, &clk_vip.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00c0000.vip");
+	clk = clk_register(NULL, &clk_gfx.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "98000000.graphics");
+	clk = clk_register(NULL, &clk_mm.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "a0000000.multimedia");
+	clk = clk_register(NULL, &clk_lcd.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "90010000.display");
+	clk = clk_register(NULL, &clk_vpp.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "90020000.vpp");
+	clk = clk_register(NULL, &clk_mmc01.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_mmc23.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_mmc45.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &usb_pll_clk_hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_usb0.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00e0000.usb");
+	clk = clk_register(NULL, &clk_usb1.hw);
+	BUG_ON(!clk);
+	clk_register_clkdev(clk, NULL, "b00f0000.usb");
+}
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index efdfd009c270..56e4495ebeb1 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -558,25 +558,6 @@ int clk_enable(struct clk *clk)
 EXPORT_SYMBOL_GPL(clk_enable);
 
 /**
- * clk_get_rate - return the rate of clk
- * @clk: the clk whose rate is being returned
- *
- * Simply returns the cached rate of the clk.  Does not query the hardware.  If
- * clk is NULL then returns 0.
- */
-unsigned long clk_get_rate(struct clk *clk)
-{
-	unsigned long rate;
-
-	mutex_lock(&prepare_lock);
-	rate = __clk_get_rate(clk);
-	mutex_unlock(&prepare_lock);
-
-	return rate;
-}
-EXPORT_SYMBOL_GPL(clk_get_rate);
-
-/**
  * __clk_round_rate - round the given rate for a clk
  * @clk: round the rate of this clock
  *
@@ -702,6 +683,30 @@ static void __clk_recalc_rates(struct clk *clk, unsigned long msg)
 }
 
 /**
+ * clk_get_rate - return the rate of clk
+ * @clk: the clk whose rate is being returned
+ *
+ * Simply returns the cached rate of the clk, unless CLK_GET_RATE_NOCACHE flag
+ * is set, which means a recalc_rate will be issued.
+ * If clk is NULL then returns 0.
+ */
+unsigned long clk_get_rate(struct clk *clk)
+{
+	unsigned long rate;
+
+	mutex_lock(&prepare_lock);
+
+	if (clk && (clk->flags & CLK_GET_RATE_NOCACHE))
+		__clk_recalc_rates(clk, 0);
+
+	rate = __clk_get_rate(clk);
+	mutex_unlock(&prepare_lock);
+
+	return rate;
+}
+EXPORT_SYMBOL_GPL(clk_get_rate);
+
+/**
  * __clk_speculate_rates
  * @clk: first clk in the subtree
  * @parent_rate: the "future" rate of clk's parent
@@ -1582,6 +1587,20 @@ struct clk *of_clk_src_simple_get(struct of_phandle_args *clkspec,
 }
 EXPORT_SYMBOL_GPL(of_clk_src_simple_get);
 
+struct clk *of_clk_src_onecell_get(struct of_phandle_args *clkspec, void *data)
+{
+	struct clk_onecell_data *clk_data = data;
+	unsigned int idx = clkspec->args[0];
+
+	if (idx >= clk_data->clk_num) {
+		pr_err("%s: invalid clock index %d\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return clk_data->clks[idx];
+}
+EXPORT_SYMBOL_GPL(of_clk_src_onecell_get);
+
 /**
  * of_clk_add_provider() - Register a clock provider for a node
  * @np: Device node pointer associated with clock provider
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index d423c9bdd71a..442a31363873 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -171,51 +171,6 @@ void clk_put(struct clk *clk)
 }
 EXPORT_SYMBOL(clk_put);
 
-static void devm_clk_release(struct device *dev, void *res)
-{
-	clk_put(*(struct clk **)res);
-}
-
-struct clk *devm_clk_get(struct device *dev, const char *id)
-{
-	struct clk **ptr, *clk;
-
-	ptr = devres_alloc(devm_clk_release, sizeof(*ptr), GFP_KERNEL);
-	if (!ptr)
-		return ERR_PTR(-ENOMEM);
-
-	clk = clk_get(dev, id);
-	if (!IS_ERR(clk)) {
-		*ptr = clk;
-		devres_add(dev, ptr);
-	} else {
-		devres_free(ptr);
-	}
-
-	return clk;
-}
-EXPORT_SYMBOL(devm_clk_get);
-
-static int devm_clk_match(struct device *dev, void *res, void *data)
-{
-	struct clk **c = res;
-	if (!c || !*c) {
-		WARN_ON(!c || !*c);
-		return 0;
-	}
-	return *c == data;
-}
-
-void devm_clk_put(struct device *dev, struct clk *clk)
-{
-	int ret;
-
-	ret = devres_destroy(dev, devm_clk_release, devm_clk_match, clk);
-
-	WARN_ON(ret);
-}
-EXPORT_SYMBOL(devm_clk_put);
-
 void clkdev_add(struct clk_lookup *cl)
 {
 	mutex_lock(&clocks_mutex);
diff --git a/drivers/clk/mmp/Makefile b/drivers/clk/mmp/Makefile
new file mode 100644
index 000000000000..392d78044ce3
--- /dev/null
+++ b/drivers/clk/mmp/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for mmp specific clk
+#
+
+obj-y += clk-apbc.o clk-apmu.o clk-frac.o
+
+obj-$(CONFIG_CPU_PXA168) += clk-pxa168.o
+obj-$(CONFIG_CPU_PXA910) += clk-pxa910.o
+obj-$(CONFIG_CPU_MMP2) += clk-mmp2.o
diff --git a/drivers/clk/mmp/clk-apbc.c b/drivers/clk/mmp/clk-apbc.c
new file mode 100644
index 000000000000..d14120eaa71f
--- /dev/null
+++ b/drivers/clk/mmp/clk-apbc.c
@@ -0,0 +1,152 @@
+/*
+ * mmp APB clock operation source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include "clk.h"
+
+/* Common APB clock register bit definitions */
+#define APBC_APBCLK	(1 << 0)  /* APB Bus Clock Enable */
+#define APBC_FNCLK	(1 << 1)  /* Functional Clock Enable */
+#define APBC_RST	(1 << 2)  /* Reset Generation */
+#define APBC_POWER	(1 << 7)  /* Reset Generation */
+
+#define to_clk_apbc(hw) container_of(hw, struct clk_apbc, hw)
+struct clk_apbc {
+	struct clk_hw		hw;
+	void __iomem		*base;
+	unsigned int		delay;
+	unsigned int		flags;
+	spinlock_t		*lock;
+};
+
+static int clk_apbc_prepare(struct clk_hw *hw)
+{
+	struct clk_apbc *apbc = to_clk_apbc(hw);
+	unsigned int data;
+	unsigned long flags = 0;
+
+	/*
+	 * It may share same register as MUX clock,
+	 * and it will impact FNCLK enable. Spinlock is needed
+	 */
+	if (apbc->lock)
+		spin_lock_irqsave(apbc->lock, flags);
+
+	data = readl_relaxed(apbc->base);
+	if (apbc->flags & APBC_POWER_CTRL)
+		data |= APBC_POWER;
+	data |= APBC_FNCLK;
+	writel_relaxed(data, apbc->base);
+
+	if (apbc->lock)
+		spin_unlock_irqrestore(apbc->lock, flags);
+
+	udelay(apbc->delay);
+
+	if (apbc->lock)
+		spin_lock_irqsave(apbc->lock, flags);
+
+	data = readl_relaxed(apbc->base);
+	data |= APBC_APBCLK;
+	writel_relaxed(data, apbc->base);
+
+	if (apbc->lock)
+		spin_unlock_irqrestore(apbc->lock, flags);
+
+	udelay(apbc->delay);
+
+	if (!(apbc->flags & APBC_NO_BUS_CTRL)) {
+		if (apbc->lock)
+			spin_lock_irqsave(apbc->lock, flags);
+
+		data = readl_relaxed(apbc->base);
+		data &= ~APBC_RST;
+		writel_relaxed(data, apbc->base);
+
+		if (apbc->lock)
+			spin_unlock_irqrestore(apbc->lock, flags);
+	}
+
+	return 0;
+}
+
+static void clk_apbc_unprepare(struct clk_hw *hw)
+{
+	struct clk_apbc *apbc = to_clk_apbc(hw);
+	unsigned long data;
+	unsigned long flags = 0;
+
+	if (apbc->lock)
+		spin_lock_irqsave(apbc->lock, flags);
+
+	data = readl_relaxed(apbc->base);
+	if (apbc->flags & APBC_POWER_CTRL)
+		data &= ~APBC_POWER;
+	data &= ~APBC_FNCLK;
+	writel_relaxed(data, apbc->base);
+
+	if (apbc->lock)
+		spin_unlock_irqrestore(apbc->lock, flags);
+
+	udelay(10);
+
+	if (apbc->lock)
+		spin_lock_irqsave(apbc->lock, flags);
+
+	data = readl_relaxed(apbc->base);
+	data &= ~APBC_APBCLK;
+	writel_relaxed(data, apbc->base);
+
+	if (apbc->lock)
+		spin_unlock_irqrestore(apbc->lock, flags);
+}
+
+struct clk_ops clk_apbc_ops = {
+	.prepare = clk_apbc_prepare,
+	.unprepare = clk_apbc_unprepare,
+};
+
+struct clk *mmp_clk_register_apbc(const char *name, const char *parent_name,
+		void __iomem *base, unsigned int delay,
+		unsigned int apbc_flags, spinlock_t *lock)
+{
+	struct clk_apbc *apbc;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	apbc = kzalloc(sizeof(*apbc), GFP_KERNEL);
+	if (!apbc)
+		return NULL;
+
+	init.name = name;
+	init.ops = &clk_apbc_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	apbc->base = base;
+	apbc->delay = delay;
+	apbc->flags = apbc_flags;
+	apbc->lock = lock;
+	apbc->hw.init = &init;
+
+	clk = clk_register(NULL, &apbc->hw);
+	if (IS_ERR(clk))
+		kfree(apbc);
+
+	return clk;
+}
diff --git a/drivers/clk/mmp/clk-apmu.c b/drivers/clk/mmp/clk-apmu.c
new file mode 100644
index 000000000000..abe182b2377f
--- /dev/null
+++ b/drivers/clk/mmp/clk-apmu.c
@@ -0,0 +1,97 @@
+/*
+ * mmp AXI peripharal clock operation source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include "clk.h"
+
+#define to_clk_apmu(clk) (container_of(clk, struct clk_apmu, clk))
+struct clk_apmu {
+	struct clk_hw   hw;
+	void __iomem    *base;
+	u32		rst_mask;
+	u32		enable_mask;
+	spinlock_t	*lock;
+};
+
+static int clk_apmu_enable(struct clk_hw *hw)
+{
+	struct clk_apmu *apmu = to_clk_apmu(hw);
+	unsigned long data;
+	unsigned long flags = 0;
+
+	if (apmu->lock)
+		spin_lock_irqsave(apmu->lock, flags);
+
+	data = readl_relaxed(apmu->base) | apmu->enable_mask;
+	writel_relaxed(data, apmu->base);
+
+	if (apmu->lock)
+		spin_unlock_irqrestore(apmu->lock, flags);
+
+	return 0;
+}
+
+static void clk_apmu_disable(struct clk_hw *hw)
+{
+	struct clk_apmu *apmu = to_clk_apmu(hw);
+	unsigned long data;
+	unsigned long flags = 0;
+
+	if (apmu->lock)
+		spin_lock_irqsave(apmu->lock, flags);
+
+	data = readl_relaxed(apmu->base) & ~apmu->enable_mask;
+	writel_relaxed(data, apmu->base);
+
+	if (apmu->lock)
+		spin_unlock_irqrestore(apmu->lock, flags);
+}
+
+struct clk_ops clk_apmu_ops = {
+	.enable = clk_apmu_enable,
+	.disable = clk_apmu_disable,
+};
+
+struct clk *mmp_clk_register_apmu(const char *name, const char *parent_name,
+		void __iomem *base, u32 enable_mask, spinlock_t *lock)
+{
+	struct clk_apmu *apmu;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	apmu = kzalloc(sizeof(*apmu), GFP_KERNEL);
+	if (!apmu)
+		return NULL;
+
+	init.name = name;
+	init.ops = &clk_apmu_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	apmu->base = base;
+	apmu->enable_mask = enable_mask;
+	apmu->lock = lock;
+	apmu->hw.init = &init;
+
+	clk = clk_register(NULL, &apmu->hw);
+
+	if (IS_ERR(clk))
+		kfree(apmu);
+
+	return clk;
+}
diff --git a/drivers/clk/mmp/clk-frac.c b/drivers/clk/mmp/clk-frac.c
new file mode 100644
index 000000000000..80c1dd15d15c
--- /dev/null
+++ b/drivers/clk/mmp/clk-frac.c
@@ -0,0 +1,153 @@
+/*
+ * mmp factor clock operation source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#include "clk.h"
+/*
+ * It is M/N clock
+ *
+ * Fout from synthesizer can be given from two equations:
+ * numerator/denominator = Fin / (Fout * factor)
+ */
+
+#define to_clk_factor(hw) container_of(hw, struct clk_factor, hw)
+struct clk_factor {
+	struct clk_hw		hw;
+	void __iomem		*base;
+	struct clk_factor_masks	*masks;
+	struct clk_factor_tbl	*ftbl;
+	unsigned int		ftbl_cnt;
+};
+
+static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
+		unsigned long *prate)
+{
+	struct clk_factor *factor = to_clk_factor(hw);
+	unsigned long rate = 0, prev_rate;
+	int i;
+
+	for (i = 0; i < factor->ftbl_cnt; i++) {
+		prev_rate = rate;
+		rate = (((*prate / 10000) * factor->ftbl[i].num) /
+			(factor->ftbl[i].den * factor->masks->factor)) * 10000;
+		if (rate > drate)
+			break;
+	}
+	if (i == 0)
+		return rate;
+	else
+		return prev_rate;
+}
+
+static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_factor *factor = to_clk_factor(hw);
+	struct clk_factor_masks *masks = factor->masks;
+	unsigned int val, num, den;
+
+	val = readl_relaxed(factor->base);
+
+	/* calculate numerator */
+	num = (val >> masks->num_shift) & masks->num_mask;
+
+	/* calculate denominator */
+	den = (val >> masks->den_shift) & masks->num_mask;
+
+	if (!den)
+		return 0;
+
+	return (((parent_rate / 10000)  * den) /
+			(num * factor->masks->factor)) * 10000;
+}
+
+/* Configures new clock rate*/
+static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_factor *factor = to_clk_factor(hw);
+	struct clk_factor_masks *masks = factor->masks;
+	int i;
+	unsigned long val;
+	unsigned long prev_rate, rate = 0;
+
+	for (i = 0; i < factor->ftbl_cnt; i++) {
+		prev_rate = rate;
+		rate = (((prate / 10000) * factor->ftbl[i].num) /
+			(factor->ftbl[i].den * factor->masks->factor)) * 10000;
+		if (rate > drate)
+			break;
+	}
+	if (i > 0)
+		i--;
+
+	val = readl_relaxed(factor->base);
+
+	val &= ~(masks->num_mask << masks->num_shift);
+	val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift;
+
+	val &= ~(masks->den_mask << masks->den_shift);
+	val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift;
+
+	writel_relaxed(val, factor->base);
+
+	return 0;
+}
+
+static struct clk_ops clk_factor_ops = {
+	.recalc_rate = clk_factor_recalc_rate,
+	.round_rate = clk_factor_round_rate,
+	.set_rate = clk_factor_set_rate,
+};
+
+struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
+		unsigned long flags, void __iomem *base,
+		struct clk_factor_masks *masks, struct clk_factor_tbl *ftbl,
+		unsigned int ftbl_cnt)
+{
+	struct clk_factor *factor;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	if (!masks) {
+		pr_err("%s: must pass a clk_factor_mask\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	factor = kzalloc(sizeof(*factor), GFP_KERNEL);
+	if (!factor) {
+		pr_err("%s: could not allocate factor  clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* struct clk_aux assignments */
+	factor->base = base;
+	factor->masks = masks;
+	factor->ftbl = ftbl;
+	factor->ftbl_cnt = ftbl_cnt;
+	factor->hw.init = &init;
+
+	init.name = name;
+	init.ops = &clk_factor_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(NULL, &factor->hw);
+	if (IS_ERR_OR_NULL(clk))
+		kfree(factor);
+
+	return clk;
+}
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
new file mode 100644
index 000000000000..ade435820c7e
--- /dev/null
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -0,0 +1,449 @@
+/*
+ * mmp2 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#include <mach/addr-map.h>
+
+#include "clk.h"
+
+#define APBC_RTC	0x0
+#define APBC_TWSI0	0x4
+#define APBC_TWSI1	0x8
+#define APBC_TWSI2	0xc
+#define APBC_TWSI3	0x10
+#define APBC_TWSI4	0x7c
+#define APBC_TWSI5	0x80
+#define APBC_KPC	0x18
+#define APBC_UART0	0x2c
+#define APBC_UART1	0x30
+#define APBC_UART2	0x34
+#define APBC_UART3	0x88
+#define APBC_GPIO	0x38
+#define APBC_PWM0	0x3c
+#define APBC_PWM1	0x40
+#define APBC_PWM2	0x44
+#define APBC_PWM3	0x48
+#define APBC_SSP0	0x50
+#define APBC_SSP1	0x54
+#define APBC_SSP2	0x58
+#define APBC_SSP3	0x5c
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_SDH2	0xe8
+#define APMU_SDH3	0xec
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_DISP1	0x110
+#define APMU_CCIC0	0x50
+#define APMU_CCIC1	0xf4
+#define MPMU_UART_PLL	0x14
+
+static DEFINE_SPINLOCK(clk_lock);
+
+static struct clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 14634, .den = 2165},	/*14.745MHZ */
+	{.num = 3521, .den = 689},	/*19.23MHZ */
+	{.num = 9679, .den = 5728},	/*58.9824MHZ */
+	{.num = 15850, .den = 9451},	/*59.429MHZ */
+};
+
+static const char *uart_parent[] = {"uart_pll", "vctcxo"};
+static const char *ssp_parent[] = {"vctcxo_4", "vctcxo_2", "vctcxo", "pll1_16"};
+static const char *sdh_parent[] = {"pll1_4", "pll2", "usb_pll", "pll1"};
+static const char *disp_parent[] = {"pll1", "pll1_16", "pll2", "vctcxo"};
+static const char *ccic_parent[] = {"pll1_2", "pll1_16", "vctcxo"};
+
+void __init mmp2_clk_init(void)
+{
+	struct clk *clk;
+	struct clk *vctcxo;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbc_base;
+
+	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	if (mpmu_base == NULL) {
+		pr_err("error to ioremap MPMU base\n");
+		return;
+	}
+
+	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	if (apmu_base == NULL) {
+		pr_err("error to ioremap APMU base\n");
+		return;
+	}
+
+	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	if (apbc_base == NULL) {
+		pr_err("error to ioremap APBC base\n");
+		return;
+	}
+
+	clk = clk_register_fixed_rate(NULL, "clk32", NULL, CLK_IS_ROOT, 3200);
+	clk_register_clkdev(clk, "clk32", NULL);
+
+	vctcxo = clk_register_fixed_rate(NULL, "vctcxo", NULL, CLK_IS_ROOT,
+				26000000);
+	clk_register_clkdev(vctcxo, "vctcxo", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll1", NULL, CLK_IS_ROOT,
+				800000000);
+	clk_register_clkdev(clk, "pll1", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "usb_pll", NULL, CLK_IS_ROOT,
+				480000000);
+	clk_register_clkdev(clk, "usb_pll", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll2", NULL, CLK_IS_ROOT,
+				960000000);
+	clk_register_clkdev(clk, "pll2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_2", "pll1",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_4", "pll1_2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_4", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_8", "pll1_4",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_8", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_16", "pll1_8",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_16", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_20", "pll1_4",
+				CLK_SET_RATE_PARENT, 1, 5);
+	clk_register_clkdev(clk, "pll1_20", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_3", "pll1",
+				CLK_SET_RATE_PARENT, 1, 3);
+	clk_register_clkdev(clk, "pll1_3", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_6", "pll1_3",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_6", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_12", "pll1_6",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_12", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_2", "pll2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_4", "pll2_2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_4", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_8", "pll2_4",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_8", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_16", "pll2_8",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_16", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_3", "pll2",
+				CLK_SET_RATE_PARENT, 1, 3);
+	clk_register_clkdev(clk, "pll2_3", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_6", "pll2_3",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_6", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll2_12", "pll2_6",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll2_12", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vctcxo_2", "vctcxo",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "vctcxo_2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vctcxo_4", "vctcxo_2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "vctcxo_4", NULL);
+
+	clk = mmp_clk_register_factor("uart_pll", "pll1_4", 0,
+				mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl));
+	clk_set_rate(clk, 14745600);
+	clk_register_clkdev(clk, "uart_pll", NULL);
+
+	clk = mmp_clk_register_apbc("twsi0", "vctcxo",
+				apbc_base + APBC_TWSI0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.0");
+
+	clk = mmp_clk_register_apbc("twsi1", "vctcxo",
+				apbc_base + APBC_TWSI1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.1");
+
+	clk = mmp_clk_register_apbc("twsi2", "vctcxo",
+				apbc_base + APBC_TWSI2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.2");
+
+	clk = mmp_clk_register_apbc("twsi3", "vctcxo",
+				apbc_base + APBC_TWSI3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.3");
+
+	clk = mmp_clk_register_apbc("twsi4", "vctcxo",
+				apbc_base + APBC_TWSI4, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.4");
+
+	clk = mmp_clk_register_apbc("twsi5", "vctcxo",
+				apbc_base + APBC_TWSI5, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.5");
+
+	clk = mmp_clk_register_apbc("gpio", "vctcxo",
+				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa-gpio");
+
+	clk = mmp_clk_register_apbc("kpc", "clk32",
+				apbc_base + APBC_KPC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa27x-keypad");
+
+	clk = mmp_clk_register_apbc("rtc", "clk32",
+				apbc_base + APBC_RTC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-rtc");
+
+	clk = mmp_clk_register_apbc("pwm0", "vctcxo",
+				apbc_base + APBC_PWM0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp2-pwm.0");
+
+	clk = mmp_clk_register_apbc("pwm1", "vctcxo",
+				apbc_base + APBC_PWM1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp2-pwm.1");
+
+	clk = mmp_clk_register_apbc("pwm2", "vctcxo",
+				apbc_base + APBC_PWM2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp2-pwm.2");
+
+	clk = mmp_clk_register_apbc("pwm3", "vctcxo",
+				apbc_base + APBC_PWM3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp2-pwm.3");
+
+	clk = clk_register_mux(NULL, "uart0_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART0, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, vctcxo);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("uart0", "uart0_mux",
+				apbc_base + APBC_UART0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.0");
+
+	clk = clk_register_mux(NULL, "uart1_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART1, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, vctcxo);
+	clk_register_clkdev(clk, "uart_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("uart1", "uart1_mux",
+				apbc_base + APBC_UART1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.1");
+
+	clk = clk_register_mux(NULL, "uart2_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART2, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, vctcxo);
+	clk_register_clkdev(clk, "uart_mux.2", NULL);
+
+	clk = mmp_clk_register_apbc("uart2", "uart2_mux",
+				apbc_base + APBC_UART2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.2");
+
+	clk = clk_register_mux(NULL, "uart3_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART3, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, vctcxo);
+	clk_register_clkdev(clk, "uart_mux.3", NULL);
+
+	clk = mmp_clk_register_apbc("uart3", "uart3_mux",
+				apbc_base + APBC_UART3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.3");
+
+	clk = clk_register_mux(NULL, "ssp0_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP0, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("ssp0", "ssp0_mux",
+				apbc_base + APBC_SSP0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.0");
+
+	clk = clk_register_mux(NULL, "ssp1_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP1, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("ssp1", "ssp1_mux",
+				apbc_base + APBC_SSP1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.1");
+
+	clk = clk_register_mux(NULL, "ssp2_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP2, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.2", NULL);
+
+	clk = mmp_clk_register_apbc("ssp2", "ssp2_mux",
+				apbc_base + APBC_SSP2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.2");
+
+	clk = clk_register_mux(NULL, "ssp3_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP3, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.3", NULL);
+
+	clk = mmp_clk_register_apbc("ssp3", "ssp3_mux",
+				apbc_base + APBC_SSP3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.3");
+
+	clk = clk_register_mux(NULL, "sdh_mux", sdh_parent,
+				ARRAY_SIZE(sdh_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_SDH0, 8, 2, 0, &clk_lock);
+	clk_register_clkdev(clk, "sdh_mux", NULL);
+
+	clk = clk_register_divider(NULL, "sdh_div", "sdh_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_SDH0,
+				10, 4, CLK_DIVIDER_ONE_BASED, &clk_lock);
+	clk_register_clkdev(clk, "sdh_div", NULL);
+
+	clk = mmp_clk_register_apmu("sdh0", "sdh_div", apmu_base + APMU_SDH0,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxav3.0");
+
+	clk = mmp_clk_register_apmu("sdh1", "sdh_div", apmu_base + APMU_SDH1,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxav3.1");
+
+	clk = mmp_clk_register_apmu("sdh2", "sdh_div", apmu_base + APMU_SDH2,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxav3.2");
+
+	clk = mmp_clk_register_apmu("sdh3", "sdh_div", apmu_base + APMU_SDH3,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxav3.3");
+
+	clk = mmp_clk_register_apmu("usb", "usb_pll", apmu_base + APMU_USB,
+				0x9, &clk_lock);
+	clk_register_clkdev(clk, "usb_clk", NULL);
+
+	clk = clk_register_mux(NULL, "disp0_mux", disp_parent,
+				ARRAY_SIZE(disp_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_DISP0, 6, 2, 0, &clk_lock);
+	clk_register_clkdev(clk, "disp_mux.0", NULL);
+
+	clk = clk_register_divider(NULL, "disp0_div", "disp0_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_DISP0,
+				8, 4, CLK_DIVIDER_ONE_BASED, &clk_lock);
+	clk_register_clkdev(clk, "disp_div.0", NULL);
+
+	clk = mmp_clk_register_apmu("disp0", "disp0_div",
+				apmu_base + APMU_DISP0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-disp.0");
+
+	clk = clk_register_divider(NULL, "disp0_sphy_div", "disp0_mux", 0,
+				apmu_base + APMU_DISP0, 15, 5, 0, &clk_lock);
+	clk_register_clkdev(clk, "disp_sphy_div.0", NULL);
+
+	clk = mmp_clk_register_apmu("disp0_sphy", "disp0_sphy_div",
+				apmu_base + APMU_DISP0, 0x1024, &clk_lock);
+	clk_register_clkdev(clk, "disp_sphy.0", NULL);
+
+	clk = clk_register_mux(NULL, "disp1_mux", disp_parent,
+				ARRAY_SIZE(disp_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_DISP1, 6, 2, 0, &clk_lock);
+	clk_register_clkdev(clk, "disp_mux.1", NULL);
+
+	clk = clk_register_divider(NULL, "disp1_div", "disp1_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_DISP1,
+				8, 4, CLK_DIVIDER_ONE_BASED, &clk_lock);
+	clk_register_clkdev(clk, "disp_div.1", NULL);
+
+	clk = mmp_clk_register_apmu("disp1", "disp1_div",
+				apmu_base + APMU_DISP1, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-disp.1");
+
+	clk = mmp_clk_register_apmu("ccic_arbiter", "vctcxo",
+				apmu_base + APMU_CCIC0, 0x1800, &clk_lock);
+	clk_register_clkdev(clk, "ccic_arbiter", NULL);
+
+	clk = clk_register_mux(NULL, "ccic0_mux", ccic_parent,
+				ARRAY_SIZE(ccic_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_CCIC0, 6, 2, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_mux.0", NULL);
+
+	clk = clk_register_divider(NULL, "ccic0_div", "ccic0_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				17, 4, CLK_DIVIDER_ONE_BASED, &clk_lock);
+	clk_register_clkdev(clk, "ccic_div.0", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0", "ccic0_div",
+				apmu_base + APMU_CCIC0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, "fnclk", "mmp-ccic.0");
+
+	clk = mmp_clk_register_apmu("ccic0_phy", "ccic0_div",
+				apmu_base + APMU_CCIC0, 0x24, &clk_lock);
+	clk_register_clkdev(clk, "phyclk", "mmp-ccic.0");
+
+	clk = clk_register_divider(NULL, "ccic0_sphy_div", "ccic0_div",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				10, 5, 0, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk_div", "mmp-ccic.0");
+
+	clk = mmp_clk_register_apmu("ccic0_sphy", "ccic0_sphy_div",
+				apmu_base + APMU_CCIC0, 0x300, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk", "mmp-ccic.0");
+
+	clk = clk_register_mux(NULL, "ccic1_mux", ccic_parent,
+				ARRAY_SIZE(ccic_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_CCIC1, 6, 2, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_mux.1", NULL);
+
+	clk = clk_register_divider(NULL, "ccic1_div", "ccic1_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC1,
+				16, 4, CLK_DIVIDER_ONE_BASED, &clk_lock);
+	clk_register_clkdev(clk, "ccic_div.1", NULL);
+
+	clk = mmp_clk_register_apmu("ccic1", "ccic1_div",
+				apmu_base + APMU_CCIC1, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, "fnclk", "mmp-ccic.1");
+
+	clk = mmp_clk_register_apmu("ccic1_phy", "ccic1_div",
+				apmu_base + APMU_CCIC1, 0x24, &clk_lock);
+	clk_register_clkdev(clk, "phyclk", "mmp-ccic.1");
+
+	clk = clk_register_divider(NULL, "ccic1_sphy_div", "ccic1_div",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC1,
+				10, 5, 0, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk_div", "mmp-ccic.1");
+
+	clk = mmp_clk_register_apmu("ccic1_sphy", "ccic1_sphy_div",
+				apmu_base + APMU_CCIC1, 0x300, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk", "mmp-ccic.1");
+}
diff --git a/drivers/clk/mmp/clk-pxa168.c b/drivers/clk/mmp/clk-pxa168.c
new file mode 100644
index 000000000000..e8d036c12cbf
--- /dev/null
+++ b/drivers/clk/mmp/clk-pxa168.c
@@ -0,0 +1,346 @@
+/*
+ * pxa168 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#include <mach/addr-map.h>
+
+#include "clk.h"
+
+#define APBC_RTC	0x28
+#define APBC_TWSI0	0x2c
+#define APBC_KPC	0x30
+#define APBC_UART0	0x0
+#define APBC_UART1	0x4
+#define APBC_GPIO	0x8
+#define APBC_PWM0	0xc
+#define APBC_PWM1	0x10
+#define APBC_PWM2	0x14
+#define APBC_PWM3	0x18
+#define APBC_SSP0	0x81c
+#define APBC_SSP1	0x820
+#define APBC_SSP2	0x84c
+#define APBC_SSP3	0x858
+#define APBC_SSP4	0x85c
+#define APBC_TWSI1	0x6c
+#define APBC_UART2	0x70
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_CCIC0	0x50
+#define APMU_DFC	0x60
+#define MPMU_UART_PLL	0x14
+
+static DEFINE_SPINLOCK(clk_lock);
+
+static struct clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 8125, .den = 1536},	/*14.745MHZ */
+};
+
+static const char *uart_parent[] = {"pll1_3_16", "uart_pll"};
+static const char *ssp_parent[] = {"pll1_96", "pll1_48", "pll1_24", "pll1_12"};
+static const char *sdh_parent[] = {"pll1_12", "pll1_13"};
+static const char *disp_parent[] = {"pll1_2", "pll1_12"};
+static const char *ccic_parent[] = {"pll1_2", "pll1_12"};
+static const char *ccic_phy_parent[] = {"pll1_6", "pll1_12"};
+
+void __init pxa168_clk_init(void)
+{
+	struct clk *clk;
+	struct clk *uart_pll;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbc_base;
+
+	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	if (mpmu_base == NULL) {
+		pr_err("error to ioremap MPMU base\n");
+		return;
+	}
+
+	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	if (apmu_base == NULL) {
+		pr_err("error to ioremap APMU base\n");
+		return;
+	}
+
+	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	if (apbc_base == NULL) {
+		pr_err("error to ioremap APBC base\n");
+		return;
+	}
+
+	clk = clk_register_fixed_rate(NULL, "clk32", NULL, CLK_IS_ROOT, 3200);
+	clk_register_clkdev(clk, "clk32", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "vctcxo", NULL, CLK_IS_ROOT,
+				26000000);
+	clk_register_clkdev(clk, "vctcxo", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll1", NULL, CLK_IS_ROOT,
+				624000000);
+	clk_register_clkdev(clk, "pll1", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_2", "pll1",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_4", "pll1_2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_4", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_8", "pll1_4",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_8", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_16", "pll1_8",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_16", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_6", "pll1_2",
+				CLK_SET_RATE_PARENT, 1, 3);
+	clk_register_clkdev(clk, "pll1_6", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_12", "pll1_6",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_12", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_24", "pll1_12",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_24", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_48", "pll1_24",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_48", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_96", "pll1_48",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_96", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_13", "pll1",
+				CLK_SET_RATE_PARENT, 1, 13);
+	clk_register_clkdev(clk, "pll1_13", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_13_1_5", "pll1",
+				CLK_SET_RATE_PARENT, 2, 3);
+	clk_register_clkdev(clk, "pll1_13_1_5", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_2_1_5", "pll1",
+				CLK_SET_RATE_PARENT, 2, 3);
+	clk_register_clkdev(clk, "pll1_2_1_5", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_3_16", "pll1",
+				CLK_SET_RATE_PARENT, 3, 16);
+	clk_register_clkdev(clk, "pll1_3_16", NULL);
+
+	uart_pll = mmp_clk_register_factor("uart_pll", "pll1_4", 0,
+				mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl));
+	clk_set_rate(uart_pll, 14745600);
+	clk_register_clkdev(uart_pll, "uart_pll", NULL);
+
+	clk = mmp_clk_register_apbc("twsi0", "pll1_13_1_5",
+				apbc_base + APBC_TWSI0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.0");
+
+	clk = mmp_clk_register_apbc("twsi1", "pll1_13_1_5",
+				apbc_base + APBC_TWSI1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.1");
+
+	clk = mmp_clk_register_apbc("gpio", "vctcxo",
+				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa-gpio");
+
+	clk = mmp_clk_register_apbc("kpc", "clk32",
+				apbc_base + APBC_KPC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa27x-keypad");
+
+	clk = mmp_clk_register_apbc("rtc", "clk32",
+				apbc_base + APBC_RTC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sa1100-rtc");
+
+	clk = mmp_clk_register_apbc("pwm0", "pll1_48",
+				apbc_base + APBC_PWM0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa168-pwm.0");
+
+	clk = mmp_clk_register_apbc("pwm1", "pll1_48",
+				apbc_base + APBC_PWM1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa168-pwm.1");
+
+	clk = mmp_clk_register_apbc("pwm2", "pll1_48",
+				apbc_base + APBC_PWM2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa168-pwm.2");
+
+	clk = mmp_clk_register_apbc("pwm3", "pll1_48",
+				apbc_base + APBC_PWM3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa168-pwm.3");
+
+	clk = clk_register_mux(NULL, "uart0_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART0, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("uart0", "uart0_mux",
+				apbc_base + APBC_UART0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.0");
+
+	clk = clk_register_mux(NULL, "uart1_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART1, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("uart1", "uart1_mux",
+				apbc_base + APBC_UART1,	10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.1");
+
+	clk = clk_register_mux(NULL, "uart2_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART2, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.2", NULL);
+
+	clk = mmp_clk_register_apbc("uart2", "uart2_mux",
+				apbc_base + APBC_UART2,	10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.2");
+
+	clk = clk_register_mux(NULL, "ssp0_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP0, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("ssp0", "ssp0_mux", apbc_base + APBC_SSP0,
+				10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.0");
+
+	clk = clk_register_mux(NULL, "ssp1_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP1, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("ssp1", "ssp1_mux", apbc_base + APBC_SSP1,
+				10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.1");
+
+	clk = clk_register_mux(NULL, "ssp2_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP2, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.2", NULL);
+
+	clk = mmp_clk_register_apbc("ssp2", "ssp1_mux", apbc_base + APBC_SSP2,
+				10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.2");
+
+	clk = clk_register_mux(NULL, "ssp3_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP3, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.3", NULL);
+
+	clk = mmp_clk_register_apbc("ssp3", "ssp1_mux", apbc_base + APBC_SSP3,
+				10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.3");
+
+	clk = clk_register_mux(NULL, "ssp4_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP4, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.4", NULL);
+
+	clk = mmp_clk_register_apbc("ssp4", "ssp1_mux", apbc_base + APBC_SSP4,
+				10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.4");
+
+	clk = mmp_clk_register_apmu("dfc", "pll1_4", apmu_base + APMU_DFC,
+				0x19b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa3xx-nand.0");
+
+	clk = clk_register_mux(NULL, "sdh0_mux", sdh_parent,
+				ARRAY_SIZE(sdh_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_SDH0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "sdh0_mux", NULL);
+
+	clk = mmp_clk_register_apmu("sdh0", "sdh_mux", apmu_base + APMU_SDH0,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxa.0");
+
+	clk = clk_register_mux(NULL, "sdh1_mux", sdh_parent,
+				ARRAY_SIZE(sdh_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_SDH1, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "sdh1_mux", NULL);
+
+	clk = mmp_clk_register_apmu("sdh1", "sdh1_mux", apmu_base + APMU_SDH1,
+				0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxa.1");
+
+	clk = mmp_clk_register_apmu("usb", "usb_pll", apmu_base + APMU_USB,
+				0x9, &clk_lock);
+	clk_register_clkdev(clk, "usb_clk", NULL);
+
+	clk = mmp_clk_register_apmu("sph", "usb_pll", apmu_base + APMU_USB,
+				0x12, &clk_lock);
+	clk_register_clkdev(clk, "sph_clk", NULL);
+
+	clk = clk_register_mux(NULL, "disp0_mux", disp_parent,
+				ARRAY_SIZE(disp_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_DISP0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "disp_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("disp0", "disp0_mux",
+				apmu_base + APMU_DISP0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, "fnclk", "mmp-disp.0");
+
+	clk = mmp_clk_register_apmu("disp0_hclk", "disp0_mux",
+				apmu_base + APMU_DISP0, 0x24, &clk_lock);
+	clk_register_clkdev(clk, "hclk", "mmp-disp.0");
+
+	clk = clk_register_mux(NULL, "ccic0_mux", ccic_parent,
+				ARRAY_SIZE(ccic_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_CCIC0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0", "ccic0_mux",
+				apmu_base + APMU_CCIC0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, "fnclk", "mmp-ccic.0");
+
+	clk = clk_register_mux(NULL, "ccic0_phy_mux", ccic_phy_parent,
+				ARRAY_SIZE(ccic_phy_parent),
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				7, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_phy_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0_phy", "ccic0_phy_mux",
+				apmu_base + APMU_CCIC0, 0x24, &clk_lock);
+	clk_register_clkdev(clk, "phyclk", "mmp-ccic.0");
+
+	clk = clk_register_divider(NULL, "ccic0_sphy_div", "ccic0_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				10, 5, 0, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk_div", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0_sphy", "ccic0_sphy_div",
+				apmu_base + APMU_CCIC0, 0x300, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk", "mmp-ccic.0");
+}
diff --git a/drivers/clk/mmp/clk-pxa910.c b/drivers/clk/mmp/clk-pxa910.c
new file mode 100644
index 000000000000..7048c31d6e7e
--- /dev/null
+++ b/drivers/clk/mmp/clk-pxa910.c
@@ -0,0 +1,320 @@
+/*
+ * pxa910 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#include <mach/addr-map.h>
+
+#include "clk.h"
+
+#define APBC_RTC	0x28
+#define APBC_TWSI0	0x2c
+#define APBC_KPC	0x18
+#define APBC_UART0	0x0
+#define APBC_UART1	0x4
+#define APBC_GPIO	0x8
+#define APBC_PWM0	0xc
+#define APBC_PWM1	0x10
+#define APBC_PWM2	0x14
+#define APBC_PWM3	0x18
+#define APBC_SSP0	0x1c
+#define APBC_SSP1	0x20
+#define APBC_SSP2	0x4c
+#define APBCP_TWSI1	0x28
+#define APBCP_UART2	0x1c
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_CCIC0	0x50
+#define APMU_DFC	0x60
+#define MPMU_UART_PLL	0x14
+
+static DEFINE_SPINLOCK(clk_lock);
+
+static struct clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 8125, .den = 1536},	/*14.745MHZ */
+};
+
+static const char *uart_parent[] = {"pll1_3_16", "uart_pll"};
+static const char *ssp_parent[] = {"pll1_96", "pll1_48", "pll1_24", "pll1_12"};
+static const char *sdh_parent[] = {"pll1_12", "pll1_13"};
+static const char *disp_parent[] = {"pll1_2", "pll1_12"};
+static const char *ccic_parent[] = {"pll1_2", "pll1_12"};
+static const char *ccic_phy_parent[] = {"pll1_6", "pll1_12"};
+
+void __init pxa910_clk_init(void)
+{
+	struct clk *clk;
+	struct clk *uart_pll;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbcp_base;
+	void __iomem *apbc_base;
+
+	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	if (mpmu_base == NULL) {
+		pr_err("error to ioremap MPMU base\n");
+		return;
+	}
+
+	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	if (apmu_base == NULL) {
+		pr_err("error to ioremap APMU base\n");
+		return;
+	}
+
+	apbcp_base = ioremap(APB_PHYS_BASE + 0x3b000, SZ_4K);
+	if (apbcp_base == NULL) {
+		pr_err("error to ioremap APBC extension base\n");
+		return;
+	}
+
+	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	if (apbc_base == NULL) {
+		pr_err("error to ioremap APBC base\n");
+		return;
+	}
+
+	clk = clk_register_fixed_rate(NULL, "clk32", NULL, CLK_IS_ROOT, 3200);
+	clk_register_clkdev(clk, "clk32", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "vctcxo", NULL, CLK_IS_ROOT,
+				26000000);
+	clk_register_clkdev(clk, "vctcxo", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll1", NULL, CLK_IS_ROOT,
+				624000000);
+	clk_register_clkdev(clk, "pll1", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_2", "pll1",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_2", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_4", "pll1_2",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_4", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_8", "pll1_4",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_8", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_16", "pll1_8",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_16", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_6", "pll1_2",
+				CLK_SET_RATE_PARENT, 1, 3);
+	clk_register_clkdev(clk, "pll1_6", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_12", "pll1_6",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_12", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_24", "pll1_12",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_24", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_48", "pll1_24",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_48", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_96", "pll1_48",
+				CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll1_96", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_13", "pll1",
+				CLK_SET_RATE_PARENT, 1, 13);
+	clk_register_clkdev(clk, "pll1_13", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_13_1_5", "pll1",
+				CLK_SET_RATE_PARENT, 2, 3);
+	clk_register_clkdev(clk, "pll1_13_1_5", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_2_1_5", "pll1",
+				CLK_SET_RATE_PARENT, 2, 3);
+	clk_register_clkdev(clk, "pll1_2_1_5", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "pll1_3_16", "pll1",
+				CLK_SET_RATE_PARENT, 3, 16);
+	clk_register_clkdev(clk, "pll1_3_16", NULL);
+
+	uart_pll =  mmp_clk_register_factor("uart_pll", "pll1_4", 0,
+				mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl));
+	clk_set_rate(uart_pll, 14745600);
+	clk_register_clkdev(uart_pll, "uart_pll", NULL);
+
+	clk = mmp_clk_register_apbc("twsi0", "pll1_13_1_5",
+				apbc_base + APBC_TWSI0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.0");
+
+	clk = mmp_clk_register_apbc("twsi1", "pll1_13_1_5",
+				apbcp_base + APBCP_TWSI1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-i2c.1");
+
+	clk = mmp_clk_register_apbc("gpio", "vctcxo",
+				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa-gpio");
+
+	clk = mmp_clk_register_apbc("kpc", "clk32",
+				apbc_base + APBC_KPC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa27x-keypad");
+
+	clk = mmp_clk_register_apbc("rtc", "clk32",
+				apbc_base + APBC_RTC, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sa1100-rtc");
+
+	clk = mmp_clk_register_apbc("pwm0", "pll1_48",
+				apbc_base + APBC_PWM0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa910-pwm.0");
+
+	clk = mmp_clk_register_apbc("pwm1", "pll1_48",
+				apbc_base + APBC_PWM1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa910-pwm.1");
+
+	clk = mmp_clk_register_apbc("pwm2", "pll1_48",
+				apbc_base + APBC_PWM2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa910-pwm.2");
+
+	clk = mmp_clk_register_apbc("pwm3", "pll1_48",
+				apbc_base + APBC_PWM3, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa910-pwm.3");
+
+	clk = clk_register_mux(NULL, "uart0_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART0, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("uart0", "uart0_mux",
+				apbc_base + APBC_UART0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.0");
+
+	clk = clk_register_mux(NULL, "uart1_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_UART1, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("uart1", "uart1_mux",
+				apbc_base + APBC_UART1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.1");
+
+	clk = clk_register_mux(NULL, "uart2_mux", uart_parent,
+				ARRAY_SIZE(uart_parent), CLK_SET_RATE_PARENT,
+				apbcp_base + APBCP_UART2, 4, 3, 0, &clk_lock);
+	clk_set_parent(clk, uart_pll);
+	clk_register_clkdev(clk, "uart_mux.2", NULL);
+
+	clk = mmp_clk_register_apbc("uart2", "uart2_mux",
+				apbcp_base + APBCP_UART2, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa2xx-uart.2");
+
+	clk = clk_register_mux(NULL, "ssp0_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP0, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "uart_mux.0", NULL);
+
+	clk = mmp_clk_register_apbc("ssp0", "ssp0_mux",
+				apbc_base + APBC_SSP0, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.0");
+
+	clk = clk_register_mux(NULL, "ssp1_mux", ssp_parent,
+				ARRAY_SIZE(ssp_parent), CLK_SET_RATE_PARENT,
+				apbc_base + APBC_SSP1, 4, 3, 0, &clk_lock);
+	clk_register_clkdev(clk, "ssp_mux.1", NULL);
+
+	clk = mmp_clk_register_apbc("ssp1", "ssp1_mux",
+				apbc_base + APBC_SSP1, 10, 0, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-ssp.1");
+
+	clk = mmp_clk_register_apmu("dfc", "pll1_4",
+				apmu_base + APMU_DFC, 0x19b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "pxa3xx-nand.0");
+
+	clk = clk_register_mux(NULL, "sdh0_mux", sdh_parent,
+				ARRAY_SIZE(sdh_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_SDH0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "sdh0_mux", NULL);
+
+	clk = mmp_clk_register_apmu("sdh0", "sdh_mux",
+				apmu_base + APMU_SDH0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxa.0");
+
+	clk = clk_register_mux(NULL, "sdh1_mux", sdh_parent,
+				ARRAY_SIZE(sdh_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_SDH1, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "sdh1_mux", NULL);
+
+	clk = mmp_clk_register_apmu("sdh1", "sdh1_mux",
+				apmu_base + APMU_SDH1, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "sdhci-pxa.1");
+
+	clk = mmp_clk_register_apmu("usb", "usb_pll",
+				apmu_base + APMU_USB, 0x9, &clk_lock);
+	clk_register_clkdev(clk, "usb_clk", NULL);
+
+	clk = mmp_clk_register_apmu("sph", "usb_pll",
+				apmu_base + APMU_USB, 0x12, &clk_lock);
+	clk_register_clkdev(clk, "sph_clk", NULL);
+
+	clk = clk_register_mux(NULL, "disp0_mux", disp_parent,
+				ARRAY_SIZE(disp_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_DISP0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "disp_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("disp0", "disp0_mux",
+				apmu_base + APMU_DISP0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, NULL, "mmp-disp.0");
+
+	clk = clk_register_mux(NULL, "ccic0_mux", ccic_parent,
+				ARRAY_SIZE(ccic_parent), CLK_SET_RATE_PARENT,
+				apmu_base + APMU_CCIC0, 6, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0", "ccic0_mux",
+				apmu_base + APMU_CCIC0, 0x1b, &clk_lock);
+	clk_register_clkdev(clk, "fnclk", "mmp-ccic.0");
+
+	clk = clk_register_mux(NULL, "ccic0_phy_mux", ccic_phy_parent,
+				ARRAY_SIZE(ccic_phy_parent),
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				7, 1, 0, &clk_lock);
+	clk_register_clkdev(clk, "ccic_phy_mux.0", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0_phy", "ccic0_phy_mux",
+				apmu_base + APMU_CCIC0, 0x24, &clk_lock);
+	clk_register_clkdev(clk, "phyclk", "mmp-ccic.0");
+
+	clk = clk_register_divider(NULL, "ccic0_sphy_div", "ccic0_mux",
+				CLK_SET_RATE_PARENT, apmu_base + APMU_CCIC0,
+				10, 5, 0, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk_div", NULL);
+
+	clk = mmp_clk_register_apmu("ccic0_sphy", "ccic0_sphy_div",
+				apmu_base + APMU_CCIC0, 0x300, &clk_lock);
+	clk_register_clkdev(clk, "sphyclk", "mmp-ccic.0");
+}
diff --git a/drivers/clk/mmp/clk.h b/drivers/clk/mmp/clk.h
new file mode 100644
index 000000000000..ab86dd4a416a
--- /dev/null
+++ b/drivers/clk/mmp/clk.h
@@ -0,0 +1,35 @@
+#ifndef __MACH_MMP_CLK_H
+#define __MACH_MMP_CLK_H
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+
+#define APBC_NO_BUS_CTRL	BIT(0)
+#define APBC_POWER_CTRL		BIT(1)
+
+struct clk_factor_masks {
+	unsigned int	factor;
+	unsigned int	num_mask;
+	unsigned int	den_mask;
+	unsigned int	num_shift;
+	unsigned int	den_shift;
+};
+
+struct clk_factor_tbl {
+	unsigned int num;
+	unsigned int den;
+};
+
+extern struct clk *mmp_clk_register_pll2(const char *name,
+		const char *parent_name, unsigned long flags);
+extern struct clk *mmp_clk_register_apbc(const char *name,
+		const char *parent_name, void __iomem *base,
+		unsigned int delay, unsigned int apbc_flags, spinlock_t *lock);
+extern struct clk *mmp_clk_register_apmu(const char *name,
+		const char *parent_name, void __iomem *base, u32 enable_mask,
+		spinlock_t *lock);
+extern struct clk *mmp_clk_register_factor(const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *base, struct clk_factor_masks *masks,
+		struct clk_factor_tbl *ftbl, unsigned int ftbl_cnt);
+#endif
diff --git a/drivers/clk/ux500/Makefile b/drivers/clk/ux500/Makefile
new file mode 100644
index 000000000000..858fbfe66281
--- /dev/null
+++ b/drivers/clk/ux500/Makefile
@@ -0,0 +1,12 @@
+#
+# Makefile for ux500 clocks
+#
+
+# Clock types
+obj-y += clk-prcc.o
+obj-y += clk-prcmu.o
+
+# Clock definitions
+obj-y += u8500_clk.o
+obj-y += u9540_clk.o
+obj-y += u8540_clk.o
diff --git a/drivers/clk/ux500/clk-prcc.c b/drivers/clk/ux500/clk-prcc.c
new file mode 100644
index 000000000000..7eee7f768355
--- /dev/null
+++ b/drivers/clk/ux500/clk-prcc.c
@@ -0,0 +1,164 @@
+/*
+ * PRCC clock implementation for ux500 platform.
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clk-private.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/types.h>
+#include <mach/hardware.h>
+
+#include "clk.h"
+
+#define PRCC_PCKEN			0x000
+#define PRCC_PCKDIS			0x004
+#define PRCC_KCKEN			0x008
+#define PRCC_KCKDIS			0x00C
+#define PRCC_PCKSR			0x010
+#define PRCC_KCKSR			0x014
+
+#define to_clk_prcc(_hw) container_of(_hw, struct clk_prcc, hw)
+
+struct clk_prcc {
+	struct clk_hw hw;
+	void __iomem *base;
+	u32 cg_sel;
+	int is_enabled;
+};
+
+/* PRCC clock operations. */
+
+static int clk_prcc_pclk_enable(struct clk_hw *hw)
+{
+	struct clk_prcc *clk = to_clk_prcc(hw);
+
+	writel(clk->cg_sel, (clk->base + PRCC_PCKEN));
+	while (!(readl(clk->base + PRCC_PCKSR) & clk->cg_sel))
+		cpu_relax();
+
+	clk->is_enabled = 1;
+	return 0;
+}
+
+static void clk_prcc_pclk_disable(struct clk_hw *hw)
+{
+	struct clk_prcc *clk = to_clk_prcc(hw);
+
+	writel(clk->cg_sel, (clk->base + PRCC_PCKDIS));
+	clk->is_enabled = 0;
+}
+
+static int clk_prcc_kclk_enable(struct clk_hw *hw)
+{
+	struct clk_prcc *clk = to_clk_prcc(hw);
+
+	writel(clk->cg_sel, (clk->base + PRCC_KCKEN));
+	while (!(readl(clk->base + PRCC_KCKSR) & clk->cg_sel))
+		cpu_relax();
+
+	clk->is_enabled = 1;
+	return 0;
+}
+
+static void clk_prcc_kclk_disable(struct clk_hw *hw)
+{
+	struct clk_prcc *clk = to_clk_prcc(hw);
+
+	writel(clk->cg_sel, (clk->base + PRCC_KCKDIS));
+	clk->is_enabled = 0;
+}
+
+static int clk_prcc_is_enabled(struct clk_hw *hw)
+{
+	struct clk_prcc *clk = to_clk_prcc(hw);
+	return clk->is_enabled;
+}
+
+static struct clk_ops clk_prcc_pclk_ops = {
+	.enable = clk_prcc_pclk_enable,
+	.disable = clk_prcc_pclk_disable,
+	.is_enabled = clk_prcc_is_enabled,
+};
+
+static struct clk_ops clk_prcc_kclk_ops = {
+	.enable = clk_prcc_kclk_enable,
+	.disable = clk_prcc_kclk_disable,
+	.is_enabled = clk_prcc_is_enabled,
+};
+
+static struct clk *clk_reg_prcc(const char *name,
+				const char *parent_name,
+				resource_size_t phy_base,
+				u32 cg_sel,
+				unsigned long flags,
+				struct clk_ops *clk_prcc_ops)
+{
+	struct clk_prcc *clk;
+	struct clk_init_data clk_prcc_init;
+	struct clk *clk_reg;
+
+	if (!name) {
+		pr_err("clk_prcc: %s invalid arguments passed\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	clk = kzalloc(sizeof(struct clk_prcc), GFP_KERNEL);
+	if (!clk) {
+		pr_err("clk_prcc: %s could not allocate clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	clk->base = ioremap(phy_base, SZ_4K);
+	if (!clk->base)
+		goto free_clk;
+
+	clk->cg_sel = cg_sel;
+	clk->is_enabled = 1;
+
+	clk_prcc_init.name = name;
+	clk_prcc_init.ops = clk_prcc_ops;
+	clk_prcc_init.flags = flags;
+	clk_prcc_init.parent_names = (parent_name ? &parent_name : NULL);
+	clk_prcc_init.num_parents = (parent_name ? 1 : 0);
+	clk->hw.init = &clk_prcc_init;
+
+	clk_reg = clk_register(NULL, &clk->hw);
+	if (IS_ERR_OR_NULL(clk_reg))
+		goto unmap_clk;
+
+	return clk_reg;
+
+unmap_clk:
+	iounmap(clk->base);
+free_clk:
+	kfree(clk);
+	pr_err("clk_prcc: %s failed to register clk\n", __func__);
+	return ERR_PTR(-ENOMEM);
+}
+
+struct clk *clk_reg_prcc_pclk(const char *name,
+			      const char *parent_name,
+			      resource_size_t phy_base,
+			      u32 cg_sel,
+			      unsigned long flags)
+{
+	return clk_reg_prcc(name, parent_name, phy_base, cg_sel, flags,
+			&clk_prcc_pclk_ops);
+}
+
+struct clk *clk_reg_prcc_kclk(const char *name,
+			      const char *parent_name,
+			      resource_size_t phy_base,
+			      u32 cg_sel,
+			      unsigned long flags)
+{
+	return clk_reg_prcc(name, parent_name, phy_base, cg_sel, flags,
+			&clk_prcc_kclk_ops);
+}
diff --git a/drivers/clk/ux500/clk-prcmu.c b/drivers/clk/ux500/clk-prcmu.c
new file mode 100644
index 000000000000..930cdfeb47ab
--- /dev/null
+++ b/drivers/clk/ux500/clk-prcmu.c
@@ -0,0 +1,252 @@
+/*
+ * PRCMU clock implementation for ux500 platform.
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clk-private.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+#define to_clk_prcmu(_hw) container_of(_hw, struct clk_prcmu, hw)
+
+struct clk_prcmu {
+	struct clk_hw hw;
+	u8 cg_sel;
+	int is_enabled;
+};
+
+/* PRCMU clock operations. */
+
+static int clk_prcmu_prepare(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return prcmu_request_clock(clk->cg_sel, true);
+}
+
+static void clk_prcmu_unprepare(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	if (prcmu_request_clock(clk->cg_sel, false))
+		pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
+			hw->init->name);
+}
+
+static int clk_prcmu_enable(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	clk->is_enabled = 1;
+	return 0;
+}
+
+static void clk_prcmu_disable(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	clk->is_enabled = 0;
+}
+
+static int clk_prcmu_is_enabled(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return clk->is_enabled;
+}
+
+static unsigned long clk_prcmu_recalc_rate(struct clk_hw *hw,
+					   unsigned long parent_rate)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return prcmu_clock_rate(clk->cg_sel);
+}
+
+static long clk_prcmu_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *parent_rate)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return prcmu_round_clock_rate(clk->cg_sel, rate);
+}
+
+static int clk_prcmu_set_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long parent_rate)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+	return prcmu_set_clock_rate(clk->cg_sel, rate);
+}
+
+static int request_ape_opp100(bool enable)
+{
+	static int reqs;
+	int err = 0;
+
+	if (enable) {
+		if (!reqs)
+			err = prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+							"clock", 100);
+		if (!err)
+			reqs++;
+	} else {
+		reqs--;
+		if (!reqs)
+			prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+						"clock");
+	}
+	return err;
+}
+
+static int clk_prcmu_opp_prepare(struct clk_hw *hw)
+{
+	int err;
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+
+	err = request_ape_opp100(true);
+	if (err) {
+		pr_err("clk_prcmu: %s failed to request APE OPP100 for %s.\n",
+			__func__, hw->init->name);
+		return err;
+	}
+
+	err = prcmu_request_clock(clk->cg_sel, true);
+	if (err)
+		request_ape_opp100(false);
+
+	return err;
+}
+
+static void clk_prcmu_opp_unprepare(struct clk_hw *hw)
+{
+	struct clk_prcmu *clk = to_clk_prcmu(hw);
+
+	if (prcmu_request_clock(clk->cg_sel, false))
+		goto out_error;
+	if (request_ape_opp100(false))
+		goto out_error;
+	return;
+
+out_error:
+	pr_err("clk_prcmu: %s failed to disable %s.\n", __func__,
+		hw->init->name);
+}
+
+static struct clk_ops clk_prcmu_scalable_ops = {
+	.prepare = clk_prcmu_prepare,
+	.unprepare = clk_prcmu_unprepare,
+	.enable = clk_prcmu_enable,
+	.disable = clk_prcmu_disable,
+	.is_enabled = clk_prcmu_is_enabled,
+	.recalc_rate = clk_prcmu_recalc_rate,
+	.round_rate = clk_prcmu_round_rate,
+	.set_rate = clk_prcmu_set_rate,
+};
+
+static struct clk_ops clk_prcmu_gate_ops = {
+	.prepare = clk_prcmu_prepare,
+	.unprepare = clk_prcmu_unprepare,
+	.enable = clk_prcmu_enable,
+	.disable = clk_prcmu_disable,
+	.is_enabled = clk_prcmu_is_enabled,
+	.recalc_rate = clk_prcmu_recalc_rate,
+};
+
+static struct clk_ops clk_prcmu_rate_ops = {
+	.is_enabled = clk_prcmu_is_enabled,
+	.recalc_rate = clk_prcmu_recalc_rate,
+};
+
+static struct clk_ops clk_prcmu_opp_gate_ops = {
+	.prepare = clk_prcmu_opp_prepare,
+	.unprepare = clk_prcmu_opp_unprepare,
+	.enable = clk_prcmu_enable,
+	.disable = clk_prcmu_disable,
+	.is_enabled = clk_prcmu_is_enabled,
+	.recalc_rate = clk_prcmu_recalc_rate,
+};
+
+static struct clk *clk_reg_prcmu(const char *name,
+				 const char *parent_name,
+				 u8 cg_sel,
+				 unsigned long rate,
+				 unsigned long flags,
+				 struct clk_ops *clk_prcmu_ops)
+{
+	struct clk_prcmu *clk;
+	struct clk_init_data clk_prcmu_init;
+	struct clk *clk_reg;
+
+	if (!name) {
+		pr_err("clk_prcmu: %s invalid arguments passed\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	clk = kzalloc(sizeof(struct clk_prcmu), GFP_KERNEL);
+	if (!clk) {
+		pr_err("clk_prcmu: %s could not allocate clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	clk->cg_sel = cg_sel;
+	clk->is_enabled = 1;
+	/* "rate" can be used for changing the initial frequency */
+	if (rate)
+		prcmu_set_clock_rate(cg_sel, rate);
+
+	clk_prcmu_init.name = name;
+	clk_prcmu_init.ops = clk_prcmu_ops;
+	clk_prcmu_init.flags = flags;
+	clk_prcmu_init.parent_names = (parent_name ? &parent_name : NULL);
+	clk_prcmu_init.num_parents = (parent_name ? 1 : 0);
+	clk->hw.init = &clk_prcmu_init;
+
+	clk_reg = clk_register(NULL, &clk->hw);
+	if (IS_ERR_OR_NULL(clk_reg))
+		goto free_clk;
+
+	return clk_reg;
+
+free_clk:
+	kfree(clk);
+	pr_err("clk_prcmu: %s failed to register clk\n", __func__);
+	return ERR_PTR(-ENOMEM);
+}
+
+struct clk *clk_reg_prcmu_scalable(const char *name,
+				   const char *parent_name,
+				   u8 cg_sel,
+				   unsigned long rate,
+				   unsigned long flags)
+{
+	return clk_reg_prcmu(name, parent_name, cg_sel, rate, flags,
+			&clk_prcmu_scalable_ops);
+}
+
+struct clk *clk_reg_prcmu_gate(const char *name,
+			       const char *parent_name,
+			       u8 cg_sel,
+			       unsigned long flags)
+{
+	return clk_reg_prcmu(name, parent_name, cg_sel, 0, flags,
+			&clk_prcmu_gate_ops);
+}
+
+struct clk *clk_reg_prcmu_rate(const char *name,
+			       const char *parent_name,
+			       u8 cg_sel,
+			       unsigned long flags)
+{
+	return clk_reg_prcmu(name, parent_name, cg_sel, 0, flags,
+			&clk_prcmu_rate_ops);
+}
+
+struct clk *clk_reg_prcmu_opp_gate(const char *name,
+				   const char *parent_name,
+				   u8 cg_sel,
+				   unsigned long flags)
+{
+	return clk_reg_prcmu(name, parent_name, cg_sel, 0, flags,
+			&clk_prcmu_opp_gate_ops);
+}
diff --git a/drivers/clk/ux500/clk.h b/drivers/clk/ux500/clk.h
new file mode 100644
index 000000000000..836d7d16751e
--- /dev/null
+++ b/drivers/clk/ux500/clk.h
@@ -0,0 +1,48 @@
+/*
+ * Clocks for ux500 platforms
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __UX500_CLK_H
+#define __UX500_CLK_H
+
+#include <linux/clk.h>
+
+struct clk *clk_reg_prcc_pclk(const char *name,
+			      const char *parent_name,
+			      unsigned int phy_base,
+			      u32 cg_sel,
+			      unsigned long flags);
+
+struct clk *clk_reg_prcc_kclk(const char *name,
+			      const char *parent_name,
+			      unsigned int phy_base,
+			      u32 cg_sel,
+			      unsigned long flags);
+
+struct clk *clk_reg_prcmu_scalable(const char *name,
+				   const char *parent_name,
+				   u8 cg_sel,
+				   unsigned long rate,
+				   unsigned long flags);
+
+struct clk *clk_reg_prcmu_gate(const char *name,
+			       const char *parent_name,
+			       u8 cg_sel,
+			       unsigned long flags);
+
+struct clk *clk_reg_prcmu_rate(const char *name,
+			       const char *parent_name,
+			       u8 cg_sel,
+			       unsigned long flags);
+
+struct clk *clk_reg_prcmu_opp_gate(const char *name,
+				   const char *parent_name,
+				   u8 cg_sel,
+				   unsigned long flags);
+
+#endif /* __UX500_CLK_H */
diff --git a/drivers/clk/ux500/u8500_clk.c b/drivers/clk/ux500/u8500_clk.c
new file mode 100644
index 000000000000..ca4a25ed844c
--- /dev/null
+++ b/drivers/clk/ux500/u8500_clk.c
@@ -0,0 +1,477 @@
+/*
+ * Clock definitions for u8500 platform.
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/platform_data/clk-ux500.h>
+
+#include "clk.h"
+
+void u8500_clk_init(void)
+{
+	struct prcmu_fw_version *fw_version;
+	const char *sgaclk_parent = NULL;
+	struct clk *clk;
+
+	/* Clock sources */
+	clk = clk_reg_prcmu_gate("soc0_pll", NULL, PRCMU_PLLSOC0,
+				CLK_IS_ROOT|CLK_IGNORE_UNUSED);
+	clk_register_clkdev(clk, "soc0_pll", NULL);
+
+	clk = clk_reg_prcmu_gate("soc1_pll", NULL, PRCMU_PLLSOC1,
+				CLK_IS_ROOT|CLK_IGNORE_UNUSED);
+	clk_register_clkdev(clk, "soc1_pll", NULL);
+
+	clk = clk_reg_prcmu_gate("ddr_pll", NULL, PRCMU_PLLDDR,
+				CLK_IS_ROOT|CLK_IGNORE_UNUSED);
+	clk_register_clkdev(clk, "ddr_pll", NULL);
+
+	/* FIXME: Add sys, ulp and int clocks here. */
+
+	clk = clk_register_fixed_rate(NULL, "rtc32k", "NULL",
+				CLK_IS_ROOT|CLK_IGNORE_UNUSED,
+				32768);
+	clk_register_clkdev(clk, "clk32k", NULL);
+	clk_register_clkdev(clk, NULL, "rtc-pl031");
+
+	/* PRCMU clocks */
+	fw_version = prcmu_get_fw_version();
+	if (fw_version != NULL) {
+		switch (fw_version->project) {
+		case PRCMU_FW_PROJECT_U8500_C2:
+		case PRCMU_FW_PROJECT_U8520:
+		case PRCMU_FW_PROJECT_U8420:
+			sgaclk_parent = "soc0_pll";
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (sgaclk_parent)
+		clk = clk_reg_prcmu_gate("sgclk", sgaclk_parent,
+					PRCMU_SGACLK, 0);
+	else
+		clk = clk_reg_prcmu_gate("sgclk", NULL,
+					PRCMU_SGACLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "mali");
+
+	clk = clk_reg_prcmu_gate("uartclk", NULL, PRCMU_UARTCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "UART");
+
+	clk = clk_reg_prcmu_gate("msp02clk", NULL, PRCMU_MSP02CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "MSP02");
+
+	clk = clk_reg_prcmu_gate("msp1clk", NULL, PRCMU_MSP1CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "MSP1");
+
+	clk = clk_reg_prcmu_gate("i2cclk", NULL, PRCMU_I2CCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "I2C");
+
+	clk = clk_reg_prcmu_gate("slimclk", NULL, PRCMU_SLIMCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "slim");
+
+	clk = clk_reg_prcmu_gate("per1clk", NULL, PRCMU_PER1CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH1");
+
+	clk = clk_reg_prcmu_gate("per2clk", NULL, PRCMU_PER2CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH2");
+
+	clk = clk_reg_prcmu_gate("per3clk", NULL, PRCMU_PER3CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH3");
+
+	clk = clk_reg_prcmu_gate("per5clk", NULL, PRCMU_PER5CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH5");
+
+	clk = clk_reg_prcmu_gate("per6clk", NULL, PRCMU_PER6CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH6");
+
+	clk = clk_reg_prcmu_gate("per7clk", NULL, PRCMU_PER7CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "PERIPH7");
+
+	clk = clk_reg_prcmu_scalable("lcdclk", NULL, PRCMU_LCDCLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "lcd");
+	clk_register_clkdev(clk, "lcd", "mcde");
+
+	clk = clk_reg_prcmu_opp_gate("bmlclk", NULL, PRCMU_BMLCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "bml");
+
+	clk = clk_reg_prcmu_scalable("hsitxclk", NULL, PRCMU_HSITXCLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcmu_scalable("hsirxclk", NULL, PRCMU_HSIRXCLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcmu_scalable("hdmiclk", NULL, PRCMU_HDMICLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "hdmi");
+	clk_register_clkdev(clk, "hdmi", "mcde");
+
+	clk = clk_reg_prcmu_gate("apeatclk", NULL, PRCMU_APEATCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "apeat");
+
+	clk = clk_reg_prcmu_gate("apetraceclk", NULL, PRCMU_APETRACECLK,
+				CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "apetrace");
+
+	clk = clk_reg_prcmu_gate("mcdeclk", NULL, PRCMU_MCDECLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "mcde");
+	clk_register_clkdev(clk, "mcde", "mcde");
+	clk_register_clkdev(clk, "dsisys", "dsilink.0");
+	clk_register_clkdev(clk, "dsisys", "dsilink.1");
+	clk_register_clkdev(clk, "dsisys", "dsilink.2");
+
+	clk = clk_reg_prcmu_opp_gate("ipi2cclk", NULL, PRCMU_IPI2CCLK,
+				CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "ipi2");
+
+	clk = clk_reg_prcmu_gate("dsialtclk", NULL, PRCMU_DSIALTCLK,
+				CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "dsialt");
+
+	clk = clk_reg_prcmu_gate("dmaclk", NULL, PRCMU_DMACLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "dma40.0");
+
+	clk = clk_reg_prcmu_gate("b2r2clk", NULL, PRCMU_B2R2CLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "b2r2");
+	clk_register_clkdev(clk, NULL, "b2r2_core");
+	clk_register_clkdev(clk, NULL, "U8500-B2R2.0");
+
+	clk = clk_reg_prcmu_scalable("tvclk", NULL, PRCMU_TVCLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "tv");
+	clk_register_clkdev(clk, "tv", "mcde");
+
+	clk = clk_reg_prcmu_gate("sspclk", NULL, PRCMU_SSPCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "SSP");
+
+	clk = clk_reg_prcmu_gate("rngclk", NULL, PRCMU_RNGCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "rngclk");
+
+	clk = clk_reg_prcmu_gate("uiccclk", NULL, PRCMU_UICCCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "uicc");
+
+	/*
+	 * FIXME: The MTU clocks might need some kind of "parent muxed join"
+	 * and these have no K-clocks. For now, we ignore the missing
+	 * connection to the corresponding P-clocks, p6_mtu0_clk and
+	 * p6_mtu1_clk. Instead timclk is used which is the valid parent.
+	 */
+	clk = clk_reg_prcmu_gate("timclk", NULL, PRCMU_TIMCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "mtu0");
+	clk_register_clkdev(clk, NULL, "mtu1");
+
+	clk = clk_reg_prcmu_gate("sdmmcclk", NULL, PRCMU_SDMMCCLK, CLK_IS_ROOT);
+	clk_register_clkdev(clk, NULL, "sdmmc");
+
+
+	clk = clk_reg_prcmu_scalable("dsi_pll", "hdmiclk",
+				PRCMU_PLLDSI, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsihs2", "mcde");
+	clk_register_clkdev(clk, "dsihs2", "dsilink.2");
+
+
+	clk = clk_reg_prcmu_scalable("dsi0clk", "dsi_pll",
+				PRCMU_DSI0CLK, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsihs0", "mcde");
+	clk_register_clkdev(clk, "dsihs0", "dsilink.0");
+
+	clk = clk_reg_prcmu_scalable("dsi1clk", "dsi_pll",
+				PRCMU_DSI1CLK, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsihs1", "mcde");
+	clk_register_clkdev(clk, "dsihs1", "dsilink.1");
+
+	clk = clk_reg_prcmu_scalable("dsi0escclk", "tvclk",
+				PRCMU_DSI0ESCCLK, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsilp0", "dsilink.0");
+	clk_register_clkdev(clk, "dsilp0", "mcde");
+
+	clk = clk_reg_prcmu_scalable("dsi1escclk", "tvclk",
+				PRCMU_DSI1ESCCLK, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsilp1", "dsilink.1");
+	clk_register_clkdev(clk, "dsilp1", "mcde");
+
+	clk = clk_reg_prcmu_scalable("dsi2escclk", "tvclk",
+				PRCMU_DSI2ESCCLK, 0, CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, "dsilp2", "dsilink.2");
+	clk_register_clkdev(clk, "dsilp2", "mcde");
+
+	clk = clk_reg_prcmu_rate("smp_twd", NULL, PRCMU_ARMSS,
+				CLK_IS_ROOT|CLK_GET_RATE_NOCACHE|
+				CLK_IGNORE_UNUSED);
+	clk_register_clkdev(clk, NULL, "smp_twd");
+
+	/*
+	 * FIXME: Add special handled PRCMU clocks here:
+	 * 1. clk_arm, use PRCMU_ARMCLK.
+	 * 2. clkout0yuv, use PRCMU as parent + need regulator + pinctrl.
+	 * 3. ab9540_clkout1yuv, see clkout0yuv
+	 */
+
+	/* PRCC P-clocks */
+	clk = clk_reg_prcc_pclk("p1_pclk0", "per1clk", U8500_CLKRST1_BASE,
+				BIT(0), 0);
+	clk_register_clkdev(clk, "apb_pclk", "uart0");
+
+	clk = clk_reg_prcc_pclk("p1_pclk1", "per1clk", U8500_CLKRST1_BASE,
+				BIT(1), 0);
+	clk_register_clkdev(clk, "apb_pclk", "uart1");
+
+	clk = clk_reg_prcc_pclk("p1_pclk2", "per1clk", U8500_CLKRST1_BASE,
+				BIT(2), 0);
+	clk = clk_reg_prcc_pclk("p1_pclk3", "per1clk", U8500_CLKRST1_BASE,
+				BIT(3), 0);
+	clk = clk_reg_prcc_pclk("p1_pclk4", "per1clk", U8500_CLKRST1_BASE,
+				BIT(4), 0);
+
+	clk = clk_reg_prcc_pclk("p1_pclk5", "per1clk", U8500_CLKRST1_BASE,
+				BIT(5), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi0");
+
+	clk = clk_reg_prcc_pclk("p1_pclk6", "per1clk", U8500_CLKRST1_BASE,
+				BIT(6), 0);
+
+	clk = clk_reg_prcc_pclk("p1_pclk7", "per1clk", U8500_CLKRST1_BASE,
+				BIT(7), 0);
+	clk_register_clkdev(clk, NULL, "spi3");
+
+	clk = clk_reg_prcc_pclk("p1_pclk8", "per1clk", U8500_CLKRST1_BASE,
+				BIT(8), 0);
+
+	clk = clk_reg_prcc_pclk("p1_pclk9", "per1clk", U8500_CLKRST1_BASE,
+				BIT(9), 0);
+	clk_register_clkdev(clk, NULL, "gpio.0");
+	clk_register_clkdev(clk, NULL, "gpio.1");
+	clk_register_clkdev(clk, NULL, "gpioblock0");
+
+	clk = clk_reg_prcc_pclk("p1_pclk10", "per1clk", U8500_CLKRST1_BASE,
+				BIT(10), 0);
+	clk = clk_reg_prcc_pclk("p1_pclk11", "per1clk", U8500_CLKRST1_BASE,
+				BIT(11), 0);
+
+	clk = clk_reg_prcc_pclk("p2_pclk0", "per2clk", U8500_CLKRST2_BASE,
+				BIT(0), 0);
+
+	clk = clk_reg_prcc_pclk("p2_pclk1", "per2clk", U8500_CLKRST2_BASE,
+				BIT(1), 0);
+	clk_register_clkdev(clk, NULL, "spi2");
+
+	clk = clk_reg_prcc_pclk("p2_pclk2", "per2clk", U8500_CLKRST2_BASE,
+				BIT(2), 0);
+	clk_register_clkdev(clk, NULL, "spi1");
+
+	clk = clk_reg_prcc_pclk("p2_pclk3", "per2clk", U8500_CLKRST2_BASE,
+				BIT(3), 0);
+	clk_register_clkdev(clk, NULL, "pwl");
+
+	clk = clk_reg_prcc_pclk("p2_pclk4", "per2clk", U8500_CLKRST2_BASE,
+				BIT(4), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi4");
+
+	clk = clk_reg_prcc_pclk("p2_pclk5", "per2clk", U8500_CLKRST2_BASE,
+				BIT(5), 0);
+
+	clk = clk_reg_prcc_pclk("p2_pclk6", "per2clk", U8500_CLKRST2_BASE,
+				BIT(6), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi1");
+
+
+	clk = clk_reg_prcc_pclk("p2_pclk7", "per2clk", U8500_CLKRST2_BASE,
+				BIT(7), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi3");
+
+	clk = clk_reg_prcc_pclk("p2_pclk8", "per2clk", U8500_CLKRST2_BASE,
+				BIT(8), 0);
+	clk_register_clkdev(clk, NULL, "spi0");
+
+	clk = clk_reg_prcc_pclk("p2_pclk9", "per2clk", U8500_CLKRST2_BASE,
+				BIT(9), 0);
+	clk_register_clkdev(clk, "hsir_hclk", "ste_hsi.0");
+
+	clk = clk_reg_prcc_pclk("p2_pclk10", "per2clk", U8500_CLKRST2_BASE,
+				BIT(10), 0);
+	clk_register_clkdev(clk, "hsit_hclk", "ste_hsi.0");
+
+	clk = clk_reg_prcc_pclk("p2_pclk11", "per2clk", U8500_CLKRST2_BASE,
+				BIT(11), 0);
+	clk_register_clkdev(clk, NULL, "gpio.6");
+	clk_register_clkdev(clk, NULL, "gpio.7");
+	clk_register_clkdev(clk, NULL, "gpioblock1");
+
+	clk = clk_reg_prcc_pclk("p2_pclk12", "per2clk", U8500_CLKRST2_BASE,
+				BIT(11), 0);
+
+	clk = clk_reg_prcc_pclk("p3_pclk0", "per3clk", U8500_CLKRST3_BASE,
+				BIT(0), 0);
+	clk_register_clkdev(clk, NULL, "fsmc");
+
+	clk = clk_reg_prcc_pclk("p3_pclk1", "per3clk", U8500_CLKRST3_BASE,
+				BIT(1), 0);
+	clk = clk_reg_prcc_pclk("p3_pclk2", "per3clk", U8500_CLKRST3_BASE,
+				BIT(2), 0);
+	clk = clk_reg_prcc_pclk("p3_pclk3", "per3clk", U8500_CLKRST3_BASE,
+				BIT(3), 0);
+
+	clk = clk_reg_prcc_pclk("p3_pclk4", "per3clk", U8500_CLKRST3_BASE,
+				BIT(4), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi2");
+
+	clk = clk_reg_prcc_pclk("p3_pclk5", "per3clk", U8500_CLKRST3_BASE,
+				BIT(5), 0);
+
+	clk = clk_reg_prcc_pclk("p3_pclk6", "per3clk", U8500_CLKRST3_BASE,
+				BIT(6), 0);
+	clk_register_clkdev(clk, "apb_pclk", "uart2");
+
+	clk = clk_reg_prcc_pclk("p3_pclk7", "per3clk", U8500_CLKRST3_BASE,
+				BIT(7), 0);
+	clk_register_clkdev(clk, "apb_pclk", "sdi5");
+
+	clk = clk_reg_prcc_pclk("p3_pclk8", "per3clk", U8500_CLKRST3_BASE,
+				BIT(8), 0);
+	clk_register_clkdev(clk, NULL, "gpio.2");
+	clk_register_clkdev(clk, NULL, "gpio.3");
+	clk_register_clkdev(clk, NULL, "gpio.4");
+	clk_register_clkdev(clk, NULL, "gpio.5");
+	clk_register_clkdev(clk, NULL, "gpioblock2");
+
+	clk = clk_reg_prcc_pclk("p5_pclk0", "per5clk", U8500_CLKRST5_BASE,
+				BIT(0), 0);
+	clk_register_clkdev(clk, "usb", "musb-ux500.0");
+
+	clk = clk_reg_prcc_pclk("p5_pclk1", "per5clk", U8500_CLKRST5_BASE,
+				BIT(1), 0);
+	clk_register_clkdev(clk, NULL, "gpio.8");
+	clk_register_clkdev(clk, NULL, "gpioblock3");
+
+	clk = clk_reg_prcc_pclk("p6_pclk0", "per6clk", U8500_CLKRST6_BASE,
+				BIT(0), 0);
+
+	clk = clk_reg_prcc_pclk("p6_pclk1", "per6clk", U8500_CLKRST6_BASE,
+				BIT(1), 0);
+	clk_register_clkdev(clk, NULL, "cryp0");
+	clk_register_clkdev(clk, NULL, "cryp1");
+
+	clk = clk_reg_prcc_pclk("p6_pclk2", "per6clk", U8500_CLKRST6_BASE,
+				BIT(2), 0);
+	clk_register_clkdev(clk, NULL, "hash0");
+
+	clk = clk_reg_prcc_pclk("p6_pclk3", "per6clk", U8500_CLKRST6_BASE,
+				BIT(3), 0);
+	clk_register_clkdev(clk, NULL, "pka");
+
+	clk = clk_reg_prcc_pclk("p6_pclk4", "per6clk", U8500_CLKRST6_BASE,
+				BIT(4), 0);
+	clk_register_clkdev(clk, NULL, "hash1");
+
+	clk = clk_reg_prcc_pclk("p6_pclk5", "per6clk", U8500_CLKRST6_BASE,
+				BIT(5), 0);
+	clk_register_clkdev(clk, NULL, "cfgreg");
+
+	clk = clk_reg_prcc_pclk("p6_pclk6", "per6clk", U8500_CLKRST6_BASE,
+				BIT(6), 0);
+	clk = clk_reg_prcc_pclk("p6_pclk7", "per6clk", U8500_CLKRST6_BASE,
+				BIT(7), 0);
+
+	/* PRCC K-clocks
+	 *
+	 * FIXME: Some drivers requires PERPIH[n| to be automatically enabled
+	 * by enabling just the K-clock, even if it is not a valid parent to
+	 * the K-clock. Until drivers get fixed we might need some kind of
+	 * "parent muxed join".
+	 */
+
+	/* Periph1 */
+	clk = clk_reg_prcc_kclk("p1_uart0_kclk", "uartclk",
+			U8500_CLKRST1_BASE, BIT(0), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "uart0");
+
+	clk = clk_reg_prcc_kclk("p1_uart1_kclk", "uartclk",
+			U8500_CLKRST1_BASE, BIT(1), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "uart1");
+
+	clk = clk_reg_prcc_kclk("p1_i2c1_kclk", "i2cclk",
+			U8500_CLKRST1_BASE, BIT(2), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p1_msp0_kclk", "msp02clk",
+			U8500_CLKRST1_BASE, BIT(3), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p1_msp1_kclk", "msp1clk",
+			U8500_CLKRST1_BASE, BIT(4), CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcc_kclk("p1_sdi0_kclk", "sdmmcclk",
+			U8500_CLKRST1_BASE, BIT(5), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi0");
+
+	clk = clk_reg_prcc_kclk("p1_i2c2_kclk", "i2cclk",
+			U8500_CLKRST1_BASE, BIT(6), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p1_slimbus0_kclk", "slimclk",
+			U8500_CLKRST1_BASE, BIT(3), CLK_SET_RATE_GATE);
+	/* FIXME: Redefinition of BIT(3). */
+	clk = clk_reg_prcc_kclk("p1_i2c4_kclk", "i2cclk",
+			U8500_CLKRST1_BASE, BIT(9), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p1_msp3_kclk", "msp1clk",
+			U8500_CLKRST1_BASE, BIT(10), CLK_SET_RATE_GATE);
+
+	/* Periph2 */
+	clk = clk_reg_prcc_kclk("p2_i2c3_kclk", "i2cclk",
+			U8500_CLKRST2_BASE, BIT(0), CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcc_kclk("p2_sdi4_kclk", "sdmmcclk",
+			U8500_CLKRST2_BASE, BIT(2), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi4");
+
+	clk = clk_reg_prcc_kclk("p2_msp2_kclk", "msp02clk",
+			U8500_CLKRST2_BASE, BIT(3), CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcc_kclk("p2_sdi1_kclk", "sdmmcclk",
+			U8500_CLKRST2_BASE, BIT(4), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi1");
+
+	clk = clk_reg_prcc_kclk("p2_sdi3_kclk", "sdmmcclk",
+			U8500_CLKRST2_BASE, BIT(5), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi3");
+
+	/* Note that rate is received from parent. */
+	clk = clk_reg_prcc_kclk("p2_ssirx_kclk", "hsirxclk",
+			U8500_CLKRST2_BASE, BIT(6),
+			CLK_SET_RATE_GATE|CLK_SET_RATE_PARENT);
+	clk = clk_reg_prcc_kclk("p2_ssitx_kclk", "hsitxclk",
+			U8500_CLKRST2_BASE, BIT(7),
+			CLK_SET_RATE_GATE|CLK_SET_RATE_PARENT);
+
+	/* Periph3 */
+	clk = clk_reg_prcc_kclk("p3_ssp0_kclk", "sspclk",
+			U8500_CLKRST3_BASE, BIT(1), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p3_ssp1_kclk", "sspclk",
+			U8500_CLKRST3_BASE, BIT(2), CLK_SET_RATE_GATE);
+	clk = clk_reg_prcc_kclk("p3_i2c0_kclk", "i2cclk",
+			U8500_CLKRST3_BASE, BIT(3), CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcc_kclk("p3_sdi2_kclk", "sdmmcclk",
+			U8500_CLKRST3_BASE, BIT(4), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi2");
+
+	clk = clk_reg_prcc_kclk("p3_ske_kclk", "rtc32k",
+			U8500_CLKRST3_BASE, BIT(5), CLK_SET_RATE_GATE);
+
+	clk = clk_reg_prcc_kclk("p3_uart2_kclk", "uartclk",
+			U8500_CLKRST3_BASE, BIT(6), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "uart2");
+
+	clk = clk_reg_prcc_kclk("p3_sdi5_kclk", "sdmmcclk",
+			U8500_CLKRST3_BASE, BIT(7), CLK_SET_RATE_GATE);
+	clk_register_clkdev(clk, NULL, "sdi5");
+
+	/* Periph6 */
+	clk = clk_reg_prcc_kclk("p3_rng_kclk", "rngclk",
+			U8500_CLKRST6_BASE, BIT(0), CLK_SET_RATE_GATE);
+
+}
diff --git a/drivers/clk/ux500/u8540_clk.c b/drivers/clk/ux500/u8540_clk.c
new file mode 100644
index 000000000000..10adfd2ead21
--- /dev/null
+++ b/drivers/clk/ux500/u8540_clk.c
@@ -0,0 +1,21 @@
+/*
+ * Clock definitions for u8540 platform.
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/platform_data/clk-ux500.h>
+
+#include "clk.h"
+
+void u8540_clk_init(void)
+{
+	/* register clocks here */
+}
diff --git a/drivers/clk/ux500/u9540_clk.c b/drivers/clk/ux500/u9540_clk.c
new file mode 100644
index 000000000000..dbc0191e16c8
--- /dev/null
+++ b/drivers/clk/ux500/u9540_clk.c
@@ -0,0 +1,21 @@
+/*
+ * Clock definitions for u9540 platform.
+ *
+ * Copyright (C) 2012 ST-Ericsson SA
+ * Author: Ulf Hansson <ulf.hansson@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/platform_data/clk-ux500.h>
+
+#include "clk.h"
+
+void u9540_clk_init(void)
+{
+	/* register clocks here */
+}
diff --git a/drivers/clk/versatile/Makefile b/drivers/clk/versatile/Makefile
index 50cf6a2ee693..c0a0f6478798 100644
--- a/drivers/clk/versatile/Makefile
+++ b/drivers/clk/versatile/Makefile
@@ -1,3 +1,4 @@
 # Makefile for Versatile-specific clocks
 obj-$(CONFIG_ICST)		+= clk-icst.o
 obj-$(CONFIG_ARCH_INTEGRATOR)	+= clk-integrator.o
+obj-$(CONFIG_ARCH_REALVIEW)	+= clk-realview.o
diff --git a/drivers/clk/versatile/clk-realview.c b/drivers/clk/versatile/clk-realview.c
new file mode 100644
index 000000000000..e21a99cef378
--- /dev/null
+++ b/drivers/clk/versatile/clk-realview.c
@@ -0,0 +1,114 @@
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+
+#include <mach/hardware.h>
+#include <mach/platform.h>
+
+#include "clk-icst.h"
+
+/*
+ * Implementation of the ARM RealView clock trees.
+ */
+
+static void __iomem *sys_lock;
+static void __iomem *sys_vcoreg;
+
+/**
+ * realview_oscvco_get() - get ICST OSC settings for the RealView
+ */
+static struct icst_vco realview_oscvco_get(void)
+{
+	u32 val;
+	struct icst_vco vco;
+
+	val = readl(sys_vcoreg);
+	vco.v = val & 0x1ff;
+	vco.r = (val >> 9) & 0x7f;
+	vco.s = (val >> 16) & 03;
+	return vco;
+}
+
+static void realview_oscvco_set(struct icst_vco vco)
+{
+	u32 val;
+
+	val = readl(sys_vcoreg) & ~0x7ffff;
+	val |= vco.v | (vco.r << 9) | (vco.s << 16);
+
+	/* This magic unlocks the CM VCO so it can be controlled */
+	writel(0xa05f, sys_lock);
+	writel(val, sys_vcoreg);
+	/* This locks the CM again */
+	writel(0, sys_lock);
+}
+
+static const struct icst_params realview_oscvco_params = {
+	.ref		= 24000000,
+	.vco_max	= ICST307_VCO_MAX,
+	.vco_min	= ICST307_VCO_MIN,
+	.vd_min		= 4 + 8,
+	.vd_max		= 511 + 8,
+	.rd_min		= 1 + 2,
+	.rd_max		= 127 + 2,
+	.s2div		= icst307_s2div,
+	.idx2s		= icst307_idx2s,
+};
+
+static const struct clk_icst_desc __initdata realview_icst_desc = {
+	.params = &realview_oscvco_params,
+	.getvco = realview_oscvco_get,
+	.setvco = realview_oscvco_set,
+};
+
+/*
+ * realview_clk_init() - set up the RealView clock tree
+ */
+void __init realview_clk_init(void __iomem *sysbase, bool is_pb1176)
+{
+	struct clk *clk;
+
+	sys_lock = sysbase + REALVIEW_SYS_LOCK_OFFSET;
+	if (is_pb1176)
+		sys_vcoreg = sysbase + REALVIEW_SYS_OSC0_OFFSET;
+	else
+		sys_vcoreg = sysbase + REALVIEW_SYS_OSC4_OFFSET;
+
+
+	/* APB clock dummy */
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	/* 24 MHz clock */
+	clk = clk_register_fixed_rate(NULL, "clk24mhz", NULL, CLK_IS_ROOT,
+				24000000);
+	clk_register_clkdev(clk, NULL, "dev:uart0");
+	clk_register_clkdev(clk, NULL, "dev:uart1");
+	clk_register_clkdev(clk, NULL, "dev:uart2");
+	clk_register_clkdev(clk, NULL, "fpga:kmi0");
+	clk_register_clkdev(clk, NULL, "fpga:kmi1");
+	clk_register_clkdev(clk, NULL, "fpga:mmc0");
+	clk_register_clkdev(clk, NULL, "dev:ssp0");
+	if (is_pb1176) {
+		/*
+		 * UART3 is on the dev chip in PB1176
+		 * UART4 only exists in PB1176
+		 */
+		clk_register_clkdev(clk, NULL, "dev:uart3");
+		clk_register_clkdev(clk, NULL, "dev:uart4");
+	} else
+		clk_register_clkdev(clk, NULL, "fpga:uart3");
+
+
+	/* 1 MHz clock */
+	clk = clk_register_fixed_rate(NULL, "clk1mhz", NULL, CLK_IS_ROOT,
+				      1000000);
+	clk_register_clkdev(clk, NULL, "sp804");
+
+	/* ICST VCO clock */
+	clk = icst_clk_register(NULL, &realview_icst_desc);
+	clk_register_clkdev(clk, NULL, "dev:clcd");
+	clk_register_clkdev(clk, NULL, "issp:clcd");
+}
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index d53cd0afc200..6a78073c3808 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -35,3 +35,8 @@ config CLKSRC_DBX500_PRCMU_SCHED_CLOCK
 	default y
 	help
 	  Use the always on PRCMU Timer as sched_clock
+
+config CLKSRC_ARM_GENERIC
+	def_bool y if ARM64
+	help
+	  This option enables support for the ARM generic timer.
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index b65d0c56ab35..65919901a301 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_DW_APB_TIMER)	+= dw_apb_timer.o
 obj-$(CONFIG_DW_APB_TIMER_OF)	+= dw_apb_timer_of.o
 obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
 obj-$(CONFIG_ARMADA_370_XP_TIMER)	+= time-armada-370-xp.o
+obj-$(CONFIG_CLKSRC_ARM_GENERIC)	+= arm_generic.o
diff --git a/drivers/clocksource/arm_generic.c b/drivers/clocksource/arm_generic.c
new file mode 100644
index 000000000000..c4d9f9566c64
--- /dev/null
+++ b/drivers/clocksource/arm_generic.c
@@ -0,0 +1,232 @@
+/*
+ * Generic timers support
+ *
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/smp.h>
+#include <linux/cpu.h>
+#include <linux/jiffies.h>
+#include <linux/interrupt.h>
+#include <linux/clockchips.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+
+#include <clocksource/arm_generic.h>
+
+#include <asm/arm_generic.h>
+
+static u32 arch_timer_rate;
+static u64 sched_clock_mult __read_mostly;
+static DEFINE_PER_CPU(struct clock_event_device, arch_timer_evt);
+static int arch_timer_ppi;
+
+static irqreturn_t arch_timer_handle_irq(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+	unsigned long ctrl;
+
+	ctrl = arch_timer_reg_read(ARCH_TIMER_REG_CTRL);
+	if (ctrl & ARCH_TIMER_CTRL_ISTATUS) {
+		ctrl |= ARCH_TIMER_CTRL_IMASK;
+		arch_timer_reg_write(ARCH_TIMER_REG_CTRL, ctrl);
+		evt->event_handler(evt);
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static void arch_timer_stop(void)
+{
+	unsigned long ctrl;
+
+	ctrl = arch_timer_reg_read(ARCH_TIMER_REG_CTRL);
+	ctrl &= ~ARCH_TIMER_CTRL_ENABLE;
+	arch_timer_reg_write(ARCH_TIMER_REG_CTRL, ctrl);
+}
+
+static void arch_timer_set_mode(enum clock_event_mode mode,
+				struct clock_event_device *clk)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+		arch_timer_stop();
+		break;
+	default:
+		break;
+	}
+}
+
+static int arch_timer_set_next_event(unsigned long evt,
+				     struct clock_event_device *unused)
+{
+	unsigned long ctrl;
+
+	ctrl = arch_timer_reg_read(ARCH_TIMER_REG_CTRL);
+	ctrl |= ARCH_TIMER_CTRL_ENABLE;
+	ctrl &= ~ARCH_TIMER_CTRL_IMASK;
+
+	arch_timer_reg_write(ARCH_TIMER_REG_TVAL, evt);
+	arch_timer_reg_write(ARCH_TIMER_REG_CTRL, ctrl);
+
+	return 0;
+}
+
+static void __cpuinit arch_timer_setup(struct clock_event_device *clk)
+{
+	/* Let's make sure the timer is off before doing anything else */
+	arch_timer_stop();
+
+	clk->features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_C3STOP;
+	clk->name = "arch_sys_timer";
+	clk->rating = 400;
+	clk->set_mode = arch_timer_set_mode;
+	clk->set_next_event = arch_timer_set_next_event;
+	clk->irq = arch_timer_ppi;
+	clk->cpumask = cpumask_of(smp_processor_id());
+
+	clockevents_config_and_register(clk, arch_timer_rate,
+					0xf, 0x7fffffff);
+
+	enable_percpu_irq(clk->irq, 0);
+
+	/* Ensure the physical counter is visible to userspace for the vDSO. */
+	arch_counter_enable_user_access();
+}
+
+static void __init arch_timer_calibrate(void)
+{
+	if (arch_timer_rate == 0) {
+		arch_timer_reg_write(ARCH_TIMER_REG_CTRL, 0);
+		arch_timer_rate = arch_timer_reg_read(ARCH_TIMER_REG_FREQ);
+
+		/* Check the timer frequency. */
+		if (arch_timer_rate == 0)
+			panic("Architected timer frequency is set to zero.\n"
+			      "You must set this in your .dts file\n");
+	}
+
+	/* Cache the sched_clock multiplier to save a divide in the hot path. */
+
+	sched_clock_mult = NSEC_PER_SEC / arch_timer_rate;
+
+	pr_info("Architected local timer running at %u.%02uMHz.\n",
+		 arch_timer_rate / 1000000, (arch_timer_rate / 10000) % 100);
+}
+
+static cycle_t arch_counter_read(struct clocksource *cs)
+{
+	return arch_counter_get_cntpct();
+}
+
+static struct clocksource clocksource_counter = {
+	.name	= "arch_sys_counter",
+	.rating	= 400,
+	.read	= arch_counter_read,
+	.mask	= CLOCKSOURCE_MASK(56),
+	.flags	= (CLOCK_SOURCE_IS_CONTINUOUS | CLOCK_SOURCE_VALID_FOR_HRES),
+};
+
+int read_current_timer(unsigned long *timer_value)
+{
+	*timer_value = arch_counter_get_cntpct();
+	return 0;
+}
+
+unsigned long long notrace sched_clock(void)
+{
+	return arch_counter_get_cntvct() * sched_clock_mult;
+}
+
+static int __cpuinit arch_timer_cpu_notify(struct notifier_block *self,
+					   unsigned long action, void *hcpu)
+{
+	int cpu = (long)hcpu;
+	struct clock_event_device *clk = per_cpu_ptr(&arch_timer_evt, cpu);
+
+	switch(action) {
+	case CPU_STARTING:
+	case CPU_STARTING_FROZEN:
+		arch_timer_setup(clk);
+		break;
+
+	case CPU_DYING:
+	case CPU_DYING_FROZEN:
+		pr_debug("arch_timer_teardown disable IRQ%d cpu #%d\n",
+			 clk->irq, cpu);
+		disable_percpu_irq(clk->irq);
+		arch_timer_set_mode(CLOCK_EVT_MODE_UNUSED, clk);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __cpuinitdata arch_timer_cpu_nb = {
+	.notifier_call = arch_timer_cpu_notify,
+};
+
+static const struct of_device_id arch_timer_of_match[] __initconst = {
+	{ .compatible = "arm,armv8-timer" },
+	{},
+};
+
+int __init arm_generic_timer_init(void)
+{
+	struct device_node *np;
+	int err;
+	u32 freq;
+
+	np = of_find_matching_node(NULL, arch_timer_of_match);
+	if (!np) {
+		pr_err("arch_timer: can't find DT node\n");
+		return -ENODEV;
+	}
+
+	/* Try to determine the frequency from the device tree or CNTFRQ */
+	if (!of_property_read_u32(np, "clock-frequency", &freq))
+		arch_timer_rate = freq;
+	arch_timer_calibrate();
+
+	arch_timer_ppi = irq_of_parse_and_map(np, 0);
+	pr_info("arch_timer: found %s irq %d\n", np->name, arch_timer_ppi);
+
+	err = request_percpu_irq(arch_timer_ppi, arch_timer_handle_irq,
+				 np->name, &arch_timer_evt);
+	if (err) {
+		pr_err("arch_timer: can't register interrupt %d (%d)\n",
+		       arch_timer_ppi, err);
+		return err;
+	}
+
+	clocksource_register_hz(&clocksource_counter, arch_timer_rate);
+
+	/* Calibrate the delay loop directly */
+	lpj_fine = arch_timer_rate / HZ;
+
+	/* Immediately configure the timer on the boot CPU */
+	arch_timer_setup(per_cpu_ptr(&arch_timer_evt, smp_processor_id()));
+
+	register_cpu_notifier(&arch_timer_cpu_nb);
+
+	return 0;
+}
diff --git a/drivers/cpufreq/powernow-k8.c b/drivers/cpufreq/powernow-k8.c
index c0e816468e30..1a40935c85fd 100644
--- a/drivers/cpufreq/powernow-k8.c
+++ b/drivers/cpufreq/powernow-k8.c
@@ -35,7 +35,6 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/cpumask.h>
-#include <linux/sched.h>	/* for current / set_cpus_allowed() */
 #include <linux/io.h>
 #include <linux/delay.h>
 
@@ -1139,16 +1138,23 @@ static int transition_frequency_pstate(struct powernow_k8_data *data,
 	return res;
 }
 
-/* Driver entry point to switch to the target frequency */
-static int powernowk8_target(struct cpufreq_policy *pol,
-		unsigned targfreq, unsigned relation)
+struct powernowk8_target_arg {
+	struct cpufreq_policy		*pol;
+	unsigned			targfreq;
+	unsigned			relation;
+};
+
+static long powernowk8_target_fn(void *arg)
 {
-	cpumask_var_t oldmask;
+	struct powernowk8_target_arg *pta = arg;
+	struct cpufreq_policy *pol = pta->pol;
+	unsigned targfreq = pta->targfreq;
+	unsigned relation = pta->relation;
 	struct powernow_k8_data *data = per_cpu(powernow_data, pol->cpu);
 	u32 checkfid;
 	u32 checkvid;
 	unsigned int newstate;
-	int ret = -EIO;
+	int ret;
 
 	if (!data)
 		return -EINVAL;
@@ -1156,29 +1162,16 @@ static int powernowk8_target(struct cpufreq_policy *pol,
 	checkfid = data->currfid;
 	checkvid = data->currvid;
 
-	/* only run on specific CPU from here on. */
-	/* This is poor form: use a workqueue or smp_call_function_single */
-	if (!alloc_cpumask_var(&oldmask, GFP_KERNEL))
-		return -ENOMEM;
-
-	cpumask_copy(oldmask, tsk_cpus_allowed(current));
-	set_cpus_allowed_ptr(current, cpumask_of(pol->cpu));
-
-	if (smp_processor_id() != pol->cpu) {
-		printk(KERN_ERR PFX "limiting to cpu %u failed\n", pol->cpu);
-		goto err_out;
-	}
-
 	if (pending_bit_stuck()) {
 		printk(KERN_ERR PFX "failing targ, change pending bit set\n");
-		goto err_out;
+		return -EIO;
 	}
 
 	pr_debug("targ: cpu %d, %d kHz, min %d, max %d, relation %d\n",
 		pol->cpu, targfreq, pol->min, pol->max, relation);
 
 	if (query_current_values_with_pending_wait(data))
-		goto err_out;
+		return -EIO;
 
 	if (cpu_family != CPU_HW_PSTATE) {
 		pr_debug("targ: curr fid 0x%x, vid 0x%x\n",
@@ -1196,7 +1189,7 @@ static int powernowk8_target(struct cpufreq_policy *pol,
 
 	if (cpufreq_frequency_table_target(pol, data->powernow_table,
 				targfreq, relation, &newstate))
-		goto err_out;
+		return -EIO;
 
 	mutex_lock(&fidvid_mutex);
 
@@ -1209,9 +1202,8 @@ static int powernowk8_target(struct cpufreq_policy *pol,
 		ret = transition_frequency_fidvid(data, newstate);
 	if (ret) {
 		printk(KERN_ERR PFX "transition frequency failed\n");
-		ret = 1;
 		mutex_unlock(&fidvid_mutex);
-		goto err_out;
+		return 1;
 	}
 	mutex_unlock(&fidvid_mutex);
 
@@ -1220,12 +1212,25 @@ static int powernowk8_target(struct cpufreq_policy *pol,
 				data->powernow_table[newstate].index);
 	else
 		pol->cur = find_khz_freq_from_fid(data->currfid);
-	ret = 0;
 
-err_out:
-	set_cpus_allowed_ptr(current, oldmask);
-	free_cpumask_var(oldmask);
-	return ret;
+	return 0;
+}
+
+/* Driver entry point to switch to the target frequency */
+static int powernowk8_target(struct cpufreq_policy *pol,
+		unsigned targfreq, unsigned relation)
+{
+	struct powernowk8_target_arg pta = { .pol = pol, .targfreq = targfreq,
+					     .relation = relation };
+
+	/*
+	 * Must run on @pol->cpu.  cpufreq core is responsible for ensuring
+	 * that we're bound to the current CPU and pol->cpu stays online.
+	 */
+	if (smp_processor_id() == pol->cpu)
+		return powernowk8_target_fn(&pta);
+	else
+		return work_on_cpu(pol->cpu, powernowk8_target_fn, &pta);
 }
 
 /* Driver entry point to verify the policy and range of frequencies */
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 3934fcc4e00b..17d6958342e7 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -168,9 +168,9 @@ static void atc_desc_put(struct at_dma_chan *atchan, struct at_desc *desc)
 }
 
 /**
- * atc_desc_chain - build chain adding a descripor
- * @first: address of first descripor of the chain
- * @prev: address of previous descripor of the chain
+ * atc_desc_chain - build chain adding a descriptor
+ * @first: address of first descriptor of the chain
+ * @prev: address of previous descriptor of the chain
  * @desc: descriptor to queue
  *
  * Called from prep_* functions
@@ -661,7 +661,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			flags);
 
 	if (unlikely(!atslave || !sg_len)) {
-		dev_dbg(chan2dev(chan), "prep_dma_memcpy: length is zero!\n");
+		dev_dbg(chan2dev(chan), "prep_slave_sg: sg length is zero!\n");
 		return NULL;
 	}
 
@@ -689,6 +689,11 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
+			if (unlikely(!len)) {
+				dev_dbg(chan2dev(chan),
+					"prep_slave_sg: sg(%d) data length is zero\n", i);
+				goto err;
+			}
 			mem_width = 2;
 			if (unlikely(mem & 3 || len & 3))
 				mem_width = 0;
@@ -724,6 +729,11 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
+			if (unlikely(!len)) {
+				dev_dbg(chan2dev(chan),
+					"prep_slave_sg: sg(%d) data length is zero\n", i);
+				goto err;
+			}
 			mem_width = 2;
 			if (unlikely(mem & 3 || len & 3))
 				mem_width = 0;
@@ -757,6 +767,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 err_desc_get:
 	dev_err(chan2dev(chan), "not enough descriptors available\n");
+err:
 	atc_desc_put(atchan, first);
 	return NULL;
 }
@@ -785,7 +796,7 @@ err_out:
 }
 
 /**
- * atc_dma_cyclic_fill_desc - Fill one period decriptor
+ * atc_dma_cyclic_fill_desc - Fill one period descriptor
  */
 static int
 atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index c64917ec313d..bb02fd981afb 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -1118,7 +1118,7 @@ fail:
  * @chan: channel
  * @dma_addr: DMA mapped address of the buffer
  * @buf_len: length of the buffer (in bytes)
- * @period_len: lenght of a single period
+ * @period_len: length of a single period
  * @dir: direction of the operation
  * @context: operation context (ignored)
  *
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 8f84761f98ba..094437b9d823 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -1015,7 +1015,7 @@ static irqreturn_t fsldma_chan_irq(int irq, void *data)
 	/*
 	 * Programming Error
 	 * The DMA_INTERRUPT async_tx is a NULL transfer, which will
-	 * triger a PE interrupt.
+	 * trigger a PE interrupt.
 	 */
 	if (stat & FSL_DMA_SR_PE) {
 		chan_dbg(chan, "irq: Programming Error INT\n");
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index 5084975d793c..54f580bb993c 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -572,8 +572,8 @@ static void imxdma_tasklet(unsigned long data)
 	if (desc->desc.callback)
 		desc->desc.callback(desc->desc.callback_param);
 
-	/* If we are dealing with a cyclic descriptor keep it on ld_active
-	 * and dont mark the descripor as complete.
+	/* If we are dealing with a cyclic descriptor, keep it on ld_active
+	 * and dont mark the descriptor as complete.
 	 * Only in non-cyclic cases it would be marked as complete
 	 */
 	if (imxdma_chan_is_doing_cyclic(imxdmac))
diff --git a/drivers/dma/intel_mid_dma.c b/drivers/dma/intel_mid_dma.c
index 222e907bfaaa..02b21d7d38e5 100644
--- a/drivers/dma/intel_mid_dma.c
+++ b/drivers/dma/intel_mid_dma.c
@@ -427,7 +427,7 @@ DMA engine callback Functions*/
  * intel_mid_dma_tx_submit -	callback to submit DMA transaction
  * @tx: dma engine descriptor
  *
- * Submit the DMA trasaction for this descriptor, start if ch idle
+ * 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)
 {
diff --git a/drivers/dma/intel_mid_dma_regs.h b/drivers/dma/intel_mid_dma_regs.h
index 1bfa9268feaf..17b42192ea58 100644
--- a/drivers/dma/intel_mid_dma_regs.h
+++ b/drivers/dma/intel_mid_dma_regs.h
@@ -168,9 +168,9 @@ union intel_mid_dma_cfg_hi {
  * @active_list: current active descriptors
  * @queue: current queued up descriptors
  * @free_list: current free descriptors
- * @slave: dma slave struture
- * @descs_allocated: total number of decsiptors allocated
- * @dma: dma device struture pointer
+ * @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
diff --git a/drivers/dma/ioat/hw.h b/drivers/dma/ioat/hw.h
index 60e675455b6a..d2ff3fda0b18 100644
--- a/drivers/dma/ioat/hw.h
+++ b/drivers/dma/ioat/hw.h
@@ -22,7 +22,6 @@
 #define _IOAT_HW_H_
 
 /* PCI Configuration Space Values */
-#define IOAT_PCI_VID            0x8086
 #define IOAT_MMIO_BAR		0
 
 /* CB device ID's */
@@ -31,9 +30,6 @@
 #define IOAT_PCI_DID_SCNB       0x65FF
 #define IOAT_PCI_DID_SNB        0x402F
 
-#define IOAT_PCI_RID            0x00
-#define IOAT_PCI_SVID           0x8086
-#define IOAT_PCI_SID            0x8086
 #define IOAT_VER_1_2            0x12    /* Version 1.2 */
 #define IOAT_VER_2_0            0x20    /* Version 2.0 */
 #define IOAT_VER_3_0            0x30    /* Version 3.0 */
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index e4feba6b03c0..5d3bbcd279b4 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -522,7 +522,7 @@ enum desc_status {
 	/* In the DMAC pool */
 	FREE,
 	/*
-	 * Allocted to some channel during prep_xxx
+	 * Allocated to some channel during prep_xxx
 	 * Also may be sitting on the work_list.
 	 */
 	PREP,
@@ -1567,17 +1567,19 @@ static int pl330_submit_req(void *ch_id, struct pl330_req *r)
 		goto xfer_exit;
 	}
 
-	/* Prefer Secure Channel */
-	if (!_manager_ns(thrd))
-		r->cfg->nonsecure = 0;
-	else
-		r->cfg->nonsecure = 1;
 
 	/* Use last settings, if not provided */
-	if (r->cfg)
+	if (r->cfg) {
+		/* Prefer Secure Channel */
+		if (!_manager_ns(thrd))
+			r->cfg->nonsecure = 0;
+		else
+			r->cfg->nonsecure = 1;
+
 		ccr = _prepare_ccr(r->cfg);
-	else
+	} else {
 		ccr = readl(regs + CC(thrd->id));
+	}
 
 	/* If this req doesn't have valid xfer settings */
 	if (!_is_valid(ccr)) {
@@ -2928,6 +2930,11 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		num_chan = max_t(int, pi->pcfg.num_peri, pi->pcfg.num_chan);
 
 	pdmac->peripherals = kzalloc(num_chan * sizeof(*pch), GFP_KERNEL);
+	if (!pdmac->peripherals) {
+		ret = -ENOMEM;
+		dev_err(&adev->dev, "unable to allocate pdmac->peripherals\n");
+		goto probe_err5;
+	}
 
 	for (i = 0; i < num_chan; i++) {
 		pch = &pdmac->peripherals[i];
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index ced98826684a..f72348d0bc41 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -4446,7 +4446,7 @@ static int __devinit ppc440spe_adma_probe(struct platform_device *ofdev)
 		ret = -ENOMEM;
 		goto err_dma_alloc;
 	}
-	dev_dbg(&ofdev->dev, "allocted descriptor pool virt 0x%p phys 0x%llx\n",
+	dev_dbg(&ofdev->dev, "allocated descriptor pool virt 0x%p phys 0x%llx\n",
 		adev->dma_desc_pool_virt, (u64)adev->dma_desc_pool);
 
 	regs = ioremap(res.start, resource_size(&res));
diff --git a/drivers/dma/ste_dma40_ll.h b/drivers/dma/ste_dma40_ll.h
index 51e8e5396e9b..6d47373f3f58 100644
--- a/drivers/dma/ste_dma40_ll.h
+++ b/drivers/dma/ste_dma40_ll.h
@@ -202,7 +202,7 @@
 /* LLI related structures */
 
 /**
- * struct d40_phy_lli - The basic configration register for each physical
+ * struct d40_phy_lli - The basic configuration register for each physical
  * channel.
  *
  * @reg_cfg: The configuration register.
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 616d90bcb3a4..d5dc9da7f99f 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -199,6 +199,36 @@ void *edac_align_ptr(void **p, unsigned size, int n_elems)
 	return (void *)(((unsigned long)ptr) + align - r);
 }
 
+static void _edac_mc_free(struct mem_ctl_info *mci)
+{
+	int i, chn, row;
+	struct csrow_info *csr;
+	const unsigned int tot_dimms = mci->tot_dimms;
+	const unsigned int tot_channels = mci->num_cschannel;
+	const unsigned int tot_csrows = mci->nr_csrows;
+
+	if (mci->dimms) {
+		for (i = 0; i < tot_dimms; i++)
+			kfree(mci->dimms[i]);
+		kfree(mci->dimms);
+	}
+	if (mci->csrows) {
+		for (row = 0; row < tot_csrows; row++) {
+			csr = mci->csrows[row];
+			if (csr) {
+				if (csr->channels) {
+					for (chn = 0; chn < tot_channels; chn++)
+						kfree(csr->channels[chn]);
+					kfree(csr->channels);
+				}
+				kfree(csr);
+			}
+		}
+		kfree(mci->csrows);
+	}
+	kfree(mci);
+}
+
 /**
  * edac_mc_alloc: Allocate and partially fill a struct mem_ctl_info structure
  * @mc_num:		Memory controller number
@@ -413,24 +443,7 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	return mci;
 
 error:
-	if (mci->dimms) {
-		for (i = 0; i < tot_dimms; i++)
-			kfree(mci->dimms[i]);
-		kfree(mci->dimms);
-	}
-	if (mci->csrows) {
-		for (chn = 0; chn < tot_channels; chn++) {
-			csr = mci->csrows[chn];
-			if (csr) {
-				for (chn = 0; chn < tot_channels; chn++)
-					kfree(csr->channels[chn]);
-				kfree(csr);
-			}
-			kfree(mci->csrows[i]);
-		}
-		kfree(mci->csrows);
-	}
-	kfree(mci);
+	_edac_mc_free(mci);
 
 	return NULL;
 }
@@ -445,6 +458,14 @@ void edac_mc_free(struct mem_ctl_info *mci)
 {
 	edac_dbg(1, "\n");
 
+	/* If we're not yet registered with sysfs free only what was allocated
+	 * in edac_mc_alloc().
+	 */
+	if (!device_is_registered(&mci->dev)) {
+		_edac_mc_free(mci);
+		return;
+	}
+
 	/* the mci instance is freed here, when the sysfs object is dropped */
 	edac_unregister_sysfs(mci);
 }
diff --git a/drivers/edac/i3200_edac.c b/drivers/edac/i3200_edac.c
index 47180a08edad..b6653a6fc5d5 100644
--- a/drivers/edac/i3200_edac.c
+++ b/drivers/edac/i3200_edac.c
@@ -391,7 +391,7 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 		for (j = 0; j < nr_channels; j++) {
 			struct dimm_info *dimm = csrow->channels[j]->dimm;
 
-			dimm->nr_pages = nr_pages / nr_channels;
+			dimm->nr_pages = nr_pages;
 			dimm->grain = nr_pages << PAGE_SHIFT;
 			dimm->mtype = MEM_DDR2;
 			dimm->dtype = DEV_UNKNOWN;
diff --git a/drivers/edac/i5000_edac.c b/drivers/edac/i5000_edac.c
index 39c63757c2a1..6a49dd00b81b 100644
--- a/drivers/edac/i5000_edac.c
+++ b/drivers/edac/i5000_edac.c
@@ -1012,6 +1012,10 @@ static void handle_channel(struct i5000_pvt *pvt, int slot, int channel,
 			/* add the number of COLUMN bits */
 			addrBits += MTR_DIMM_COLS_ADDR_BITS(mtr);
 
+			/* Dual-rank memories have twice the size */
+			if (dinfo->dual_rank)
+				addrBits++;
+
 			addrBits += 6;	/* add 64 bits per DIMM */
 			addrBits -= 20;	/* divide by 2^^20 */
 			addrBits -= 3;	/* 8 bits per bytes */
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index f3b1f9fafa4b..5715b7c2c517 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -513,7 +513,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
 	struct dimm_info *dimm;
-	int i, j, banks, ranks, rows, cols, size, npages;
+	unsigned i, j, banks, ranks, rows, cols, npages;
+	u64 size;
 	u32 reg;
 	enum edac_type mode;
 	enum mem_type mtype;
@@ -585,10 +586,10 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				cols = numcol(mtr);
 
 				/* DDR3 has 8 I/O banks */
-				size = (rows * cols * banks * ranks) >> (20 - 3);
+				size = ((u64)rows * cols * banks * ranks) >> (20 - 3);
 				npages = MiB_TO_PAGES(size);
 
-				edac_dbg(0, "mc#%d: channel %d, dimm %d, %d Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
+				edac_dbg(0, "mc#%d: channel %d, dimm %d, %Ld Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
 					 pvt->sbridge_dev->mc, i, j,
 					 size, npages,
 					 banks, ranks, rows, cols);
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index e175c8ed4ec4..07122a9ef36e 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -21,6 +21,12 @@ config EXTCON_GPIO
 	  Say Y here to enable GPIO based extcon support. Note that GPIO
 	  extcon supports single state per extcon instance.
 
+config EXTCON_ADC_JACK
+	tristate "ADC Jack extcon support"
+	depends on IIO
+	help
+	  Say Y here to enable extcon device driver based on ADC values.
+
 config EXTCON_MAX77693
 	tristate "MAX77693 EXTCON Support"
 	depends on MFD_MAX77693
@@ -41,7 +47,7 @@ config EXTCON_MAX8997
 
 config EXTCON_ARIZONA
 	tristate "Wolfson Arizona EXTCON support"
-	depends on MFD_ARIZONA
+	depends on MFD_ARIZONA && INPUT
 	help
 	  Say Y here to enable support for external accessory detection
 	  with Wolfson Arizona devices. These are audio CODECs with
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index 88961b332348..f98a3c4d46e0 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -2,8 +2,9 @@
 # Makefile for external connector class (extcon) devices
 #
 
-obj-$(CONFIG_EXTCON)		+= extcon_class.o
-obj-$(CONFIG_EXTCON_GPIO)	+= extcon_gpio.o
+obj-$(CONFIG_EXTCON)		+= extcon-class.o
+obj-$(CONFIG_EXTCON_GPIO)	+= extcon-gpio.o
+obj-$(CONFIG_EXTCON_ADC_JACK)	+= extcon-adc-jack.o
 obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
 obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
 obj-$(CONFIG_EXTCON_ARIZONA)	+= extcon-arizona.o
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
new file mode 100644
index 000000000000..60ac3fbb4cde
--- /dev/null
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -0,0 +1,198 @@
+/*
+ * drivers/extcon/extcon-adc-jack.c
+ *
+ * Analog Jack extcon driver with ADC-based detection capability.
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * MyungJoo Ham <myungjoo.ham@samsung.com>
+ *
+ * Modified for calling to IIO to get adc by <anish.singh@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/iio/consumer.h>
+#include <linux/extcon/extcon-adc-jack.h>
+#include <linux/extcon.h>
+
+/**
+ * struct adc_jack_data - internal data for adc_jack device driver
+ * @edev        - extcon device.
+ * @cable_names - list of supported cables.
+ * @num_cables  - size of cable_names.
+ * @adc_conditions       - list of adc value conditions.
+ * @num_conditions       - size of adc_conditions.
+ * @irq         - irq number of attach/detach event (0 if not exist).
+ * @handling_delay      - interrupt handler will schedule extcon event
+ *                      handling at handling_delay jiffies.
+ * @handler     - extcon event handler called by interrupt handler.
+ * @chan       - iio channel being queried.
+ */
+struct adc_jack_data {
+	struct extcon_dev edev;
+
+	const char **cable_names;
+	int num_cables;
+	struct adc_jack_cond *adc_conditions;
+	int num_conditions;
+
+	int irq;
+	unsigned long handling_delay; /* in jiffies */
+	struct delayed_work handler;
+
+	struct iio_channel *chan;
+};
+
+static void adc_jack_handler(struct work_struct *work)
+{
+	struct adc_jack_data *data = container_of(to_delayed_work(work),
+			struct adc_jack_data,
+			handler);
+	u32 state = 0;
+	int ret, adc_val;
+	int i;
+
+	ret = iio_read_channel_raw(data->chan, &adc_val);
+	if (ret < 0) {
+		dev_err(data->edev.dev, "read channel() error: %d\n", ret);
+		return;
+	}
+
+	/* Get state from adc value with adc_conditions */
+	for (i = 0; i < data->num_conditions; i++) {
+		struct adc_jack_cond *def = &data->adc_conditions[i];
+		if (!def->state)
+			break;
+		if (def->min_adc <= adc_val && def->max_adc >= adc_val) {
+			state = def->state;
+			break;
+		}
+	}
+	/* if no def has met, it means state = 0 (no cables attached) */
+
+	extcon_set_state(&data->edev, state);
+}
+
+static irqreturn_t adc_jack_irq_thread(int irq, void *_data)
+{
+	struct adc_jack_data *data = _data;
+
+	schedule_delayed_work(&data->handler, data->handling_delay);
+	return IRQ_HANDLED;
+}
+
+static int __devinit adc_jack_probe(struct platform_device *pdev)
+{
+	struct adc_jack_data *data;
+	struct adc_jack_pdata *pdata = pdev->dev.platform_data;
+	int i, err = 0;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->edev.name = pdata->name;
+
+	if (!pdata->cable_names) {
+		err = -EINVAL;
+		dev_err(&pdev->dev, "error: cable_names not defined.\n");
+		goto out;
+	}
+
+	data->edev.supported_cable = pdata->cable_names;
+
+	/* Check the length of array and set num_cables */
+	for (i = 0; data->edev.supported_cable[i]; i++)
+		;
+	if (i == 0 || i > SUPPORTED_CABLE_MAX) {
+		err = -EINVAL;
+		dev_err(&pdev->dev, "error: pdata->cable_names size = %d\n",
+				i - 1);
+		goto out;
+	}
+	data->num_cables = i;
+
+	if (!pdata->adc_conditions ||
+			!pdata->adc_conditions[0].state) {
+		err = -EINVAL;
+		dev_err(&pdev->dev, "error: adc_conditions not defined.\n");
+		goto out;
+	}
+	data->adc_conditions = pdata->adc_conditions;
+
+	/* Check the length of array and set num_conditions */
+	for (i = 0; data->adc_conditions[i].state; i++)
+		;
+	data->num_conditions = i;
+
+	data->chan = iio_channel_get(dev_name(&pdev->dev),
+			pdata->consumer_channel);
+	if (IS_ERR(data->chan)) {
+		err = PTR_ERR(data->chan);
+		goto out;
+	}
+
+	data->handling_delay = msecs_to_jiffies(pdata->handling_delay_ms);
+
+	INIT_DELAYED_WORK_DEFERRABLE(&data->handler, adc_jack_handler);
+
+	platform_set_drvdata(pdev, data);
+
+	err = extcon_dev_register(&data->edev, &pdev->dev);
+	if (err)
+		goto out;
+
+	data->irq = platform_get_irq(pdev, 0);
+	if (!data->irq) {
+		dev_err(&pdev->dev, "platform_get_irq failed\n");
+		err = -ENODEV;
+		goto err_irq;
+	}
+
+	err = request_any_context_irq(data->irq, adc_jack_irq_thread,
+			pdata->irq_flags, pdata->name, data);
+
+	if (err) {
+		dev_err(&pdev->dev, "error: irq %d\n", data->irq);
+		err = -EINVAL;
+		goto err_irq;
+	}
+
+	goto out;
+
+err_irq:
+	extcon_dev_unregister(&data->edev);
+out:
+	return err;
+}
+
+static int __devexit adc_jack_remove(struct platform_device *pdev)
+{
+	struct adc_jack_data *data = platform_get_drvdata(pdev);
+
+	free_irq(data->irq, data);
+	cancel_work_sync(&data->handler.work);
+	extcon_dev_unregister(&data->edev);
+
+	return 0;
+}
+
+static struct platform_driver adc_jack_driver = {
+	.probe          = adc_jack_probe,
+	.remove         = __devexit_p(adc_jack_remove),
+	.driver         = {
+		.name   = "adc-jack",
+		.owner  = THIS_MODULE,
+	},
+};
+
+module_platform_driver(adc_jack_driver);
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index 427a289f32a5..cdab9e598297 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
+#include <linux/input.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
@@ -30,11 +31,14 @@
 #include <linux/mfd/arizona/pdata.h>
 #include <linux/mfd/arizona/registers.h>
 
+#define ARIZONA_NUM_BUTTONS 6
+
 struct arizona_extcon_info {
 	struct device *dev;
 	struct arizona *arizona;
 	struct mutex lock;
 	struct regulator *micvdd;
+	struct input_dev *input;
 
 	int micd_mode;
 	const struct arizona_micd_config *micd_modes;
@@ -54,6 +58,18 @@ static const struct arizona_micd_config micd_default_modes[] = {
 	{ 0,                  2 << ARIZONA_MICD_BIAS_SRC_SHIFT, 1 },
 };
 
+static struct {
+	u16 status;
+	int report;
+} arizona_lvl_to_key[ARIZONA_NUM_BUTTONS] = {
+	{  0x1, BTN_0 },
+	{  0x2, BTN_1 },
+	{  0x4, BTN_2 },
+	{  0x8, BTN_3 },
+	{ 0x10, BTN_4 },
+	{ 0x20, BTN_5 },
+};
+
 #define ARIZONA_CABLE_MECHANICAL 0
 #define ARIZONA_CABLE_MICROPHONE 1
 #define ARIZONA_CABLE_HEADPHONE  2
@@ -133,6 +149,7 @@ static void arizona_stop_mic(struct arizona_extcon_info *info)
 
 	if (change) {
 		regulator_disable(info->micvdd);
+		pm_runtime_mark_last_busy(info->dev);
 		pm_runtime_put_autosuspend(info->dev);
 	}
 }
@@ -141,8 +158,8 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 {
 	struct arizona_extcon_info *info = data;
 	struct arizona *arizona = info->arizona;
-	unsigned int val;
-	int ret;
+	unsigned int val, lvl;
+	int ret, i;
 
 	mutex_lock(&info->lock);
 
@@ -219,13 +236,22 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 
 	/*
 	 * If we're still detecting and we detect a short then we've
-	 * got a headphone.  Otherwise it's a button press, the
-	 * button reporting is stubbed out for now.
+	 * got a headphone.  Otherwise it's a button press.
 	 */
 	if (val & 0x3fc) {
 		if (info->mic) {
 			dev_dbg(arizona->dev, "Mic button detected\n");
 
+			lvl = val & ARIZONA_MICD_LVL_MASK;
+			lvl >>= ARIZONA_MICD_LVL_SHIFT;
+
+			for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+				if (lvl & arizona_lvl_to_key[i].status)
+					input_report_key(info->input,
+							 arizona_lvl_to_key[i].report,
+							 1);
+			input_sync(info->input);
+
 		} else if (info->detecting) {
 			dev_dbg(arizona->dev, "Headphone detected\n");
 			info->detecting = false;
@@ -244,6 +270,10 @@ static irqreturn_t arizona_micdet(int irq, void *data)
 		}
 	} else {
 		dev_dbg(arizona->dev, "Mic button released\n");
+		for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+			input_report_key(info->input,
+					 arizona_lvl_to_key[i].report, 0);
+		input_sync(info->input);
 	}
 
 handled:
@@ -258,7 +288,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 	struct arizona_extcon_info *info = data;
 	struct arizona *arizona = info->arizona;
 	unsigned int val;
-	int ret;
+	int ret, i;
 
 	pm_runtime_get_sync(info->dev);
 
@@ -288,6 +318,11 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 
 		arizona_stop_mic(info);
 
+		for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+			input_report_key(info->input,
+					 arizona_lvl_to_key[i].report, 0);
+		input_sync(info->input);
+
 		ret = extcon_update_state(&info->edev, 0xffffffff, 0);
 		if (ret != 0)
 			dev_err(arizona->dev, "Removal report failed: %d\n",
@@ -307,13 +342,13 @@ static int __devinit arizona_extcon_probe(struct platform_device *pdev)
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
 	struct arizona_pdata *pdata;
 	struct arizona_extcon_info *info;
-	int ret, mode;
+	int ret, mode, i;
 
 	pdata = dev_get_platdata(arizona->dev);
 
 	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (!info) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+		dev_err(&pdev->dev, "Failed to allocate memory\n");
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -350,7 +385,7 @@ static int __devinit arizona_extcon_probe(struct platform_device *pdev)
 
 	ret = extcon_dev_register(&info->edev, arizona->dev);
 	if (ret < 0) {
-		dev_err(arizona->dev, "extcon_dev_regster() failed: %d\n",
+		dev_err(arizona->dev, "extcon_dev_register() failed: %d\n",
 			ret);
 		goto err;
 	}
@@ -382,6 +417,20 @@ static int __devinit arizona_extcon_probe(struct platform_device *pdev)
 
 	arizona_extcon_set_mode(info, 0);
 
+	info->input = input_allocate_device();
+	if (!info->input) {
+		dev_err(arizona->dev, "Can't allocate input dev\n");
+		ret = -ENOMEM;
+		goto err_register;
+	}
+
+	for (i = 0; i < ARIZONA_NUM_BUTTONS; i++)
+		input_set_capability(info->input, EV_KEY,
+				     arizona_lvl_to_key[i].report);
+	info->input->name = "Headset";
+	info->input->phys = "arizona/extcon";
+	info->input->dev.parent = &pdev->dev;
+
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_idle(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
@@ -391,7 +440,7 @@ static int __devinit arizona_extcon_probe(struct platform_device *pdev)
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to get JACKDET rise IRQ: %d\n",
 			ret);
-		goto err_register;
+		goto err_input;
 	}
 
 	ret = arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_RISE, 1);
@@ -434,10 +483,23 @@ static int __devinit arizona_extcon_probe(struct platform_device *pdev)
 	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
 			   ARIZONA_JD1_ENA, ARIZONA_JD1_ENA);
 
+	ret = regulator_allow_bypass(info->micvdd, true);
+	if (ret != 0)
+		dev_warn(arizona->dev, "Failed to set MICVDD to bypass: %d\n",
+			 ret);
+
 	pm_runtime_put(&pdev->dev);
 
+	ret = input_register_device(info->input);
+	if (ret) {
+		dev_err(&pdev->dev, "Can't register input device: %d\n", ret);
+		goto err_micdet;
+	}
+
 	return 0;
 
+err_micdet:
+	arizona_free_irq(arizona, ARIZONA_IRQ_MICDET, info);
 err_fall_wake:
 	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_FALL, 0);
 err_fall:
@@ -446,6 +508,8 @@ err_rise_wake:
 	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_RISE, 0);
 err_rise:
 	arizona_free_irq(arizona, ARIZONA_IRQ_JD_RISE, info);
+err_input:
+	input_free_device(info->input);
 err_register:
 	pm_runtime_disable(&pdev->dev);
 	extcon_dev_unregister(&info->edev);
@@ -468,6 +532,7 @@ static int __devexit arizona_extcon_remove(struct platform_device *pdev)
 	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
 			   ARIZONA_JD1_ENA, 0);
 	arizona_clk32k_disable(arizona);
+	input_unregister_device(info->input);
 	extcon_dev_unregister(&info->edev);
 
 	return 0;
diff --git a/drivers/extcon/extcon_class.c b/drivers/extcon/extcon-class.c
index f6419f9db76c..946a3188b2b7 100644
--- a/drivers/extcon/extcon_class.c
+++ b/drivers/extcon/extcon-class.c
@@ -30,6 +30,7 @@
 #include <linux/err.h>
 #include <linux/extcon.h>
 #include <linux/slab.h>
+#include <linux/sysfs.h>
 
 /*
  * extcon_cable_name suggests the standard cable names for commonly used
@@ -442,7 +443,7 @@ static int _call_per_cable(struct notifier_block *nb, unsigned long val,
 
 /**
  * extcon_register_interest() - Register a notifier for a state change of a
- *			      specific cable, not a entier set of cables of a
+ *			      specific cable, not an entier set of cables of a
  *			      extcon device.
  * @obj:	an empty extcon_specific_cable_nb object to be returned.
  * @extcon_name:	the name of extcon device.
@@ -498,7 +499,7 @@ int extcon_unregister_interest(struct extcon_specific_cable_nb *obj)
 }
 
 /**
- * extcon_register_notifier() - Register a notifee to get notified by
+ * extcon_register_notifier() - Register a notifiee to get notified by
  *			      any attach status changes from the extcon.
  * @edev:	the extcon device.
  * @nb:		a notifier block to be registered.
@@ -515,7 +516,7 @@ int extcon_register_notifier(struct extcon_dev *edev,
 EXPORT_SYMBOL_GPL(extcon_register_notifier);
 
 /**
- * extcon_unregister_notifier() - Unregister a notifee from the extcon device.
+ * extcon_unregister_notifier() - Unregister a notifiee from the extcon device.
  * @edev:	the extcon device.
  * @nb:		a registered notifier block to be unregistered.
  */
@@ -673,10 +674,12 @@ int extcon_dev_register(struct extcon_dev *edev, struct device *dev)
 			cable->attr_g.name = str;
 			cable->attr_g.attrs = cable->attrs;
 
+			sysfs_attr_init(&cable->attr_name.attr);
 			cable->attr_name.attr.name = "name";
 			cable->attr_name.attr.mode = 0444;
 			cable->attr_name.show = cable_name_show;
 
+			sysfs_attr_init(&cable->attr_state.attr);
 			cable->attr_state.attr.name = "state";
 			cable->attr_state.attr.mode = 0644;
 			cable->attr_state.show = cable_state_show;
@@ -722,6 +725,7 @@ int extcon_dev_register(struct extcon_dev *edev, struct device *dev)
 				goto err_muex;
 			}
 			strcpy(name, buf);
+			sysfs_attr_init(&edev->d_attrs_muex[index].attr);
 			edev->d_attrs_muex[index].attr.name = name;
 			edev->d_attrs_muex[index].attr.mode = 0000;
 			edev->attrs_muex[index] = &edev->d_attrs_muex[index]
@@ -802,7 +806,7 @@ EXPORT_SYMBOL_GPL(extcon_dev_register);
 
 /**
  * extcon_dev_unregister() - Unregister the extcon device.
- * @edev:	the extcon device instance to be unregitered.
+ * @edev:	the extcon device instance to be unregistered.
  *
  * Note that this does not call kfree(edev) because edev was not allocated
  * by this class.
diff --git a/drivers/extcon/extcon_gpio.c b/drivers/extcon/extcon-gpio.c
index 3cc152e690b0..3cc152e690b0 100644
--- a/drivers/extcon/extcon_gpio.c
+++ b/drivers/extcon/extcon-gpio.c
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index 38f9e52f358b..e21387e2da5c 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -356,7 +356,7 @@ static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info,
 		extcon_set_cable_state(info->edev, "MHL", attached);
 		break;
 	default:
-		dev_err(info->dev, "faild to detect %s accessory\n",
+		dev_err(info->dev, "failed to detect %s accessory\n",
 			attached ? "attached" : "detached");
 		dev_err(info->dev, "- adc:0x%x, adclow:0x%x, adc1k:0x%x\n",
 			adc, adclow, adc1k);
@@ -548,7 +548,7 @@ static void max77693_muic_irq_work(struct work_struct *work)
 		curr_adc = info->status[0] & STATUS1_ADC_MASK;
 		curr_adc >>= STATUS1_ADC_SHIFT;
 
-		/* Check accossory state which is either detached or attached */
+		/* Check accessory state which is either detached or attached */
 		if (curr_adc == MAX77693_MUIC_ADC_OPEN)
 			attached = false;
 
@@ -564,7 +564,7 @@ static void max77693_muic_irq_work(struct work_struct *work)
 		curr_chg_type = info->status[1] & STATUS2_CHGTYP_MASK;
 		curr_chg_type >>= STATUS2_CHGTYP_SHIFT;
 
-		/* Check charger accossory state which
+		/* Check charger accessory state which
 		   is either detached or attached */
 		if (curr_chg_type == MAX77693_CHARGER_TYPE_NONE)
 			attached = false;
@@ -699,7 +699,7 @@ static int __devinit max77693_muic_probe(struct platform_device *pdev)
 
 		ret = request_threaded_irq(virq, NULL,
 				max77693_muic_irq_handler,
-				0, muic_irq->name, info);
+				IRQF_ONESHOT, muic_irq->name, info);
 		if (ret) {
 			dev_err(&pdev->dev,
 				"failed: irq request (IRQ: %d,"
diff --git a/drivers/firmware/efivars.c b/drivers/firmware/efivars.c
index 47408e802ab6..d10c9873dd9a 100644
--- a/drivers/firmware/efivars.c
+++ b/drivers/firmware/efivars.c
@@ -435,12 +435,23 @@ efivar_attr_read(struct efivar_entry *entry, char *buf)
 	if (status != EFI_SUCCESS)
 		return -EIO;
 
-	if (var->Attributes & 0x1)
+	if (var->Attributes & EFI_VARIABLE_NON_VOLATILE)
 		str += sprintf(str, "EFI_VARIABLE_NON_VOLATILE\n");
-	if (var->Attributes & 0x2)
+	if (var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)
 		str += sprintf(str, "EFI_VARIABLE_BOOTSERVICE_ACCESS\n");
-	if (var->Attributes & 0x4)
+	if (var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS)
 		str += sprintf(str, "EFI_VARIABLE_RUNTIME_ACCESS\n");
+	if (var->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD)
+		str += sprintf(str, "EFI_VARIABLE_HARDWARE_ERROR_RECORD\n");
+	if (var->Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
+		str += sprintf(str,
+			"EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS\n");
+	if (var->Attributes &
+			EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
+		str += sprintf(str,
+			"EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS\n");
+	if (var->Attributes & EFI_VARIABLE_APPEND_WRITE)
+		str += sprintf(str, "EFI_VARIABLE_APPEND_WRITE\n");
 	return str - buf;
 }
 
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index 8a420f13905e..ed94b4ea72e9 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -308,6 +308,7 @@ static int lpc32xx_gpio_dir_output_p012(struct gpio_chip *chip, unsigned pin,
 {
 	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
 
+	__set_gpio_level_p012(group, pin, value);
 	__set_gpio_dir_p012(group, pin, 0);
 
 	return 0;
@@ -318,6 +319,7 @@ static int lpc32xx_gpio_dir_output_p3(struct gpio_chip *chip, unsigned pin,
 {
 	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
 
+	__set_gpio_level_p3(group, pin, value);
 	__set_gpio_dir_p3(group, pin, 0);
 
 	return 0;
@@ -326,6 +328,9 @@ static int lpc32xx_gpio_dir_output_p3(struct gpio_chip *chip, unsigned pin,
 static int lpc32xx_gpio_dir_out_always(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
+	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+
+	__set_gpo_level_p3(group, pin, value);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 489e2b162b27..274d25de521e 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -3242,7 +3242,8 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 {
 	int ret;
 
-	BUG_ON(obj->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT);
+	if (WARN_ON(obj->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT))
+		return -EBUSY;
 
 	if (obj->gtt_space != NULL) {
 		if ((alignment && obj->gtt_offset & (alignment - 1)) ||
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index bc2ad348e5d8..c040aee1341c 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -4191,12 +4191,6 @@ static void i8xx_update_pll(struct drm_crtc *crtc,
 	POSTING_READ(DPLL(pipe));
 	udelay(150);
 
-	I915_WRITE(DPLL(pipe), dpll);
-
-	/* Wait for the clocks to stabilize. */
-	POSTING_READ(DPLL(pipe));
-	udelay(150);
-
 	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
 	 * This is an exception to the general rule that mode_set doesn't turn
 	 * things on.
@@ -4204,6 +4198,12 @@ static void i8xx_update_pll(struct drm_crtc *crtc,
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
 		intel_update_lvds(crtc, clock, adjusted_mode);
 
+	I915_WRITE(DPLL(pipe), dpll);
+
+	/* Wait for the clocks to stabilize. */
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
 	/* The pixel multiplier can only be updated once the
 	 * DPLL is enabled and the clocks are stable.
 	 *
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 98f602427eb8..12dc3308ab8c 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -609,7 +609,7 @@ static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
 	u32 temp;
 	u32 enable_bits = SDVO_ENABLE;
 
-	if (intel_hdmi->has_audio)
+	if (intel_hdmi->has_audio || mode != DRM_MODE_DPMS_ON)
 		enable_bits |= SDVO_AUDIO_ENABLE;
 
 	temp = I915_READ(intel_hdmi->sdvox_reg);
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index ff23d88880e5..3ca240b4413d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -179,7 +179,7 @@ nouveau_abi16_ioctl_grobj_alloc(ABI16_IOCTL_ARGS)
 			return 0;
 	} else
 	if (init->class == 0x906e) {
-		NV_ERROR(dev, "906e not supported yet\n");
+		NV_DEBUG(dev, "906e not supported yet\n");
 		return -EINVAL;
 	}
 
diff --git a/drivers/gpu/drm/nouveau/nv50_gpio.c b/drivers/gpu/drm/nouveau/nv50_gpio.c
index f03490534893..c399d510b27a 100644
--- a/drivers/gpu/drm/nouveau/nv50_gpio.c
+++ b/drivers/gpu/drm/nouveau/nv50_gpio.c
@@ -22,6 +22,7 @@
  * Authors: Ben Skeggs
  */
 
+#include <linux/dmi.h>
 #include "drmP.h"
 #include "nouveau_drv.h"
 #include "nouveau_hw.h"
@@ -110,13 +111,25 @@ nv50_gpio_isr(struct drm_device *dev)
 		nv_wr32(dev, 0xe074, intr1);
 }
 
+static struct dmi_system_id gpio_reset_ids[] = {
+	{
+		.ident = "Apple Macbook 10,1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro10,1"),
+		}
+	},
+	{ }
+};
+
 int
 nv50_gpio_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
 	/* initialise gpios and routing to vbios defaults */
-	nouveau_gpio_reset(dev);
+	if (dmi_check_system(gpio_reset_ids))
+		nouveau_gpio_reset(dev);
 
 	/* disable, and ack any pending gpio interrupts */
 	nv_wr32(dev, 0xe050, 0x00000000);
diff --git a/drivers/gpu/drm/nouveau/nvc0_fb.c b/drivers/gpu/drm/nouveau/nvc0_fb.c
index f704e942372e..f376c39310df 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fb.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fb.c
@@ -124,6 +124,7 @@ nvc0_fb_init(struct drm_device *dev)
 	priv = dev_priv->engine.fb.priv;
 
 	nv_wr32(dev, 0x100c10, priv->r100c10 >> 8);
+	nv_mask(dev, 0x17e820, 0x00100000, 0x00000000); /* NV_PLTCG_INTR_EN */
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvc0_fifo.c b/drivers/gpu/drm/nouveau/nvc0_fifo.c
index 7d85553d518c..cd39eb99f5b1 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fifo.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fifo.c
@@ -373,7 +373,8 @@ nvc0_fifo_isr_subfifo_intr(struct drm_device *dev, int unit)
 static void
 nvc0_fifo_isr(struct drm_device *dev)
 {
-	u32 stat = nv_rd32(dev, 0x002100);
+	u32 mask = nv_rd32(dev, 0x002140);
+	u32 stat = nv_rd32(dev, 0x002100) & mask;
 
 	if (stat & 0x00000100) {
 		NV_INFO(dev, "PFIFO: unknown status 0x00000100\n");
diff --git a/drivers/gpu/drm/nouveau/nve0_fifo.c b/drivers/gpu/drm/nouveau/nve0_fifo.c
index e98d144e6eb9..281bece751b6 100644
--- a/drivers/gpu/drm/nouveau/nve0_fifo.c
+++ b/drivers/gpu/drm/nouveau/nve0_fifo.c
@@ -345,7 +345,8 @@ nve0_fifo_isr_subfifo_intr(struct drm_device *dev, int unit)
 static void
 nve0_fifo_isr(struct drm_device *dev)
 {
-	u32 stat = nv_rd32(dev, 0x002100);
+	u32 mask = nv_rd32(dev, 0x002140);
+	u32 stat = nv_rd32(dev, 0x002100) & mask;
 
 	if (stat & 0x00000100) {
 		NV_INFO(dev, "PFIFO: unknown status 0x00000100\n");
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index e721e3087b99..2817101fb167 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1479,98 +1479,14 @@ static void radeon_legacy_atom_fixup(struct drm_crtc *crtc)
 	}
 }
 
-/**
- * radeon_get_pll_use_mask - look up a mask of which pplls are in use
- *
- * @crtc: drm crtc
- *
- * Returns the mask of which PPLLs (Pixel PLLs) are in use.
- */
-static u32 radeon_get_pll_use_mask(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_crtc *test_crtc;
-	struct radeon_crtc *radeon_test_crtc;
-	u32 pll_in_use = 0;
-
-	list_for_each_entry(test_crtc, &dev->mode_config.crtc_list, head) {
-		if (crtc == test_crtc)
-			continue;
-
-		radeon_test_crtc = to_radeon_crtc(test_crtc);
-		if (radeon_test_crtc->pll_id != ATOM_PPLL_INVALID)
-			pll_in_use |= (1 << radeon_test_crtc->pll_id);
-	}
-	return pll_in_use;
-}
-
-/**
- * radeon_get_shared_dp_ppll - return the PPLL used by another crtc for DP
- *
- * @crtc: drm crtc
- *
- * Returns the PPLL (Pixel PLL) used by another crtc/encoder which is
- * also in DP mode.  For DP, a single PPLL can be used for all DP
- * crtcs/encoders.
- */
-static int radeon_get_shared_dp_ppll(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_encoder *test_encoder;
-	struct radeon_crtc *radeon_test_crtc;
-
-	list_for_each_entry(test_encoder, &dev->mode_config.encoder_list, head) {
-		if (test_encoder->crtc && (test_encoder->crtc != crtc)) {
-			if (ENCODER_MODE_IS_DP(atombios_get_encoder_mode(test_encoder))) {
-				/* for DP use the same PLL for all */
-				radeon_test_crtc = to_radeon_crtc(test_encoder->crtc);
-				if (radeon_test_crtc->pll_id != ATOM_PPLL_INVALID)
-					return radeon_test_crtc->pll_id;
-			}
-		}
-	}
-	return ATOM_PPLL_INVALID;
-}
-
-/**
- * radeon_atom_pick_pll - Allocate a PPLL for use by the crtc.
- *
- * @crtc: drm crtc
- *
- * Returns the PPLL (Pixel PLL) to be used by the crtc.  For DP monitors
- * a single PPLL can be used for all DP crtcs/encoders.  For non-DP
- * monitors a dedicated PPLL must be used.  If a particular board has
- * an external DP PLL, return ATOM_PPLL_INVALID to skip PLL programming
- * as there is no need to program the PLL itself.  If we are not able to
- * allocate a PLL, return ATOM_PPLL_INVALID to skip PLL programming to
- * avoid messing up an existing monitor.
- *
- * Asic specific PLL information
- *
- * DCE 6.1
- * - PPLL2 is only available to UNIPHYA (both DP and non-DP)
- * - PPLL0, PPLL1 are available for UNIPHYB/C/D/E/F (both DP and non-DP)
- *
- * DCE 6.0
- * - PPLL0 is available to all UNIPHY (DP only)
- * - PPLL1, PPLL2 are available for all UNIPHY (both DP and non-DP) and DAC
- *
- * DCE 5.0
- * - DCPLL is available to all UNIPHY (DP only)
- * - PPLL1, PPLL2 are available for all UNIPHY (both DP and non-DP) and DAC
- *
- * DCE 3.0/4.0/4.1
- * - PPLL1, PPLL2 are available for all UNIPHY (both DP and non-DP) and DAC
- *
- */
 static int radeon_atom_pick_pll(struct drm_crtc *crtc)
 {
 	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct drm_encoder *test_encoder;
-	u32 pll_in_use;
-	int pll;
+	struct drm_crtc *test_crtc;
+	uint32_t pll_in_use = 0;
 
 	if (ASIC_IS_DCE61(rdev)) {
 		list_for_each_entry(test_encoder, &dev->mode_config.encoder_list, head) {
@@ -1582,40 +1498,32 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
 
 				if ((test_radeon_encoder->encoder_id ==
 				     ENCODER_OBJECT_ID_INTERNAL_UNIPHY) &&
-				    (dig->linkb == false))
-					/* UNIPHY A uses PPLL2 */
+				    (dig->linkb == false)) /* UNIPHY A uses PPLL2 */
 					return ATOM_PPLL2;
-				else if (ENCODER_MODE_IS_DP(atombios_get_encoder_mode(test_encoder))) {
-					/* UNIPHY B/C/D/E/F */
-					if (rdev->clock.dp_extclk)
-						/* skip PPLL programming if using ext clock */
-						return ATOM_PPLL_INVALID;
-					else {
-						/* use the same PPLL for all DP monitors */
-						pll = radeon_get_shared_dp_ppll(crtc);
-						if (pll != ATOM_PPLL_INVALID)
-							return pll;
-					}
-				}
-				break;
 			}
 		}
 		/* UNIPHY B/C/D/E/F */
-		pll_in_use = radeon_get_pll_use_mask(crtc);
-		if (!(pll_in_use & (1 << ATOM_PPLL0)))
+		list_for_each_entry(test_crtc, &dev->mode_config.crtc_list, head) {
+			struct radeon_crtc *radeon_test_crtc;
+
+			if (crtc == test_crtc)
+				continue;
+
+			radeon_test_crtc = to_radeon_crtc(test_crtc);
+			if ((radeon_test_crtc->pll_id == ATOM_PPLL0) ||
+			    (radeon_test_crtc->pll_id == ATOM_PPLL1))
+				pll_in_use |= (1 << radeon_test_crtc->pll_id);
+		}
+		if (!(pll_in_use & 4))
 			return ATOM_PPLL0;
-		if (!(pll_in_use & (1 << ATOM_PPLL1)))
-			return ATOM_PPLL1;
-		DRM_ERROR("unable to allocate a PPLL\n");
-		return ATOM_PPLL_INVALID;
+		return ATOM_PPLL1;
 	} else if (ASIC_IS_DCE4(rdev)) {
 		list_for_each_entry(test_encoder, &dev->mode_config.encoder_list, head) {
 			if (test_encoder->crtc && (test_encoder->crtc == crtc)) {
 				/* in DP mode, the DP ref clock can come from PPLL, DCPLL, or ext clock,
 				 * depending on the asic:
 				 * DCE4: PPLL or ext clock
-				 * DCE5: PPLL, DCPLL, or ext clock
-				 * DCE6: PPLL, PPLL0, or ext clock
+				 * DCE5: DCPLL or ext clock
 				 *
 				 * Setting ATOM_PPLL_INVALID will cause SetPixelClock to skip
 				 * PPLL/DCPLL programming and only program the DP DTO for the
@@ -1623,34 +1531,31 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
 				 */
 				if (ENCODER_MODE_IS_DP(atombios_get_encoder_mode(test_encoder))) {
 					if (rdev->clock.dp_extclk)
-						/* skip PPLL programming if using ext clock */
 						return ATOM_PPLL_INVALID;
 					else if (ASIC_IS_DCE6(rdev))
-						/* use PPLL0 for all DP */
 						return ATOM_PPLL0;
 					else if (ASIC_IS_DCE5(rdev))
-						/* use DCPLL for all DP */
 						return ATOM_DCPLL;
-					else {
-						/* use the same PPLL for all DP monitors */
-						pll = radeon_get_shared_dp_ppll(crtc);
-						if (pll != ATOM_PPLL_INVALID)
-							return pll;
-					}
 				}
-				break;
 			}
 		}
-		/* all other cases */
-		pll_in_use = radeon_get_pll_use_mask(crtc);
-		if (!(pll_in_use & (1 << ATOM_PPLL2)))
-			return ATOM_PPLL2;
-		if (!(pll_in_use & (1 << ATOM_PPLL1)))
+
+		/* otherwise, pick one of the plls */
+		list_for_each_entry(test_crtc, &dev->mode_config.crtc_list, head) {
+			struct radeon_crtc *radeon_test_crtc;
+
+			if (crtc == test_crtc)
+				continue;
+
+			radeon_test_crtc = to_radeon_crtc(test_crtc);
+			if ((radeon_test_crtc->pll_id >= ATOM_PPLL1) &&
+			    (radeon_test_crtc->pll_id <= ATOM_PPLL2))
+				pll_in_use |= (1 << radeon_test_crtc->pll_id);
+		}
+		if (!(pll_in_use & 1))
 			return ATOM_PPLL1;
-		DRM_ERROR("unable to allocate a PPLL\n");
-		return ATOM_PPLL_INVALID;
+		return ATOM_PPLL2;
 	} else
-		/* use PPLL1 or PPLL2 */
 		return radeon_crtc->crtc_id;
 
 }
@@ -1792,7 +1697,7 @@ static void atombios_crtc_disable(struct drm_crtc *crtc)
 		break;
 	}
 done:
-	radeon_crtc->pll_id = ATOM_PPLL_INVALID;
+	radeon_crtc->pll_id = -1;
 }
 
 static const struct drm_crtc_helper_funcs atombios_helper_funcs = {
@@ -1841,6 +1746,6 @@ void radeon_atombios_init_crtc(struct drm_device *dev,
 		else
 			radeon_crtc->crtc_offset = 0;
 	}
-	radeon_crtc->pll_id = ATOM_PPLL_INVALID;
+	radeon_crtc->pll_id = -1;
 	drm_crtc_helper_add(&radeon_crtc->base, &atombios_helper_funcs);
 }
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index e93b80a6d4e9..ed3340adeb6f 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -77,13 +77,9 @@ void evergreen_tiling_fields(unsigned tiling_flags, unsigned *bankw,
 void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 {
 	u16 ctl, v;
-	int cap, err;
+	int err;
 
-	cap = pci_pcie_cap(rdev->pdev);
-	if (!cap)
-		return;
-
-	err = pci_read_config_word(rdev->pdev, cap + PCI_EXP_DEVCTL, &ctl);
+	err = pcie_capability_read_word(rdev->pdev, PCI_EXP_DEVCTL, &ctl);
 	if (err)
 		return;
 
@@ -95,7 +91,7 @@ void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 	if ((v == 0) || (v == 6) || (v == 7)) {
 		ctl &= ~PCI_EXP_DEVCTL_READRQ;
 		ctl |= (2 << 12);
-		pci_write_config_word(rdev->pdev, cap + PCI_EXP_DEVCTL, ctl);
+		pcie_capability_write_word(rdev->pdev, PCI_EXP_DEVCTL, ctl);
 	}
 }
 
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 8acb34fd3fd5..8d7e33a0b243 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -1182,7 +1182,8 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	ring->ready = true;
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.real_vram_size);
 
-	if (radeon_ring_supports_scratch_reg(rdev, ring)) {
+	if (!ring->rptr_save_reg /* not resuming from suspend */
+	    && radeon_ring_supports_scratch_reg(rdev, ring)) {
 		r = radeon_scratch_get(rdev, &ring->rptr_save_reg);
 		if (r) {
 			DRM_ERROR("failed to get scratch reg for rptr save (%d).\n", r);
diff --git a/drivers/gpu/drm/udl/udl_connector.c b/drivers/gpu/drm/udl/udl_connector.c
index ba055e9ca007..8d9dc44f1f94 100644
--- a/drivers/gpu/drm/udl/udl_connector.c
+++ b/drivers/gpu/drm/udl/udl_connector.c
@@ -69,6 +69,13 @@ static int udl_get_modes(struct drm_connector *connector)
 static int udl_mode_valid(struct drm_connector *connector,
 			  struct drm_display_mode *mode)
 {
+	struct udl_device *udl = connector->dev->dev_private;
+	if (!udl->sku_pixel_limit)
+		return 0;
+
+	if (mode->vdisplay * mode->hdisplay > udl->sku_pixel_limit)
+		return MODE_VIRTUAL_Y;
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
index f2fb8f15e2f1..7e0743358dff 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
@@ -1018,7 +1018,7 @@ int vmw_event_fence_action_create(struct drm_file *file_priv,
 	}
 
 
-	event = kzalloc(sizeof(event->event), GFP_KERNEL);
+	event = kzalloc(sizeof(*event), GFP_KERNEL);
 	if (unlikely(event == NULL)) {
 		DRM_ERROR("Failed to allocate an event.\n");
 		ret = -ENOMEM;
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 3df8fc0ec01a..e893f6e1937d 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -141,7 +141,11 @@ EXPORT_SYMBOL_GPL(vga_default_device);
 
 void vga_set_default_device(struct pci_dev *pdev)
 {
-	vga_default = pdev;
+	if (vga_default == pdev)
+		return;
+
+	pci_dev_put(vga_default);
+	vga_default = pci_dev_get(pdev);
 }
 #endif
 
@@ -577,7 +581,7 @@ static bool vga_arbiter_add_pci_device(struct pci_dev *pdev)
 #ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 	if (vga_default == NULL &&
 	    ((vgadev->owns & VGA_RSRC_LEGACY_MASK) == VGA_RSRC_LEGACY_MASK))
-		vga_default = pci_dev_get(pdev);
+		vga_set_default_device(pdev);
 #endif
 
 	vga_arbiter_check_bridge_sharing(vgadev);
@@ -613,10 +617,8 @@ static bool vga_arbiter_del_pci_device(struct pci_dev *pdev)
 	}
 
 #ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
-	if (vga_default == pdev) {
-		pci_dev_put(vga_default);
-		vga_default = NULL;
-	}
+	if (vga_default == pdev)
+		vga_set_default_device(NULL);
 #endif
 
 	if (vgadev->decodes & (VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM))
@@ -1066,7 +1068,6 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
 		}
 
 	} else if (strncmp(curr_pos, "target ", 7) == 0) {
-		struct pci_bus *pbus;
 		unsigned int domain, bus, devfn;
 		struct vga_device *vgadev;
 
@@ -1085,19 +1086,11 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
 			pr_debug("vgaarb: %s ==> %x:%x:%x.%x\n", curr_pos,
 				domain, bus, PCI_SLOT(devfn), PCI_FUNC(devfn));
 
-			pbus = pci_find_bus(domain, bus);
-			pr_debug("vgaarb: pbus %p\n", pbus);
-			if (pbus == NULL) {
-				pr_err("vgaarb: invalid PCI domain and/or bus address %x:%x\n",
-					domain, bus);
-				ret_val = -ENODEV;
-				goto done;
-			}
-			pdev = pci_get_slot(pbus, devfn);
+			pdev = pci_get_domain_bus_and_slot(domain, bus, devfn);
 			pr_debug("vgaarb: pdev %p\n", pdev);
 			if (!pdev) {
-				pr_err("vgaarb: invalid PCI address %x:%x\n",
-					bus, devfn);
+				pr_err("vgaarb: invalid PCI address %x:%x:%x\n",
+					domain, bus, devfn);
 				ret_val = -ENODEV;
 				goto done;
 			}
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index fbf49503508d..1630150ad2b1 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -307,7 +307,6 @@ config HID_LOGITECH
 config HID_LOGITECH_DJ
 	tristate "Logitech Unifying receivers full support"
 	depends on HID_LOGITECH
-	default m
 	---help---
 	Say Y if you want support for Logitech Unifying receivers and devices.
 	Unifying receivers are capable of pairing up to 6 Logitech compliant
@@ -527,6 +526,14 @@ config HID_PICOLCD_LEDS
 	---help---
 	  Provide access to PicoLCD's GPO pins via leds class.
 
+config HID_PICOLCD_CIR
+	bool "CIR via RC class" if EXPERT
+	default !EXPERT
+	depends on HID_PICOLCD
+	depends on HID_PICOLCD=RC_CORE || RC_CORE=y
+	---help---
+	  Provide access to PicoLCD's CIR interface via remote control (LIRC).
+
 config HID_PRIMAX
 	tristate "Primax non-fully HID-compliant devices"
 	depends on USB_HID
@@ -534,6 +541,15 @@ config HID_PRIMAX
 	Support for Primax devices that are not fully compliant with the
 	HID standard.
 
+config HID_PS3REMOTE
+	tristate "Sony PS3 BD Remote Control"
+	depends on BT_HIDP
+	---help---
+	Support for the Sony PS3 Blue-ray Disk Remote Control and Logitech
+	Harmony Adapter for PS3, which connect over Bluetooth.
+
+	Support for the 6-axis controllers is provided by HID_SONY.
+
 config HID_ROCCAT
 	tristate "Roccat device support"
 	depends on USB_HID
@@ -561,7 +577,9 @@ config HID_SONY
 	tristate "Sony PS3 controller"
 	depends on USB_HID
 	---help---
-	Support for Sony PS3 controller.
+	Support for Sony PS3 6-axis controllers.
+
+	Support for the Sony PS3 BD Remote is provided by HID_PS3REMOTE.
 
 config HID_SPEEDLINK
 	tristate "Speedlink VAD Cezanne mouse support"
@@ -690,6 +708,20 @@ config HID_ZYDACRON
 	---help---
 	Support for Zydacron remote control.
 
+config HID_SENSOR_HUB
+	tristate "HID Sensors framework support"
+	depends on USB_HID
+	select MFD_CORE
+	default n
+	-- help---
+	  Support for HID Sensor framework. This creates a MFD instance
+	  for a sensor hub and identifies all the sensors connected to it.
+	  Each sensor is registered as a MFD cell, so that sensor specific
+	  processing can be done in a separate driver. Each sensor
+	  drivers can use the service provided by this driver to register
+	  for events and handle data streams. Each sensor driver can format
+	  data and present to user mode using input or IIO interface.
+
 endmenu
 
 endif # HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index f975485f88b2..cef68ca859d3 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -69,7 +69,28 @@ obj-$(CONFIG_HID_PRODIKEYS)	+= hid-prodikeys.o
 obj-$(CONFIG_HID_PANTHERLORD)	+= hid-pl.o
 obj-$(CONFIG_HID_PETALYNX)	+= hid-petalynx.o
 obj-$(CONFIG_HID_PICOLCD)	+= hid-picolcd.o
+hid-picolcd-y			+= hid-picolcd_core.o
+ifdef CONFIG_HID_PICOLCD_FB
+hid-picolcd-y			+= hid-picolcd_fb.o
+endif
+ifdef CONFIG_HID_PICOLCD_BACKLIGHT
+hid-picolcd-y			+= hid-picolcd_backlight.o
+endif
+ifdef CONFIG_HID_PICOLCD_LCD
+hid-picolcd-y			+= hid-picolcd_lcd.o
+endif
+ifdef CONFIG_HID_PICOLCD_LEDS
+hid-picolcd-y			+= hid-picolcd_leds.o
+endif
+ifdef CONFIG_HID_PICOLCD_CIR
+hid-picolcd-y			+= hid-picolcd_cir.o
+endif
+ifdef CONFIG_DEBUG_FS
+hid-picolcd-y			+= hid-picolcd_debugfs.o
+endif
+
 obj-$(CONFIG_HID_PRIMAX)	+= hid-primax.o
+obj-$(CONFIG_HID_PS3REMOTE)	+= hid-ps3remote.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o hid-roccat-common.o \
 	hid-roccat-arvo.o hid-roccat-isku.o hid-roccat-kone.o \
 	hid-roccat-koneplus.o hid-roccat-kovaplus.o hid-roccat-pyra.o \
@@ -91,6 +112,7 @@ obj-$(CONFIG_HID_ZYDACRON)	+= hid-zydacron.o
 obj-$(CONFIG_HID_WACOM)		+= hid-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_USB_HID)		+= usbhid/
 obj-$(CONFIG_USB_MOUSE)		+= usbhid/
diff --git a/drivers/hid/hid-a4tech.c b/drivers/hid/hid-a4tech.c
index 902d1dfeb1b5..0a239885e67c 100644
--- a/drivers/hid/hid-a4tech.c
+++ b/drivers/hid/hid-a4tech.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-apple.c b/drivers/hid/hid-apple.c
index 585344b6d338..06ebdbb6ea02 100644
--- a/drivers/hid/hid-apple.c
+++ b/drivers/hid/hid-apple.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby <jirislaby@gmail.com>
  */
 
diff --git a/drivers/hid/hid-aureal.c b/drivers/hid/hid-aureal.c
index ba64b041b8bf..7968187ddf7b 100644
--- a/drivers/hid/hid-aureal.c
+++ b/drivers/hid/hid-aureal.c
@@ -9,7 +9,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 #include <linux/device.h>
diff --git a/drivers/hid/hid-belkin.c b/drivers/hid/hid-belkin.c
index a1a765a5b08a..a1a5a12c3a6b 100644
--- a/drivers/hid/hid-belkin.c
+++ b/drivers/hid/hid-belkin.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-cherry.c b/drivers/hid/hid-cherry.c
index 888ece68a47c..af034d3d9256 100644
--- a/drivers/hid/hid-cherry.c
+++ b/drivers/hid/hid-cherry.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 8bcd168fffae..bd3971bf31bf 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -126,7 +126,7 @@ static int open_collection(struct hid_parser *parser, unsigned type)
 
 	if (parser->collection_stack_ptr == HID_COLLECTION_STACK_SIZE) {
 		hid_err(parser->device, "collection stack overflow\n");
-		return -1;
+		return -EINVAL;
 	}
 
 	if (parser->device->maxcollection == parser->device->collection_size) {
@@ -134,7 +134,7 @@ static int open_collection(struct hid_parser *parser, unsigned type)
 				parser->device->collection_size * 2, GFP_KERNEL);
 		if (collection == NULL) {
 			hid_err(parser->device, "failed to reallocate collection array\n");
-			return -1;
+			return -ENOMEM;
 		}
 		memcpy(collection, parser->device->collection,
 			sizeof(struct hid_collection) *
@@ -170,7 +170,7 @@ static int close_collection(struct hid_parser *parser)
 {
 	if (!parser->collection_stack_ptr) {
 		hid_err(parser->device, "collection stack underflow\n");
-		return -1;
+		return -EINVAL;
 	}
 	parser->collection_stack_ptr--;
 	return 0;
@@ -374,7 +374,7 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 
 	case HID_GLOBAL_ITEM_TAG_REPORT_SIZE:
 		parser->global.report_size = item_udata(item);
-		if (parser->global.report_size > 96) {
+		if (parser->global.report_size > 128) {
 			hid_err(parser->device, "invalid report_size %d\n",
 					parser->global.report_size);
 			return -1;
@@ -757,6 +757,7 @@ int hid_open_report(struct hid_device *device)
 	struct hid_item item;
 	unsigned int size;
 	__u8 *start;
+	__u8 *buf;
 	__u8 *end;
 	int ret;
 	static int (*dispatch_type[])(struct hid_parser *parser,
@@ -775,12 +776,21 @@ int hid_open_report(struct hid_device *device)
 		return -ENODEV;
 	size = device->dev_rsize;
 
+	buf = kmemdup(start, size, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
 	if (device->driver->report_fixup)
-		start = device->driver->report_fixup(device, start, &size);
+		start = device->driver->report_fixup(device, buf, &size);
+	else
+		start = buf;
 
-	device->rdesc = kmemdup(start, size, GFP_KERNEL);
-	if (device->rdesc == NULL)
+	start = kmemdup(start, size, GFP_KERNEL);
+	kfree(buf);
+	if (start == NULL)
 		return -ENOMEM;
+
+	device->rdesc = start;
 	device->rsize = size;
 
 	parser = vzalloc(sizeof(struct hid_parser));
@@ -1448,7 +1458,14 @@ void hid_disconnect(struct hid_device *hdev)
 }
 EXPORT_SYMBOL_GPL(hid_disconnect);
 
-/* a list of devices for which there is a specialized driver on HID bus */
+/*
+ * A list of devices for which there is a specialized driver on HID bus.
+ *
+ * Please note that for multitouch devices (driven by hid-multitouch driver),
+ * there is a proper autodetection and autoloading in place (based on presence
+ * of HID_DG_CONTACTID), so those devices don't need to be added to this list,
+ * as we are doing the right thing in hid_scan_usage().
+ */
 static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_WCP32PU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_X5_005D) },
@@ -1551,6 +1568,10 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK, USB_DEVICE_ID_HOLTEK_ON_LINE_GRIP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8086, USB_DEVICE_ID_SENSOR_HUB_1020) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8086, USB_DEVICE_ID_SENSOR_HUB_09FA) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8087, USB_DEVICE_ID_SENSOR_HUB_1020) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8087, USB_DEVICE_ID_SENSOR_HUB_09FA) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KENSINGTON, USB_DEVICE_ID_KS_SLIMBLADE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KEYTOUCH, USB_DEVICE_ID_KEYTOUCH_IEC) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
@@ -1566,6 +1587,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RECEIVER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_DESKTOP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_EDGE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_MINI) },
@@ -1627,6 +1649,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
+#if IS_ENABLED(CONFIG_HID_ROCCAT)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
@@ -1635,15 +1658,18 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRED) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRELESS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_SAVU) },
+#endif
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_IR_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SKYCABLE, USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_BDREMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SUNPLUS, USB_DEVICE_ID_SUNPLUS_WDESKTOP) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_STM, USB_DEVICE_ID_SENSOR_HUB_7014) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb300) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb304) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb323) },
@@ -1663,6 +1689,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, 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_WISEGROUP, USB_DEVICE_ID_SMARTJOY_PLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SUPER_JOY_BOX_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD) },
diff --git a/drivers/hid/hid-cypress.c b/drivers/hid/hid-cypress.c
index 9e43aaca9774..3e159a50dac7 100644
--- a/drivers/hid/hid-cypress.c
+++ b/drivers/hid/hid-cypress.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 01dd9a7daf7a..933fff0fff1f 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -911,15 +911,21 @@ static void hid_dump_input_mapping(struct hid_device *hid, struct seq_file *f)
 
 }
 
-
 static int hid_debug_rdesc_show(struct seq_file *f, void *p)
 {
 	struct hid_device *hdev = f->private;
+	const __u8 *rdesc = hdev->rdesc;
+	unsigned rsize = hdev->rsize;
 	int i;
 
+	if (!rdesc) {
+		rdesc = hdev->dev_rdesc;
+		rsize = hdev->dev_rsize;
+	}
+
 	/* dump HID report descriptor */
-	for (i = 0; i < hdev->rsize; i++)
-		seq_printf(f, "%02x ", hdev->rdesc[i]);
+	for (i = 0; i < rsize; i++)
+		seq_printf(f, "%02x ", rdesc[i]);
 	seq_printf(f, "\n\n");
 
 	/* dump parsed data and input mappings */
diff --git a/drivers/hid/hid-ezkey.c b/drivers/hid/hid-ezkey.c
index ca1163e9d42d..6540af2871a7 100644
--- a/drivers/hid/hid-ezkey.c
+++ b/drivers/hid/hid-ezkey.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-gyration.c b/drivers/hid/hid-gyration.c
index e88b951cd10d..4442c30ef531 100644
--- a/drivers/hid/hid-gyration.c
+++ b/drivers/hid/hid-gyration.c
@@ -4,7 +4,6 @@
  *  Copyright (c) 1999 Andreas Gal
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  *  Copyright (c) 2006-2008 Jiri Kosina
  */
diff --git a/drivers/hid/hid-holtekff.c b/drivers/hid/hid-holtekff.c
index 4e7542151e22..ff295e60059b 100644
--- a/drivers/hid/hid-holtekff.c
+++ b/drivers/hid/hid-holtekff.c
@@ -100,8 +100,7 @@ static void holtekff_send(struct holtekff_device *holtekff,
 		holtekff->field->value[i] = data[i];
 	}
 
-	dbg_hid("sending %02x %02x %02x %02x %02x %02x %02x\n", data[0],
-		data[1], data[2], data[3], data[4], data[5], data[6]);
+	dbg_hid("sending %*ph\n", 7, data);
 
 	usbhid_submit_report(hid, holtekff->field->report, USB_DIR_OUT);
 }
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 1dcb76ff51e3..269b50912a4a 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  */
 
 /*
@@ -269,7 +268,11 @@
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72FA	0x72fa
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302	0x7302
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7349	0x7349
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_73F7	0x73f7
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001	0xa001
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7224      0x7224
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72D0      0x72d0
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72C4      0x72c4
 
 #define USB_VENDOR_ID_ELECOM		0x056e
 #define USB_DEVICE_ID_ELECOM_BM084	0x0061
@@ -283,6 +286,9 @@
 #define USB_VENDOR_ID_EMS		0x2006
 #define USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II 0x0118
 
+#define USB_VENDOR_ID_FLATFROG		0x25b5
+#define USB_DEVICE_ID_MULTITOUCH_3200	0x0002
+
 #define USB_VENDOR_ID_ESSENTIAL_REALITY	0x0d7f
 #define USB_DEVICE_ID_ESSENTIAL_REALITY_P5 0x0100
 
@@ -296,6 +302,9 @@
 #define USB_VENDOR_ID_EZKEY		0x0518
 #define USB_DEVICE_ID_BTC_8193		0x0002
 
+#define USB_VENDOR_ID_FREESCALE		0x15A2
+#define USB_DEVICE_ID_FREESCALE_MX28	0x004F
+
 #define USB_VENDOR_ID_FRUCTEL	0x25B6
 #define USB_DEVICE_ID_GAMETEL_MT_MODE	0x0002
 
@@ -305,6 +314,7 @@
 
 #define USB_VENDOR_ID_GENERAL_TOUCH	0x0dfc
 #define USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS 0x0003
+#define USB_DEVICE_ID_GENERAL_TOUCH_WIN8_PWT_TENFINGERS 0x0100
 
 #define USB_VENDOR_ID_GLAB		0x06c2
 #define USB_DEVICE_ID_4_PHIDGETSERVO_30	0x0038
@@ -419,6 +429,11 @@
 #define USB_VENDOR_ID_IMATION		0x0718
 #define USB_DEVICE_ID_DISC_STAKKA	0xd000
 
+#define USB_VENDOR_ID_INTEL_8086	0x8086
+#define USB_VENDOR_ID_INTEL_8087	0x8087
+#define USB_DEVICE_ID_SENSOR_HUB_1020	0x1020
+#define USB_DEVICE_ID_SENSOR_HUB_09FA	0x09FA
+
 #define USB_VENDOR_ID_IRTOUCHSYSTEMS	0x6615
 #define USB_DEVICE_ID_IRTOUCH_INFRARED_USB	0x0070
 
@@ -496,6 +511,7 @@
 #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_RUMBLEPAD_CORD	0xc20a
 #define USB_DEVICE_ID_LOGITECH_RUMBLEPAD	0xc211
 #define USB_DEVICE_ID_LOGITECH_EXTREME_3D	0xc215
@@ -652,7 +668,6 @@
 #define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH		0x3000
 #define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001		0x3001
 #define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008		0x3008
-#define USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN	0x3001
 
 #define USB_VENDOR_ID_ROCCAT		0x1e7d
 #define USB_DEVICE_ID_ROCCAT_ARVO	0x30d4
@@ -683,6 +698,7 @@
 
 #define USB_VENDOR_ID_SONY			0x054c
 #define USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE	0x024b
+#define USB_DEVICE_ID_SONY_PS3_BDREMOTE		0x0306
 #define USB_DEVICE_ID_SONY_PS3_CONTROLLER	0x0268
 #define USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER	0x042f
 
@@ -695,6 +711,7 @@
 
 #define USB_VENDOR_ID_STANTUM_STM		0x0483
 #define USB_DEVICE_ID_MTP_STM		0x3261
+#define USB_DEVICE_ID_SENSOR_HUB_7014	0x7014
 
 #define USB_VENDOR_ID_STANTUM_SITRONIX		0x1403
 #define USB_DEVICE_ID_MTP_SITRONIX		0x5001
@@ -758,6 +775,7 @@
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U	0x0005
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP1062	0x0064
 #define USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850	0x0522
+#define USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60	0x0781
 
 #define USB_VENDOR_ID_UNITEC	0x227d
 #define USB_DEVICE_ID_UNITEC_USB_TOUCH_0709	0x0709
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 811bfad64609..d917c0d53685 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -1154,6 +1154,7 @@ static void report_features(struct hid_device *hid)
 
 int hidinput_connect(struct hid_device *hid, unsigned int force)
 {
+	struct hid_driver *drv = hid->driver;
 	struct hid_report *report;
 	struct hid_input *hidinput = NULL;
 	struct input_dev *input_dev;
@@ -1228,6 +1229,8 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 				 * UGCI) cram a lot of unrelated inputs into the
 				 * same interface. */
 				hidinput->report = report;
+				if (drv->input_configured)
+					drv->input_configured(hid, hidinput);
 				if (input_register_device(hidinput->input))
 					goto out_cleanup;
 				hidinput = NULL;
@@ -1235,8 +1238,12 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 		}
 	}
 
-	if (hidinput && input_register_device(hidinput->input))
-		goto out_cleanup;
+	if (hidinput) {
+		if (drv->input_configured)
+			drv->input_configured(hid, hidinput);
+		if (input_register_device(hidinput->input))
+			goto out_cleanup;
+	}
 
 	return 0;
 
diff --git a/drivers/hid/hid-lcpower.c b/drivers/hid/hid-lcpower.c
index c4fe9bd095b7..22bc14abdfa3 100644
--- a/drivers/hid/hid-lcpower.c
+++ b/drivers/hid/hid-lcpower.c
@@ -24,7 +24,7 @@ static int ts_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
-	if ((usage->hid & HID_USAGE_PAGE) != 0x0ffbc0000)
+	if ((usage->hid & HID_USAGE_PAGE) != HID_UP_LOGIVENDOR)
 		return 0;
 
 	switch (usage->hid & HID_USAGE) {
diff --git a/drivers/hid/hid-lenovo-tpkbd.c b/drivers/hid/hid-lenovo-tpkbd.c
index 77d2df04c97b..cea016e94f43 100644
--- a/drivers/hid/hid-lenovo-tpkbd.c
+++ b/drivers/hid/hid-lenovo-tpkbd.c
@@ -56,9 +56,8 @@ static int tpkbd_input_mapping(struct hid_device *hdev,
 static int tpkbd_features_set(struct hid_device *hdev)
 {
 	struct hid_report *report;
-	struct tpkbd_data_pointer *data_pointer;
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
 	report = hdev->report_enum[HID_FEATURE_REPORT].report_id_hash[4];
 
 	report->field[0]->value[0]  = data_pointer->press_to_select   ? 0x01 : 0x02;
@@ -77,14 +76,8 @@ static ssize_t pointer_press_to_select_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
-
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->press_to_select);
 }
@@ -94,16 +87,10 @@ static ssize_t pointer_press_to_select_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value))
 		return -EINVAL;
 	if (value < 0 || value > 1)
@@ -119,14 +106,8 @@ static ssize_t pointer_dragging_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
-
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->dragging);
 }
@@ -136,16 +117,10 @@ static ssize_t pointer_dragging_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value))
 		return -EINVAL;
 	if (value < 0 || value > 1)
@@ -161,14 +136,8 @@ static ssize_t pointer_release_to_select_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
-
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->release_to_select);
 }
@@ -178,16 +147,10 @@ static ssize_t pointer_release_to_select_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value))
 		return -EINVAL;
 	if (value < 0 || value > 1)
@@ -203,14 +166,8 @@ static ssize_t pointer_select_right_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
-
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->select_right);
 }
@@ -220,16 +177,10 @@ static ssize_t pointer_select_right_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value))
 		return -EINVAL;
 	if (value < 0 || value > 1)
@@ -245,14 +196,8 @@ static ssize_t pointer_sensitivity_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
-
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n",
 		data_pointer->sensitivity);
@@ -263,16 +208,10 @@ static ssize_t pointer_sensitivity_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value) || value < 1 || value > 255)
 		return -EINVAL;
 
@@ -286,14 +225,10 @@ static ssize_t pointer_press_speed_show(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n",
 		data_pointer->press_speed);
@@ -304,16 +239,10 @@ static ssize_t pointer_press_speed_store(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
-	hdev = container_of(dev, struct hid_device, dev);
-	if (hdev == NULL)
-		return -ENODEV;
-
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (kstrtoint(buf, 10, &value) || value < 1 || value > 255)
 		return -EINVAL;
 
@@ -370,15 +299,11 @@ static const struct attribute_group tpkbd_attr_group_pointer = {
 static enum led_brightness tpkbd_led_brightness_get(
 			struct led_classdev *led_cdev)
 {
-	struct device *dev;
-	struct hid_device *hdev;
-	struct tpkbd_data_pointer *data_pointer;
+	struct device *dev = led_cdev->dev->parent;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	int led_nr = 0;
 
-	dev = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (led_cdev == &data_pointer->led_micmute)
 		led_nr = 1;
 
@@ -390,16 +315,12 @@ static enum led_brightness tpkbd_led_brightness_get(
 static void tpkbd_led_brightness_set(struct led_classdev *led_cdev,
 			enum led_brightness value)
 {
-	struct device *dev;
-	struct hid_device *hdev;
+	struct device *dev = led_cdev->dev->parent;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 	struct hid_report *report;
-	struct tpkbd_data_pointer *data_pointer;
 	int led_nr = 0;
 
-	dev = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	if (led_cdev == &data_pointer->led_micmute)
 		led_nr = 1;
 
@@ -508,17 +429,17 @@ err_free:
 
 static void tpkbd_remove_tp(struct hid_device *hdev)
 {
-	struct tpkbd_data_pointer *data_pointer;
+	struct tpkbd_data_pointer *data_pointer = hid_get_drvdata(hdev);
 
 	sysfs_remove_group(&hdev->dev.kobj,
 			&tpkbd_attr_group_pointer);
 
-	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
-
 	led_classdev_unregister(&data_pointer->led_micmute);
 	led_classdev_unregister(&data_pointer->led_mute);
 
 	hid_set_drvdata(hdev, NULL);
+	kfree(data_pointer->led_micmute.name);
+	kfree(data_pointer->led_mute.name);
 	kfree(data_pointer);
 }
 
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index fc37ed6b108c..a2f8e88b9fa2 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  *  Copyright (c) 2010 Hendrik Iben
  */
@@ -109,7 +108,7 @@ static __u8 dfp_rdesc_fixed[] = {
 static __u8 *lg_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 
 	if ((drv_data->quirks & LG_RDESC) && *rsize >= 90 && rdesc[83] == 0x26 &&
 			rdesc[84] == 0x8c && rdesc[85] == 0x02) {
@@ -278,7 +277,7 @@ static int lg_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		  0,  0,  0,  0,  0,183,184,185,186,187,
 		188,189,190,191,192,193,194,  0,  0,  0
 	};
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 	unsigned int hid = usage->hid;
 
 	if (hdev->product == USB_DEVICE_ID_LOGITECH_RECEIVER &&
@@ -319,7 +318,7 @@ static int lg_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 
 	if ((drv_data->quirks & LG_BAD_RELATIVE_KEYS) && usage->type == EV_KEY &&
 			(field->flags & HID_MAIN_ITEM_RELATIVE))
@@ -335,13 +334,16 @@ static int lg_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 static int lg_event(struct hid_device *hdev, struct hid_field *field,
 		struct hid_usage *usage, __s32 value)
 {
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 
 	if ((drv_data->quirks & LG_INVERT_HWHEEL) && usage->code == REL_HWHEEL) {
 		input_event(field->hidinput->input, usage->type, usage->code,
 				-value);
 		return 1;
 	}
+	if (drv_data->quirks & LG_FF4) {
+		return lg4ff_adjust_input_event(hdev, field, usage, value, drv_data);
+	}
 
 	return 0;
 }
@@ -358,7 +360,7 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		return -ENOMEM;
 	}
 	drv_data->quirks = id->driver_data;
-	
+
 	hid_set_drvdata(hdev, (void *)drv_data);
 
 	if (drv_data->quirks & LG_NOGET)
@@ -380,7 +382,7 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	}
 
 	/* Setup wireless link with Logitech Wii wheel */
-	if(hdev->product == USB_DEVICE_ID_LOGITECH_WII_WHEEL) {
+	if (hdev->product == USB_DEVICE_ID_LOGITECH_WII_WHEEL) {
 		unsigned char buf[] = { 0x00, 0xAF,  0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
 		ret = hdev->hid_output_raw_report(hdev, buf, sizeof(buf), HID_FEATURE_REPORT);
@@ -416,7 +418,7 @@ err_free:
 
 static void lg_remove(struct hid_device *hdev)
 {
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 	if (drv_data->quirks & LG_FF4)
 		lg4ff_deinit(hdev);
 
@@ -476,7 +478,7 @@ static const struct hid_device_id lg_devices[] = {
 		.driver_data = LG_NOGET | LG_FF4 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WII_WHEEL),
 		.driver_data = LG_FF4 },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_FFG ),
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_FFG),
 		.driver_data = LG_FF },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2),
 		.driver_data = LG_FF2 },
diff --git a/drivers/hid/hid-lg.h b/drivers/hid/hid-lg.h
index d64cf8d2751e..142ce3f5f055 100644
--- a/drivers/hid/hid-lg.h
+++ b/drivers/hid/hid-lg.h
@@ -25,9 +25,13 @@ 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
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index f3390ee6105c..d7947c701f30 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -43,6 +43,11 @@
 #define G27_REV_MAJ 0x12
 #define G27_REV_MIN 0x38
 
+#define DFP_X_MIN 0
+#define DFP_X_MAX 16383
+#define DFP_PEDAL_MIN 0
+#define DFP_PEDAL_MAX 255
+
 #define to_hid_device(pdev) container_of(pdev, struct hid_device, dev)
 
 static void hid_lg4ff_set_range_dfp(struct hid_device *hid, u16 range);
@@ -53,6 +58,7 @@ static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *at
 static DEVICE_ATTR(range, S_IRWXU | S_IRWXG | S_IRWXO, lg4ff_range_show, lg4ff_range_store);
 
 struct lg4ff_device_entry {
+	__u32 product_id;
 	__u16 range;
 	__u16 min_range;
 	__u16 max_range;
@@ -129,26 +135,77 @@ static const struct lg4ff_usb_revision lg4ff_revs[] = {
 	{G27_REV_MAJ,  G27_REV_MIN,  &native_g27},	/* G27 */
 };
 
+/* Recalculates X axis value accordingly to currently selected range */
+static __s32 lg4ff_adjust_dfp_x_axis(__s32 value, __u16 range)
+{
+	__u16 max_range;
+	__s32 new_value;
+
+	if (range == 900)
+		return value;
+	else if (range == 200)
+		return value;
+	else if (range < 200)
+		max_range = 200;
+	else
+		max_range = 900;
+
+	new_value = 8192 + mult_frac(value - 8192, max_range, range);
+	if (new_value < 0)
+		return 0;
+	else if (new_value > 16383)
+		return 16383;
+	else
+		return new_value;
+}
+
+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)
+{
+	struct lg4ff_device_entry *entry = drv_data->device_props;
+	__s32 new_value = 0;
+
+	if (!entry) {
+		hid_err(hid, "Device properties not found");
+		return 0;
+	}
+
+	switch (entry->product_id) {
+	case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+		switch (usage->code) {
+		case ABS_X:
+			new_value = lg4ff_adjust_dfp_x_axis(value, entry->range);
+			input_event(field->hidinput->input, usage->type, usage->code, new_value);
+			return 1;
+		default:
+			return 0;
+		}
+	default:
+		return 0;
+	}
+}
+
 static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *effect)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
 	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);
+	__s32 *value = report->field[0]->value;
 	int x;
 
-#define CLAMP(x) if (x < 0) x = 0; if (x > 0xff) x = 0xff
+#define CLAMP(x) do { if (x < 0) x = 0; else if (x > 0xff) x = 0xff; } while (0)
 
 	switch (effect->type) {
 	case FF_CONSTANT:
 		x = effect->u.ramp.start_level + 0x80;	/* 0x80 is no force */
 		CLAMP(x);
-		report->field[0]->value[0] = 0x11;	/* Slot 1 */
-		report->field[0]->value[1] = 0x08;
-		report->field[0]->value[2] = x;
-		report->field[0]->value[3] = 0x80;
-		report->field[0]->value[4] = 0x00;
-		report->field[0]->value[5] = 0x00;
-		report->field[0]->value[6] = 0x00;
+		value[0] = 0x11;	/* Slot 1 */
+		value[1] = 0x08;
+		value[2] = x;
+		value[3] = 0x80;
+		value[4] = 0x00;
+		value[5] = 0x00;
+		value[6] = 0x00;
 
 		usbhid_submit_report(hid, report, USB_DIR_OUT);
 		break;
@@ -163,14 +220,15 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 	struct hid_device *hid = input_get_drvdata(dev);
 	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);
+	__s32 *value = report->field[0]->value;
 
-	report->field[0]->value[0] = 0xfe;
-	report->field[0]->value[1] = 0x0d;
-	report->field[0]->value[2] = magnitude >> 13;
-	report->field[0]->value[3] = magnitude >> 13;
-	report->field[0]->value[4] = magnitude >> 8;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	value[0] = 0xfe;
+	value[1] = 0x0d;
+	value[2] = magnitude >> 13;
+	value[3] = magnitude >> 13;
+	value[4] = magnitude >> 8;
+	value[5] = 0x00;
+	value[6] = 0x00;
 
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 }
@@ -181,16 +239,16 @@ static void hid_lg4ff_set_autocenter_ffex(struct input_dev *dev, u16 magnitude)
 	struct hid_device *hid = input_get_drvdata(dev);
 	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);
+	__s32 *value = report->field[0]->value;
 	magnitude = magnitude * 90 / 65535;
-	
 
-	report->field[0]->value[0] = 0xfe;
-	report->field[0]->value[1] = 0x03;
-	report->field[0]->value[2] = magnitude >> 14;
-	report->field[0]->value[3] = magnitude >> 14;
-	report->field[0]->value[4] = magnitude;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	value[0] = 0xfe;
+	value[1] = 0x03;
+	value[2] = magnitude >> 14;
+	value[3] = magnitude >> 14;
+	value[4] = magnitude;
+	value[5] = 0x00;
+	value[6] = 0x00;
 
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 }
@@ -200,15 +258,17 @@ static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range)
 {
 	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);
+	__s32 *value = report->field[0]->value;
+
 	dbg_hid("G25/G27/DFGT: setting range to %u\n", range);
 
-	report->field[0]->value[0] = 0xf8;
-	report->field[0]->value[1] = 0x81;
-	report->field[0]->value[2] = range & 0x00ff;
-	report->field[0]->value[3] = (range & 0xff00) >> 8;
-	report->field[0]->value[4] = 0x00;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	value[0] = 0xf8;
+	value[1] = 0x81;
+	value[2] = range & 0x00ff;
+	value[3] = (range & 0xff00) >> 8;
+	value[4] = 0x00;
+	value[5] = 0x00;
+	value[6] = 0x00;
 
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 }
@@ -219,16 +279,18 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	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);
 	int start_left, start_right, full_range;
+	__s32 *value = report->field[0]->value;
+
 	dbg_hid("Driving Force Pro: setting range to %u\n", range);
 
 	/* Prepare "coarse" limit command */
-	report->field[0]->value[0] = 0xf8;
-	report->field[0]->value[1] = 0x00; 	/* Set later */
-	report->field[0]->value[2] = 0x00;
-	report->field[0]->value[3] = 0x00;
-	report->field[0]->value[4] = 0x00;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	value[0] = 0xf8;
+	value[1] = 0x00;	/* Set later */
+	value[2] = 0x00;
+	value[3] = 0x00;
+	value[4] = 0x00;
+	value[5] = 0x00;
+	value[6] = 0x00;
 
 	if (range > 200) {
 		report->field[0]->value[1] = 0x03;
@@ -240,13 +302,13 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 
 	/* Prepare "fine" limit command */
-	report->field[0]->value[0] = 0x81;
-	report->field[0]->value[1] = 0x0b;
-	report->field[0]->value[2] = 0x00;
-	report->field[0]->value[3] = 0x00;
-	report->field[0]->value[4] = 0x00;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	value[0] = 0x81;
+	value[1] = 0x0b;
+	value[2] = 0x00;
+	value[3] = 0x00;
+	value[4] = 0x00;
+	value[5] = 0x00;
+	value[6] = 0x00;
 
 	if (range == 200 || range == 900) {	/* Do not apply any fine limit */
 		usbhid_submit_report(hid, report, USB_DIR_OUT);
@@ -257,11 +319,11 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	start_left = (((full_range - range + 1) * 2047) / full_range);
 	start_right = 0xfff - start_left;
 
-	report->field[0]->value[2] = start_left >> 4;
-	report->field[0]->value[3] = start_right >> 4;
-	report->field[0]->value[4] = 0xff;
-	report->field[0]->value[5] = (start_right & 0xe) << 4 | (start_left & 0xe);
-	report->field[0]->value[6] = 0xff;
+	value[2] = start_left >> 4;
+	value[3] = start_right >> 4;
+	value[4] = 0xff;
+	value[5] = (start_right & 0xe) << 4 | (start_left & 0xe);
+	value[6] = 0xff;
 
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 }
@@ -344,14 +406,15 @@ static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
 {
 	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);
-
-	report->field[0]->value[0] = 0xf8;
-	report->field[0]->value[1] = 0x12;
-	report->field[0]->value[2] = leds;
-	report->field[0]->value[3] = 0x00;
-	report->field[0]->value[4] = 0x00;
-	report->field[0]->value[5] = 0x00;
-	report->field[0]->value[6] = 0x00;
+	__s32 *value = report->field[0]->value;
+
+	value[0] = 0xf8;
+	value[1] = 0x12;
+	value[2] = leds;
+	value[3] = 0x00;
+	value[4] = 0x00;
+	value[5] = 0x00;
+	value[6] = 0x00;
 	usbhid_submit_report(hid, report, USB_DIR_OUT);
 }
 
@@ -360,7 +423,7 @@ static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
 {
 	struct device *dev = led_cdev->dev->parent;
 	struct hid_device *hid = container_of(dev, struct hid_device, dev);
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hid);
 	struct lg4ff_device_entry *entry;
 	int i, state = 0;
 
@@ -395,7 +458,7 @@ static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cde
 {
 	struct device *dev = led_cdev->dev->parent;
 	struct hid_device *hid = container_of(dev, struct hid_device, dev);
-	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid);
+	struct lg_drv_data *drv_data = hid_get_drvdata(hid);
 	struct lg4ff_device_entry *entry;
 	int i, value = 0;
 
@@ -501,7 +564,7 @@ int lg4ff_init(struct hid_device *hid)
 	/* 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 */
+		if (rev_maj == FFEX_REV_MAJ && rev_min == FFEX_REV_MIN)	/* Formula Force EX expects different autocentering command */
 			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_ffex;
 		else
 			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_default;
@@ -524,6 +587,7 @@ int lg4ff_init(struct hid_device *hid)
 	}
 	drv_data->device_props = entry;
 
+	entry->product_id = lg4ff_devices[i].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;
@@ -534,6 +598,18 @@ int lg4ff_init(struct hid_device *hid)
 		return error;
 	dbg_hid("sysfs interface created\n");
 
+	/* Set default axes parameters */
+	switch (lg4ff_devices[i].product_id) {
+	case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+		dbg_hid("Setting axes parameters for Driving Force Pro\n");
+		input_set_abs_params(dev, ABS_X, DFP_X_MIN, DFP_X_MAX, 0, 0);
+		input_set_abs_params(dev, ABS_Y, DFP_PEDAL_MIN, DFP_PEDAL_MAX, 0, 0);
+		input_set_abs_params(dev, ABS_RZ, DFP_PEDAL_MIN, DFP_PEDAL_MAX, 0, 0);
+		break;
+	default:
+		break;
+	}
+
 	/* Set the maximum range to start with */
 	entry->range = entry->max_range;
 	if (entry->set_range != NULL)
@@ -594,6 +670,8 @@ out:
 	return 0;
 }
 
+
+
 int lg4ff_deinit(struct hid_device *hid)
 {
 	struct lg4ff_device_entry *entry;
diff --git a/drivers/hid/hid-logitech-dj.c b/drivers/hid/hid-logitech-dj.c
index 4d524b5f52f5..9500f2f3f8fe 100644
--- a/drivers/hid/hid-logitech-dj.c
+++ b/drivers/hid/hid-logitech-dj.c
@@ -193,6 +193,7 @@ static struct hid_ll_driver logi_dj_ll_driver;
 static int logi_dj_output_hidraw_report(struct hid_device *hid, u8 * buf,
 					size_t count,
 					unsigned char report_type);
+static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev);
 
 static void logi_dj_recv_destroy_djhid_device(struct dj_receiver_dev *djrcv_dev,
 						struct dj_report *dj_report)
@@ -233,6 +234,7 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 	if (dj_report->report_params[DEVICE_PAIRED_PARAM_SPFUNCTION] &
 	    SPFUNCTION_DEVICE_LIST_EMPTY) {
 		dbg_hid("%s: device list is empty\n", __func__);
+		djrcv_dev->querying_devices = false;
 		return;
 	}
 
@@ -243,6 +245,12 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 		return;
 	}
 
+	if (djrcv_dev->paired_dj_devices[dj_report->device_index]) {
+		/* The device is already known. No need to reallocate it. */
+		dbg_hid("%s: device is already known\n", __func__);
+		return;
+	}
+
 	dj_hiddev = hid_allocate_device();
 	if (IS_ERR(dj_hiddev)) {
 		dev_err(&djrcv_hdev->dev, "%s: hid_allocate_device failed\n",
@@ -306,6 +314,7 @@ static void delayedwork_callback(struct work_struct *work)
 	struct dj_report dj_report;
 	unsigned long flags;
 	int count;
+	int retval;
 
 	dbg_hid("%s\n", __func__);
 
@@ -338,6 +347,25 @@ static void delayedwork_callback(struct work_struct *work)
 		logi_dj_recv_destroy_djhid_device(djrcv_dev, &dj_report);
 		break;
 	default:
+	/* A normal report (i. e. not belonging to a pair/unpair notification)
+	 * arriving here, means that the report arrived but we did not have a
+	 * paired dj_device associated to the report's device_index, this
+	 * means that the original "device paired" notification corresponding
+	 * to this dj_device never arrived to this driver. The reason is that
+	 * hid-core discards all packets coming from a device while probe() is
+	 * executing. */
+	if (!djrcv_dev->paired_dj_devices[dj_report.device_index]) {
+		/* ok, we don't know the device, just re-ask the
+		 * receiver for the list of connected devices. */
+		retval = logi_dj_recv_query_paired_devices(djrcv_dev);
+		if (!retval) {
+			/* everything went fine, so just leave */
+			break;
+		}
+		dev_err(&djrcv_dev->hdev->dev,
+			"%s:logi_dj_recv_query_paired_devices "
+			"error:%d\n", __func__, retval);
+		}
 		dbg_hid("%s: unexpected report type\n", __func__);
 	}
 }
@@ -368,6 +396,12 @@ static void logi_dj_recv_forward_null_report(struct dj_receiver_dev *djrcv_dev,
 	if (!djdev) {
 		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
 			" is NULL, index %d\n", dj_report->device_index);
+		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
+
+		if (schedule_work(&djrcv_dev->work) == 0) {
+			dbg_hid("%s: did not schedule the work item, was already "
+			"queued\n", __func__);
+		}
 		return;
 	}
 
@@ -398,6 +432,12 @@ static void logi_dj_recv_forward_report(struct dj_receiver_dev *djrcv_dev,
 	if (dj_device == NULL) {
 		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
 			" is NULL, index %d\n", dj_report->device_index);
+		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
+
+		if (schedule_work(&djrcv_dev->work) == 0) {
+			dbg_hid("%s: did not schedule the work item, was already "
+			"queued\n", __func__);
+		}
 		return;
 	}
 
@@ -439,6 +479,10 @@ static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev)
 	struct dj_report *dj_report;
 	int retval;
 
+	/* no need to protect djrcv_dev->querying_devices */
+	if (djrcv_dev->querying_devices)
+		return 0;
+
 	dj_report = kzalloc(sizeof(struct dj_report), GFP_KERNEL);
 	if (!dj_report)
 		return -ENOMEM;
@@ -450,6 +494,7 @@ static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev)
 	return retval;
 }
 
+
 static int logi_dj_recv_switch_to_dj_mode(struct dj_receiver_dev *djrcv_dev,
 					  unsigned timeout)
 {
diff --git a/drivers/hid/hid-logitech-dj.h b/drivers/hid/hid-logitech-dj.h
index fd28a5e0ca3b..4a4000340ce1 100644
--- a/drivers/hid/hid-logitech-dj.h
+++ b/drivers/hid/hid-logitech-dj.h
@@ -101,6 +101,7 @@ struct dj_receiver_dev {
 	struct work_struct work;
 	struct kfifo notif_fifo;
 	spinlock_t lock;
+	bool querying_devices;
 };
 
 struct dj_device {
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index 73647266daad..25ddf3e3aec6 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -392,7 +392,7 @@ static int magicmouse_setup_input(struct input_dev *input, struct hid_device *hd
 
 	__set_bit(EV_ABS, input->evbit);
 
-	error = input_mt_init_slots(input, 16);
+	error = input_mt_init_slots(input, 16, 0);
 	if (error)
 		return error;
 	input_set_abs_params(input, ABS_MT_TOUCH_MAJOR, 0, 255 << 2,
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index e5c699b6c6f3..3acdcfcc17df 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-monterey.c b/drivers/hid/hid-monterey.c
index dedf757781ae..cd3643e06fa6 100644
--- a/drivers/hid/hid-monterey.c
+++ b/drivers/hid/hid-monterey.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 59c8b5c1d2de..3eb02b94fc87 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -51,12 +51,12 @@ MODULE_LICENSE("GPL");
 #define MT_QUIRK_VALID_IS_INRANGE	(1 << 5)
 #define MT_QUIRK_VALID_IS_CONFIDENCE	(1 << 6)
 #define MT_QUIRK_SLOT_IS_CONTACTID_MINUS_ONE	(1 << 8)
+#define MT_QUIRK_NO_AREA		(1 << 9)
 
 struct mt_slot {
 	__s32 x, y, p, w, h;
 	__s32 contactid;	/* the device ContactID assigned to this slot */
 	bool touch_state;	/* is the touch valid? */
-	bool seen_in_this_frame;/* has this slot been updated */
 };
 
 struct mt_class {
@@ -92,8 +92,9 @@ 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 */
+	bool serial_maybe;	/* need to check for serial protocol */
 	bool curvalid;		/* is the current contact valid? */
-	struct mt_slot *slots;
+	unsigned mt_flags;	/* flags to pass to input-mt */
 };
 
 /* classes of device behavior */
@@ -115,6 +116,9 @@ struct mt_device {
 #define MT_CLS_EGALAX_SERIAL			0x0104
 #define MT_CLS_TOPSEED				0x0105
 #define MT_CLS_PANASONIC			0x0106
+#define MT_CLS_FLATFROG				0x0107
+#define MT_CLS_GENERALTOUCH_TWOFINGERS		0x0108
+#define MT_CLS_GENERALTOUCH_PWT_TENFINGERS	0x0109
 
 #define MT_DEFAULT_MAXCONTACT	10
 
@@ -134,25 +138,6 @@ static int cypress_compute_slot(struct mt_device *td)
 		return -1;
 }
 
-static int find_slot_from_contactid(struct mt_device *td)
-{
-	int i;
-	for (i = 0; i < td->maxcontacts; ++i) {
-		if (td->slots[i].contactid == td->curdata.contactid &&
-			td->slots[i].touch_state)
-			return i;
-	}
-	for (i = 0; i < td->maxcontacts; ++i) {
-		if (!td->slots[i].seen_in_this_frame &&
-			!td->slots[i].touch_state)
-			return i;
-	}
-	/* should not occurs. If this happens that means
-	 * that the device sent more touches that it says
-	 * in the report descriptor. It is ignored then. */
-	return -1;
-}
-
 static struct mt_class mt_classes[] = {
 	{ .name = MT_CLS_DEFAULT,
 		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP },
@@ -190,7 +175,9 @@ static struct mt_class mt_classes[] = {
 			MT_QUIRK_SLOT_IS_CONTACTID,
 		.sn_move = 2048,
 		.sn_width = 128,
-		.sn_height = 128 },
+		.sn_height = 128,
+		.maxcontacts = 60,
+	},
 	{ .name = MT_CLS_CYPRESS,
 		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP |
 			MT_QUIRK_CYPRESS,
@@ -215,7 +202,24 @@ static struct mt_class mt_classes[] = {
 	{ .name = MT_CLS_PANASONIC,
 		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP,
 		.maxcontacts = 4 },
+	{ .name	= MT_CLS_GENERALTOUCH_TWOFINGERS,
+		.quirks	= MT_QUIRK_NOT_SEEN_MEANS_UP |
+			MT_QUIRK_VALID_IS_INRANGE |
+			MT_QUIRK_SLOT_IS_CONTACTNUMBER,
+		.maxcontacts = 2
+	},
+	{ .name	= MT_CLS_GENERALTOUCH_PWT_TENFINGERS,
+		.quirks	= MT_QUIRK_NOT_SEEN_MEANS_UP |
+			MT_QUIRK_SLOT_IS_CONTACTNUMBER,
+		.maxcontacts = 10
+	},
 
+	{ .name = MT_CLS_FLATFROG,
+		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP |
+			MT_QUIRK_NO_AREA,
+		.sn_move = 2048,
+		.maxcontacts = 40,
+	},
 	{ }
 };
 
@@ -319,24 +323,16 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	* We need to ignore fields that belong to other collections
 	* such as Mouse that might have the same GenericDesktop usages. */
 	if (field->application == HID_DG_TOUCHSCREEN)
-		set_bit(INPUT_PROP_DIRECT, hi->input->propbit);
+		td->mt_flags |= INPUT_MT_DIRECT;
 	else if (field->application != HID_DG_TOUCHPAD)
 		return 0;
 
-	/* In case of an indirect device (touchpad), we need to add
-	 * specific BTN_TOOL_* to be handled by the synaptics xorg
-	 * driver.
-	 * We also consider that touchscreens providing buttons are touchpads.
+	/*
+	 * Model touchscreens providing buttons as touchpads.
 	 */
 	if (field->application == HID_DG_TOUCHPAD ||
-	    (usage->hid & HID_USAGE_PAGE) == HID_UP_BUTTON ||
-	    cls->is_indirect) {
-		set_bit(INPUT_PROP_POINTER, hi->input->propbit);
-		set_bit(BTN_TOOL_FINGER, hi->input->keybit);
-		set_bit(BTN_TOOL_DOUBLETAP, hi->input->keybit);
-		set_bit(BTN_TOOL_TRIPLETAP, hi->input->keybit);
-		set_bit(BTN_TOOL_QUADTAP, hi->input->keybit);
-	}
+	    (usage->hid & HID_USAGE_PAGE) == HID_UP_BUTTON)
+		td->mt_flags |= INPUT_MT_POINTER;
 
 	/* eGalax devices provide a Digitizer.Stylus input which overrides
 	 * the correct Digitizers.Finger X/Y ranges.
@@ -353,8 +349,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					EV_ABS, ABS_MT_POSITION_X);
 			set_abs(hi->input, ABS_MT_POSITION_X, field,
 				cls->sn_move);
-			/* touchscreen emulation */
-			set_abs(hi->input, ABS_X, field, cls->sn_move);
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
@@ -363,8 +357,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					EV_ABS, ABS_MT_POSITION_Y);
 			set_abs(hi->input, ABS_MT_POSITION_Y, field,
 				cls->sn_move);
-			/* touchscreen emulation */
-			set_abs(hi->input, ABS_Y, field, cls->sn_move);
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
@@ -388,9 +380,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTID:
-			if (!td->maxcontacts)
-				td->maxcontacts = MT_DEFAULT_MAXCONTACT;
-			input_mt_init_slots(hi->input, td->maxcontacts);
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			td->touches_by_report++;
@@ -398,18 +387,21 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		case HID_DG_WIDTH:
 			hid_map_usage(hi, usage, bit, max,
 					EV_ABS, ABS_MT_TOUCH_MAJOR);
-			set_abs(hi->input, ABS_MT_TOUCH_MAJOR, field,
-				cls->sn_width);
+			if (!(cls->quirks & MT_QUIRK_NO_AREA))
+				set_abs(hi->input, ABS_MT_TOUCH_MAJOR, field,
+					cls->sn_width);
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_HEIGHT:
 			hid_map_usage(hi, usage, bit, max,
 					EV_ABS, ABS_MT_TOUCH_MINOR);
-			set_abs(hi->input, ABS_MT_TOUCH_MINOR, field,
-				cls->sn_height);
-			input_set_abs_params(hi->input,
+			if (!(cls->quirks & MT_QUIRK_NO_AREA)) {
+				set_abs(hi->input, ABS_MT_TOUCH_MINOR, field,
+					cls->sn_height);
+				input_set_abs_params(hi->input,
 					ABS_MT_ORIENTATION, 0, 1, 0, 0);
+			}
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
@@ -418,9 +410,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					EV_ABS, ABS_MT_PRESSURE);
 			set_abs(hi->input, ABS_MT_PRESSURE, field,
 				cls->sn_pressure);
-			/* touchscreen emulation */
-			set_abs(hi->input, ABS_PRESSURE, field,
-				cls->sn_pressure);
 			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
@@ -464,7 +453,7 @@ static int mt_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 	return -1;
 }
 
-static int mt_compute_slot(struct mt_device *td)
+static int mt_compute_slot(struct mt_device *td, struct input_dev *input)
 {
 	__s32 quirks = td->mtclass.quirks;
 
@@ -480,42 +469,23 @@ static int mt_compute_slot(struct mt_device *td)
 	if (quirks & MT_QUIRK_SLOT_IS_CONTACTID_MINUS_ONE)
 		return td->curdata.contactid - 1;
 
-	return find_slot_from_contactid(td);
+	return input_mt_get_slot_by_key(input, td->curdata.contactid);
 }
 
 /*
  * this function is called when a whole contact has been processed,
  * so that it can assign it to a slot and store the data there
  */
-static void mt_complete_slot(struct mt_device *td)
+static void mt_complete_slot(struct mt_device *td, struct input_dev *input)
 {
-	td->curdata.seen_in_this_frame = true;
 	if (td->curvalid) {
-		int slotnum = mt_compute_slot(td);
-
-		if (slotnum >= 0 && slotnum < td->maxcontacts)
-			td->slots[slotnum] = td->curdata;
-	}
-	td->num_received++;
-}
+		int slotnum = mt_compute_slot(td, input);
+		struct mt_slot *s = &td->curdata;
 
+		if (slotnum < 0 || slotnum >= td->maxcontacts)
+			return;
 
-/*
- * this function is called when a whole packet has been received and processed,
- * so that it can decide what to send to the input layer.
- */
-static void mt_emit_event(struct mt_device *td, struct input_dev *input)
-{
-	int i;
-
-	for (i = 0; i < td->maxcontacts; ++i) {
-		struct mt_slot *s = &(td->slots[i]);
-		if ((td->mtclass.quirks & MT_QUIRK_NOT_SEEN_MEANS_UP) &&
-			!s->seen_in_this_frame) {
-			s->touch_state = false;
-		}
-
-		input_mt_slot(input, i);
+		input_mt_slot(input, slotnum);
 		input_mt_report_slot_state(input, MT_TOOL_FINGER,
 			s->touch_state);
 		if (s->touch_state) {
@@ -532,24 +502,29 @@ static void mt_emit_event(struct mt_device *td, struct input_dev *input)
 			input_event(input, EV_ABS, ABS_MT_TOUCH_MAJOR, major);
 			input_event(input, EV_ABS, ABS_MT_TOUCH_MINOR, minor);
 		}
-		s->seen_in_this_frame = false;
-
 	}
 
-	input_mt_report_pointer_emulation(input, true);
+	td->num_received++;
+}
+
+/*
+ * this function is called when a whole packet has been received and processed,
+ * so that it can decide what to send to the input layer.
+ */
+static void mt_sync_frame(struct mt_device *td, struct input_dev *input)
+{
+	input_mt_sync_frame(input);
 	input_sync(input);
 	td->num_received = 0;
 }
 
-
-
 static int mt_event(struct hid_device *hid, struct hid_field *field,
 				struct hid_usage *usage, __s32 value)
 {
 	struct mt_device *td = hid_get_drvdata(hid);
 	__s32 quirks = td->mtclass.quirks;
 
-	if (hid->claimed & HID_CLAIMED_INPUT && td->slots) {
+	if (hid->claimed & HID_CLAIMED_INPUT) {
 		switch (usage->hid) {
 		case HID_DG_INRANGE:
 			if (quirks & MT_QUIRK_ALWAYS_VALID)
@@ -602,11 +577,11 @@ static int mt_event(struct hid_device *hid, struct hid_field *field,
 		}
 
 		if (usage->hid == td->last_slot_field)
-			mt_complete_slot(td);
+			mt_complete_slot(td, field->hidinput->input);
 
 		if (field->index == td->last_field_index
 			&& td->num_received >= td->num_expected)
-			mt_emit_event(td, field->hidinput->input);
+			mt_sync_frame(td, field->hidinput->input);
 
 	}
 
@@ -685,18 +660,45 @@ static void mt_post_parse(struct mt_device *td)
 	}
 }
 
+static void mt_input_configured(struct hid_device *hdev, struct hid_input *hi)
+
+{
+	struct mt_device *td = hid_get_drvdata(hdev);
+	struct mt_class *cls = &td->mtclass;
+	struct input_dev *input = hi->input;
+
+	/* Only initialize slots for MT input devices */
+	if (!test_bit(ABS_MT_POSITION_X, input->absbit))
+		return;
+
+	if (!td->maxcontacts)
+		td->maxcontacts = MT_DEFAULT_MAXCONTACT;
+
+	mt_post_parse(td);
+	if (td->serial_maybe)
+		mt_post_parse_default_settings(td);
+
+	if (cls->is_indirect)
+		td->mt_flags |= INPUT_MT_POINTER;
+
+	if (cls->quirks & MT_QUIRK_NOT_SEEN_MEANS_UP)
+		td->mt_flags |= INPUT_MT_DROP_UNUSED;
+
+	input_mt_init_slots(input, td->maxcontacts, td->mt_flags);
+
+	td->mt_flags = 0;
+}
+
 static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	int ret, i;
 	struct mt_device *td;
 	struct mt_class *mtclass = mt_classes; /* MT_CLS_DEFAULT */
 
-	if (id) {
-		for (i = 0; mt_classes[i].name ; i++) {
-			if (id->driver_data == mt_classes[i].name) {
-				mtclass = &(mt_classes[i]);
-				break;
-			}
+	for (i = 0; mt_classes[i].name ; i++) {
+		if (id->driver_data == mt_classes[i].name) {
+			mtclass = &(mt_classes[i]);
+			break;
 		}
 	}
 
@@ -722,6 +724,9 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		goto fail;
 	}
 
+	if (id->vendor == HID_ANY_ID && id->product == HID_ANY_ID)
+		td->serial_maybe = true;
+
 	ret = hid_parse(hdev);
 	if (ret != 0)
 		goto fail;
@@ -730,20 +735,6 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (ret)
 		goto fail;
 
-	mt_post_parse(td);
-
-	if (id->vendor == HID_ANY_ID && id->product == HID_ANY_ID)
-		mt_post_parse_default_settings(td);
-
-	td->slots = kzalloc(td->maxcontacts * sizeof(struct mt_slot),
-				GFP_KERNEL);
-	if (!td->slots) {
-		dev_err(&hdev->dev, "cannot allocate multitouch slots\n");
-		hid_hw_stop(hdev);
-		ret = -ENOMEM;
-		goto fail;
-	}
-
 	ret = sysfs_create_group(&hdev->dev.kobj, &mt_attribute_group);
 
 	mt_set_maxcontacts(hdev);
@@ -767,6 +758,32 @@ static int mt_reset_resume(struct hid_device *hdev)
 	mt_set_input_mode(hdev);
 	return 0;
 }
+
+static int mt_resume(struct hid_device *hdev)
+{
+	struct usb_interface *intf;
+	struct usb_host_interface *interface;
+	struct usb_device *dev;
+
+	if (hdev->bus != BUS_USB)
+		return 0;
+
+	intf = to_usb_interface(hdev->dev.parent);
+	interface = intf->cur_altsetting;
+	dev = hid_to_usb_dev(hdev);
+
+	/* Some Elan legacy devices require SET_IDLE to be set on resume.
+	 * It should be safe to send it to other devices too.
+	 * Tested on 3M, Stantum, Cypress, Zytronic, eGalax, and Elan panels. */
+
+	usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
+			HID_REQ_SET_IDLE,
+			USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+			0, interface->desc.bInterfaceNumber,
+			NULL, 0, USB_CTRL_SET_TIMEOUT);
+
+	return 0;
+}
 #endif
 
 static void mt_remove(struct hid_device *hdev)
@@ -774,7 +791,6 @@ static void mt_remove(struct hid_device *hdev)
 	struct mt_device *td = hid_get_drvdata(hdev);
 	sysfs_remove_group(&hdev->dev.kobj, &mt_attribute_group);
 	hid_hw_stop(hdev);
-	kfree(td->slots);
 	kfree(td);
 	hid_set_drvdata(hdev, NULL);
 }
@@ -885,17 +901,37 @@ static const struct hid_device_id mt_devices[] = {
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7349) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
 		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_73F7) },
+	{ .driver_data = MT_CLS_EGALAX_SERIAL,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
+	{ .driver_data = MT_CLS_EGALAX,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7224) },
+	{ .driver_data = MT_CLS_EGALAX,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72D0) },
+	{ .driver_data = MT_CLS_EGALAX,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72C4) },
 
 	/* Elo TouchSystems IntelliTouch Plus panel */
 	{ .driver_data = MT_CLS_DUAL_NSMU_CONTACTID,
 		MT_USB_DEVICE(USB_VENDOR_ID_ELO,
 			USB_DEVICE_ID_ELO_TS2515) },
 
+	/* Flatfrog Panels */
+	{ .driver_data = MT_CLS_FLATFROG,
+		MT_USB_DEVICE(USB_VENDOR_ID_FLATFROG,
+			USB_DEVICE_ID_MULTITOUCH_3200) },
+
 	/* GeneralTouch panel */
-	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
+	{ .driver_data = MT_CLS_GENERALTOUCH_TWOFINGERS,
 		MT_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH,
 			USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS) },
+	{ .driver_data = MT_CLS_GENERALTOUCH_PWT_TENFINGERS,
+		MT_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH,
+			USB_DEVICE_ID_GENERAL_TOUCH_WIN8_PWT_TENFINGERS) },
 
 	/* Gametel game controller */
 	{ .driver_data = MT_CLS_DEFAULT,
@@ -1087,11 +1123,13 @@ static struct hid_driver mt_driver = {
 	.remove = mt_remove,
 	.input_mapping = mt_input_mapping,
 	.input_mapped = mt_input_mapped,
+	.input_configured = mt_input_configured,
 	.feature_mapping = mt_feature_mapping,
 	.usage_table = mt_grabbed_usages,
 	.event = mt_event,
 #ifdef CONFIG_PM
 	.reset_resume = mt_reset_resume,
+	.resume = mt_resume,
 #endif
 };
 
diff --git a/drivers/hid/hid-ntrig.c b/drivers/hid/hid-ntrig.c
index 9fae2ebdd758..86a969f63292 100644
--- a/drivers/hid/hid-ntrig.c
+++ b/drivers/hid/hid-ntrig.c
@@ -882,10 +882,10 @@ static int ntrig_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	nd->activate_slack = activate_slack;
 	nd->act_state = activate_slack;
 	nd->deactivate_slack = -deactivate_slack;
-	nd->sensor_logical_width = 0;
-	nd->sensor_logical_height = 0;
-	nd->sensor_physical_width = 0;
-	nd->sensor_physical_height = 0;
+	nd->sensor_logical_width = 1;
+	nd->sensor_logical_height = 1;
+	nd->sensor_physical_width = 1;
+	nd->sensor_physical_height = 1;
 
 	hid_set_drvdata(hdev, nd);
 
diff --git a/drivers/hid/hid-petalynx.c b/drivers/hid/hid-petalynx.c
index f1ea3ff8a98d..4c521de4e7e6 100644
--- a/drivers/hid/hid-petalynx.c
+++ b/drivers/hid/hid-petalynx.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-picolcd.c b/drivers/hid/hid-picolcd.c
deleted file mode 100644
index 27c8ebdfad01..000000000000
--- a/drivers/hid/hid-picolcd.c
+++ /dev/null
@@ -1,2748 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2010 by Bruno Prémont <bonbons@linux-vserver.org>       *
- *                                                                         *
- *   Based on Logitech G13 driver (v0.4)                                   *
- *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
- *                                                                         *
- *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
- *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
- *   General Public License for more details.                              *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
- ***************************************************************************/
-
-#include <linux/hid.h>
-#include <linux/hid-debug.h>
-#include <linux/input.h>
-#include "hid-ids.h"
-#include "usbhid/usbhid.h"
-#include <linux/usb.h>
-
-#include <linux/fb.h>
-#include <linux/vmalloc.h>
-#include <linux/backlight.h>
-#include <linux/lcd.h>
-
-#include <linux/leds.h>
-
-#include <linux/seq_file.h>
-#include <linux/debugfs.h>
-
-#include <linux/completion.h>
-#include <linux/uaccess.h>
-#include <linux/module.h>
-
-#define PICOLCD_NAME "PicoLCD (graphic)"
-
-/* Report numbers */
-#define REPORT_ERROR_CODE      0x10 /* LCD: IN[16]  */
-#define   ERR_SUCCESS            0x00
-#define   ERR_PARAMETER_MISSING  0x01
-#define   ERR_DATA_MISSING       0x02
-#define   ERR_BLOCK_READ_ONLY    0x03
-#define   ERR_BLOCK_NOT_ERASABLE 0x04
-#define   ERR_BLOCK_TOO_BIG      0x05
-#define   ERR_SECTION_OVERFLOW   0x06
-#define   ERR_INVALID_CMD_LEN    0x07
-#define   ERR_INVALID_DATA_LEN   0x08
-#define REPORT_KEY_STATE       0x11 /* LCD: IN[2]   */
-#define REPORT_IR_DATA         0x21 /* LCD: IN[63]  */
-#define REPORT_EE_DATA         0x32 /* LCD: IN[63]  */
-#define REPORT_MEMORY          0x41 /* LCD: IN[63]  */
-#define REPORT_LED_STATE       0x81 /* LCD: OUT[1]  */
-#define REPORT_BRIGHTNESS      0x91 /* LCD: OUT[1]  */
-#define REPORT_CONTRAST        0x92 /* LCD: OUT[1]  */
-#define REPORT_RESET           0x93 /* LCD: OUT[2]  */
-#define REPORT_LCD_CMD         0x94 /* LCD: OUT[63] */
-#define REPORT_LCD_DATA        0x95 /* LCD: OUT[63] */
-#define REPORT_LCD_CMD_DATA    0x96 /* LCD: OUT[63] */
-#define	REPORT_EE_READ         0xa3 /* LCD: OUT[63] */
-#define REPORT_EE_WRITE        0xa4 /* LCD: OUT[63] */
-#define REPORT_ERASE_MEMORY    0xb2 /* LCD: OUT[2]  */
-#define REPORT_READ_MEMORY     0xb3 /* LCD: OUT[3]  */
-#define REPORT_WRITE_MEMORY    0xb4 /* LCD: OUT[63] */
-#define REPORT_SPLASH_RESTART  0xc1 /* LCD: OUT[1]  */
-#define REPORT_EXIT_KEYBOARD   0xef /* LCD: OUT[2]  */
-#define REPORT_VERSION         0xf1 /* LCD: IN[2],OUT[1]    Bootloader: IN[2],OUT[1]   */
-#define REPORT_BL_ERASE_MEMORY 0xf2 /*                      Bootloader: IN[36],OUT[4]  */
-#define REPORT_BL_READ_MEMORY  0xf3 /*                      Bootloader: IN[36],OUT[4]  */
-#define REPORT_BL_WRITE_MEMORY 0xf4 /*                      Bootloader: IN[36],OUT[36] */
-#define REPORT_DEVID           0xf5 /* LCD: IN[5], OUT[1]   Bootloader: IN[5],OUT[1]   */
-#define REPORT_SPLASH_SIZE     0xf6 /* LCD: IN[4], OUT[1]   */
-#define REPORT_HOOK_VERSION    0xf7 /* LCD: IN[2], OUT[1]   */
-#define REPORT_EXIT_FLASHER    0xff /*                      Bootloader: OUT[2]         */
-
-#ifdef CONFIG_HID_PICOLCD_FB
-/* Framebuffer
- *
- * The PicoLCD use a Topway LCD module of 256x64 pixel
- * This display area is tiled over 4 controllers with 8 tiles
- * each. Each tile has 8x64 pixel, each data byte representing
- * a 1-bit wide vertical line of the tile.
- *
- * The display can be updated at a tile granularity.
- *
- *       Chip 1           Chip 2           Chip 3           Chip 4
- * +----------------+----------------+----------------+----------------+
- * |     Tile 1     |     Tile 1     |     Tile 1     |     Tile 1     |
- * +----------------+----------------+----------------+----------------+
- * |     Tile 2     |     Tile 2     |     Tile 2     |     Tile 2     |
- * +----------------+----------------+----------------+----------------+
- *                                  ...
- * +----------------+----------------+----------------+----------------+
- * |     Tile 8     |     Tile 8     |     Tile 8     |     Tile 8     |
- * +----------------+----------------+----------------+----------------+
- */
-#define PICOLCDFB_NAME "picolcdfb"
-#define PICOLCDFB_WIDTH (256)
-#define PICOLCDFB_HEIGHT (64)
-#define PICOLCDFB_SIZE (PICOLCDFB_WIDTH * PICOLCDFB_HEIGHT / 8)
-
-#define PICOLCDFB_UPDATE_RATE_LIMIT   10
-#define PICOLCDFB_UPDATE_RATE_DEFAULT  2
-
-/* Framebuffer visual structures */
-static const struct fb_fix_screeninfo picolcdfb_fix = {
-	.id          = PICOLCDFB_NAME,
-	.type        = FB_TYPE_PACKED_PIXELS,
-	.visual      = FB_VISUAL_MONO01,
-	.xpanstep    = 0,
-	.ypanstep    = 0,
-	.ywrapstep   = 0,
-	.line_length = PICOLCDFB_WIDTH / 8,
-	.accel       = FB_ACCEL_NONE,
-};
-
-static const struct fb_var_screeninfo picolcdfb_var = {
-	.xres           = PICOLCDFB_WIDTH,
-	.yres           = PICOLCDFB_HEIGHT,
-	.xres_virtual   = PICOLCDFB_WIDTH,
-	.yres_virtual   = PICOLCDFB_HEIGHT,
-	.width          = 103,
-	.height         = 26,
-	.bits_per_pixel = 1,
-	.grayscale      = 1,
-	.red            = {
-		.offset = 0,
-		.length = 1,
-		.msb_right = 0,
-	},
-	.green          = {
-		.offset = 0,
-		.length = 1,
-		.msb_right = 0,
-	},
-	.blue           = {
-		.offset = 0,
-		.length = 1,
-		.msb_right = 0,
-	},
-	.transp         = {
-		.offset = 0,
-		.length = 0,
-		.msb_right = 0,
-	},
-};
-#endif /* CONFIG_HID_PICOLCD_FB */
-
-/* Input device
- *
- * The PicoLCD has an IR receiver header, a built-in keypad with 5 keys
- * and header for 4x4 key matrix. The built-in keys are part of the matrix.
- */
-static const unsigned short def_keymap[] = {
-	KEY_RESERVED,	/* none */
-	KEY_BACK,	/* col 4 + row 1 */
-	KEY_HOMEPAGE,	/* col 3 + row 1 */
-	KEY_RESERVED,	/* col 2 + row 1 */
-	KEY_RESERVED,	/* col 1 + row 1 */
-	KEY_SCROLLUP,	/* col 4 + row 2 */
-	KEY_OK,		/* col 3 + row 2 */
-	KEY_SCROLLDOWN,	/* col 2 + row 2 */
-	KEY_RESERVED,	/* col 1 + row 2 */
-	KEY_RESERVED,	/* col 4 + row 3 */
-	KEY_RESERVED,	/* col 3 + row 3 */
-	KEY_RESERVED,	/* col 2 + row 3 */
-	KEY_RESERVED,	/* col 1 + row 3 */
-	KEY_RESERVED,	/* col 4 + row 4 */
-	KEY_RESERVED,	/* col 3 + row 4 */
-	KEY_RESERVED,	/* col 2 + row 4 */
-	KEY_RESERVED,	/* col 1 + row 4 */
-};
-#define PICOLCD_KEYS ARRAY_SIZE(def_keymap)
-
-/* Description of in-progress IO operation, used for operations
- * that trigger response from device */
-struct picolcd_pending {
-	struct hid_report *out_report;
-	struct hid_report *in_report;
-	struct completion ready;
-	int raw_size;
-	u8 raw_data[64];
-};
-
-/* Per device data structure */
-struct picolcd_data {
-	struct hid_device *hdev;
-#ifdef CONFIG_DEBUG_FS
-	struct dentry *debug_reset;
-	struct dentry *debug_eeprom;
-	struct dentry *debug_flash;
-	struct mutex mutex_flash;
-	int addr_sz;
-#endif
-	u8 version[2];
-	unsigned short opmode_delay;
-	/* input stuff */
-	u8 pressed_keys[2];
-	struct input_dev *input_keys;
-	struct input_dev *input_cir;
-	unsigned short keycode[PICOLCD_KEYS];
-
-#ifdef CONFIG_HID_PICOLCD_FB
-	/* Framebuffer stuff */
-	u8 fb_update_rate;
-	u8 fb_bpp;
-	u8 fb_force;
-	u8 *fb_vbitmap;		/* local copy of what was sent to PicoLCD */
-	u8 *fb_bitmap;		/* framebuffer */
-	struct fb_info *fb_info;
-	struct fb_deferred_io fb_defio;
-#endif /* CONFIG_HID_PICOLCD_FB */
-#ifdef CONFIG_HID_PICOLCD_LCD
-	struct lcd_device *lcd;
-	u8 lcd_contrast;
-#endif /* CONFIG_HID_PICOLCD_LCD */
-#ifdef CONFIG_HID_PICOLCD_BACKLIGHT
-	struct backlight_device *backlight;
-	u8 lcd_brightness;
-	u8 lcd_power;
-#endif /* CONFIG_HID_PICOLCD_BACKLIGHT */
-#ifdef CONFIG_HID_PICOLCD_LEDS
-	/* LED stuff */
-	u8 led_state;
-	struct led_classdev *led[8];
-#endif /* CONFIG_HID_PICOLCD_LEDS */
-
-	/* Housekeeping stuff */
-	spinlock_t lock;
-	struct mutex mutex;
-	struct picolcd_pending *pending;
-	int status;
-#define PICOLCD_BOOTLOADER 1
-#define PICOLCD_FAILED 2
-#define PICOLCD_READY_FB 4
-};
-
-
-/* Find a given report */
-#define picolcd_in_report(id, dev) picolcd_report(id, dev, HID_INPUT_REPORT)
-#define picolcd_out_report(id, dev) picolcd_report(id, dev, HID_OUTPUT_REPORT)
-
-static struct hid_report *picolcd_report(int id, struct hid_device *hdev, int dir)
-{
-	struct list_head *feature_report_list = &hdev->report_enum[dir].report_list;
-	struct hid_report *report = NULL;
-
-	list_for_each_entry(report, feature_report_list, list) {
-		if (report->id == id)
-			return report;
-	}
-	hid_warn(hdev, "No report with id 0x%x found\n", id);
-	return NULL;
-}
-
-#ifdef CONFIG_DEBUG_FS
-static void picolcd_debug_out_report(struct picolcd_data *data,
-		struct hid_device *hdev, struct hid_report *report);
-#define usbhid_submit_report(a, b, c) \
-	do { \
-		picolcd_debug_out_report(hid_get_drvdata(a), a, b); \
-		usbhid_submit_report(a, b, c); \
-	} while (0)
-#endif
-
-/* Submit a report and wait for a reply from device - if device fades away
- * or does not respond in time, return NULL */
-static struct picolcd_pending *picolcd_send_and_wait(struct hid_device *hdev,
-		int report_id, const u8 *raw_data, int size)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	struct picolcd_pending *work;
-	struct hid_report *report = picolcd_out_report(report_id, hdev);
-	unsigned long flags;
-	int i, j, k;
-
-	if (!report || !data)
-		return NULL;
-	if (data->status & PICOLCD_FAILED)
-		return NULL;
-	work = kzalloc(sizeof(*work), GFP_KERNEL);
-	if (!work)
-		return NULL;
-
-	init_completion(&work->ready);
-	work->out_report = report;
-	work->in_report  = NULL;
-	work->raw_size   = 0;
-
-	mutex_lock(&data->mutex);
-	spin_lock_irqsave(&data->lock, flags);
-	for (i = k = 0; i < report->maxfield; i++)
-		for (j = 0; j < report->field[i]->report_count; j++) {
-			hid_set_field(report->field[i], j, k < size ? raw_data[k] : 0);
-			k++;
-		}
-	data->pending = work;
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-	wait_for_completion_interruptible_timeout(&work->ready, HZ*2);
-	spin_lock_irqsave(&data->lock, flags);
-	data->pending = NULL;
-	spin_unlock_irqrestore(&data->lock, flags);
-	mutex_unlock(&data->mutex);
-	return work;
-}
-
-#ifdef CONFIG_HID_PICOLCD_FB
-/* Send a given tile to PicoLCD */
-static int picolcd_fb_send_tile(struct hid_device *hdev, int chip, int tile)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	struct hid_report *report1 = picolcd_out_report(REPORT_LCD_CMD_DATA, hdev);
-	struct hid_report *report2 = picolcd_out_report(REPORT_LCD_DATA, hdev);
-	unsigned long flags;
-	u8 *tdata;
-	int i;
-
-	if (!report1 || report1->maxfield != 1 || !report2 || report2->maxfield != 1)
-		return -ENODEV;
-
-	spin_lock_irqsave(&data->lock, flags);
-	hid_set_field(report1->field[0],  0, chip << 2);
-	hid_set_field(report1->field[0],  1, 0x02);
-	hid_set_field(report1->field[0],  2, 0x00);
-	hid_set_field(report1->field[0],  3, 0x00);
-	hid_set_field(report1->field[0],  4, 0xb8 | tile);
-	hid_set_field(report1->field[0],  5, 0x00);
-	hid_set_field(report1->field[0],  6, 0x00);
-	hid_set_field(report1->field[0],  7, 0x40);
-	hid_set_field(report1->field[0],  8, 0x00);
-	hid_set_field(report1->field[0],  9, 0x00);
-	hid_set_field(report1->field[0], 10,   32);
-
-	hid_set_field(report2->field[0],  0, (chip << 2) | 0x01);
-	hid_set_field(report2->field[0],  1, 0x00);
-	hid_set_field(report2->field[0],  2, 0x00);
-	hid_set_field(report2->field[0],  3,   32);
-
-	tdata = data->fb_vbitmap + (tile * 4 + chip) * 64;
-	for (i = 0; i < 64; i++)
-		if (i < 32)
-			hid_set_field(report1->field[0], 11 + i, tdata[i]);
-		else
-			hid_set_field(report2->field[0], 4 + i - 32, tdata[i]);
-
-	usbhid_submit_report(data->hdev, report1, USB_DIR_OUT);
-	usbhid_submit_report(data->hdev, report2, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-	return 0;
-}
-
-/* Translate a single tile*/
-static int picolcd_fb_update_tile(u8 *vbitmap, const u8 *bitmap, int bpp,
-		int chip, int tile)
-{
-	int i, b, changed = 0;
-	u8 tdata[64];
-	u8 *vdata = vbitmap + (tile * 4 + chip) * 64;
-
-	if (bpp == 1) {
-		for (b = 7; b >= 0; b--) {
-			const u8 *bdata = bitmap + tile * 256 + chip * 8 + b * 32;
-			for (i = 0; i < 64; i++) {
-				tdata[i] <<= 1;
-				tdata[i] |= (bdata[i/8] >> (i % 8)) & 0x01;
-			}
-		}
-	} else if (bpp == 8) {
-		for (b = 7; b >= 0; b--) {
-			const u8 *bdata = bitmap + (tile * 256 + chip * 8 + b * 32) * 8;
-			for (i = 0; i < 64; i++) {
-				tdata[i] <<= 1;
-				tdata[i] |= (bdata[i] & 0x80) ? 0x01 : 0x00;
-			}
-		}
-	} else {
-		/* Oops, we should never get here! */
-		WARN_ON(1);
-		return 0;
-	}
-
-	for (i = 0; i < 64; i++)
-		if (tdata[i] != vdata[i]) {
-			changed = 1;
-			vdata[i] = tdata[i];
-		}
-	return changed;
-}
-
-/* Reconfigure LCD display */
-static int picolcd_fb_reset(struct picolcd_data *data, int clear)
-{
-	struct hid_report *report = picolcd_out_report(REPORT_LCD_CMD, data->hdev);
-	int i, j;
-	unsigned long flags;
-	static const u8 mapcmd[8] = { 0x00, 0x02, 0x00, 0x64, 0x3f, 0x00, 0x64, 0xc0 };
-
-	if (!report || report->maxfield != 1)
-		return -ENODEV;
-
-	spin_lock_irqsave(&data->lock, flags);
-	for (i = 0; i < 4; i++) {
-		for (j = 0; j < report->field[0]->maxusage; j++)
-			if (j == 0)
-				hid_set_field(report->field[0], j, i << 2);
-			else if (j < sizeof(mapcmd))
-				hid_set_field(report->field[0], j, mapcmd[j]);
-			else
-				hid_set_field(report->field[0], j, 0);
-		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	}
-
-	data->status |= PICOLCD_READY_FB;
-	spin_unlock_irqrestore(&data->lock, flags);
-
-	if (data->fb_bitmap) {
-		if (clear) {
-			memset(data->fb_vbitmap, 0, PICOLCDFB_SIZE);
-			memset(data->fb_bitmap, 0, PICOLCDFB_SIZE*data->fb_bpp);
-		}
-		data->fb_force = 1;
-	}
-
-	/* schedule first output of framebuffer */
-	if (data->fb_info)
-		schedule_delayed_work(&data->fb_info->deferred_work, 0);
-
-	return 0;
-}
-
-/* Update fb_vbitmap from the screen_base and send changed tiles to device */
-static void picolcd_fb_update(struct picolcd_data *data)
-{
-	int chip, tile, n;
-	unsigned long flags;
-
-	if (!data)
-		return;
-
-	spin_lock_irqsave(&data->lock, flags);
-	if (!(data->status & PICOLCD_READY_FB)) {
-		spin_unlock_irqrestore(&data->lock, flags);
-		picolcd_fb_reset(data, 0);
-	} else {
-		spin_unlock_irqrestore(&data->lock, flags);
-	}
-
-	/*
-	 * Translate the framebuffer into the format needed by the PicoLCD.
-	 * See display layout above.
-	 * Do this one tile after the other and push those tiles that changed.
-	 *
-	 * Wait for our IO to complete as otherwise we might flood the queue!
-	 */
-	n = 0;
-	for (chip = 0; chip < 4; chip++)
-		for (tile = 0; tile < 8; tile++)
-			if (picolcd_fb_update_tile(data->fb_vbitmap,
-					data->fb_bitmap, data->fb_bpp, chip, tile) ||
-				data->fb_force) {
-				n += 2;
-				if (!data->fb_info->par)
-					return; /* device lost! */
-				if (n >= HID_OUTPUT_FIFO_SIZE / 2) {
-					usbhid_wait_io(data->hdev);
-					n = 0;
-				}
-				picolcd_fb_send_tile(data->hdev, chip, tile);
-			}
-	data->fb_force = false;
-	if (n)
-		usbhid_wait_io(data->hdev);
-}
-
-/* Stub to call the system default and update the image on the picoLCD */
-static void picolcd_fb_fillrect(struct fb_info *info,
-		const struct fb_fillrect *rect)
-{
-	if (!info->par)
-		return;
-	sys_fillrect(info, rect);
-
-	schedule_delayed_work(&info->deferred_work, 0);
-}
-
-/* Stub to call the system default and update the image on the picoLCD */
-static void picolcd_fb_copyarea(struct fb_info *info,
-		const struct fb_copyarea *area)
-{
-	if (!info->par)
-		return;
-	sys_copyarea(info, area);
-
-	schedule_delayed_work(&info->deferred_work, 0);
-}
-
-/* Stub to call the system default and update the image on the picoLCD */
-static void picolcd_fb_imageblit(struct fb_info *info, const struct fb_image *image)
-{
-	if (!info->par)
-		return;
-	sys_imageblit(info, image);
-
-	schedule_delayed_work(&info->deferred_work, 0);
-}
-
-/*
- * this is the slow path from userspace. they can seek and write to
- * the fb. it's inefficient to do anything less than a full screen draw
- */
-static ssize_t picolcd_fb_write(struct fb_info *info, const char __user *buf,
-		size_t count, loff_t *ppos)
-{
-	ssize_t ret;
-	if (!info->par)
-		return -ENODEV;
-	ret = fb_sys_write(info, buf, count, ppos);
-	if (ret >= 0)
-		schedule_delayed_work(&info->deferred_work, 0);
-	return ret;
-}
-
-static int picolcd_fb_blank(int blank, struct fb_info *info)
-{
-	if (!info->par)
-		return -ENODEV;
-	/* We let fb notification do this for us via lcd/backlight device */
-	return 0;
-}
-
-static void picolcd_fb_destroy(struct fb_info *info)
-{
-	struct picolcd_data *data = info->par;
-	u32 *ref_cnt = info->pseudo_palette;
-	int may_release;
-
-	info->par = NULL;
-	if (data)
-		data->fb_info = NULL;
-	fb_deferred_io_cleanup(info);
-
-	ref_cnt--;
-	mutex_lock(&info->lock);
-	(*ref_cnt)--;
-	may_release = !*ref_cnt;
-	mutex_unlock(&info->lock);
-	if (may_release) {
-		vfree((u8 *)info->fix.smem_start);
-		framebuffer_release(info);
-	}
-}
-
-static int picolcd_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
-{
-	__u32 bpp      = var->bits_per_pixel;
-	__u32 activate = var->activate;
-
-	/* only allow 1/8 bit depth (8-bit is grayscale) */
-	*var = picolcdfb_var;
-	var->activate = activate;
-	if (bpp >= 8) {
-		var->bits_per_pixel = 8;
-		var->red.length     = 8;
-		var->green.length   = 8;
-		var->blue.length    = 8;
-	} else {
-		var->bits_per_pixel = 1;
-		var->red.length     = 1;
-		var->green.length   = 1;
-		var->blue.length    = 1;
-	}
-	return 0;
-}
-
-static int picolcd_set_par(struct fb_info *info)
-{
-	struct picolcd_data *data = info->par;
-	u8 *tmp_fb, *o_fb;
-	if (!data)
-		return -ENODEV;
-	if (info->var.bits_per_pixel == data->fb_bpp)
-		return 0;
-	/* switch between 1/8 bit depths */
-	if (info->var.bits_per_pixel != 1 && info->var.bits_per_pixel != 8)
-		return -EINVAL;
-
-	o_fb   = data->fb_bitmap;
-	tmp_fb = kmalloc(PICOLCDFB_SIZE*info->var.bits_per_pixel, GFP_KERNEL);
-	if (!tmp_fb)
-		return -ENOMEM;
-
-	/* translate FB content to new bits-per-pixel */
-	if (info->var.bits_per_pixel == 1) {
-		int i, b;
-		for (i = 0; i < PICOLCDFB_SIZE; i++) {
-			u8 p = 0;
-			for (b = 0; b < 8; b++) {
-				p <<= 1;
-				p |= o_fb[i*8+b] ? 0x01 : 0x00;
-			}
-			tmp_fb[i] = p;
-		}
-		memcpy(o_fb, tmp_fb, PICOLCDFB_SIZE);
-		info->fix.visual = FB_VISUAL_MONO01;
-		info->fix.line_length = PICOLCDFB_WIDTH / 8;
-	} else {
-		int i;
-		memcpy(tmp_fb, o_fb, PICOLCDFB_SIZE);
-		for (i = 0; i < PICOLCDFB_SIZE * 8; i++)
-			o_fb[i] = tmp_fb[i/8] & (0x01 << (7 - i % 8)) ? 0xff : 0x00;
-		info->fix.visual = FB_VISUAL_DIRECTCOLOR;
-		info->fix.line_length = PICOLCDFB_WIDTH;
-	}
-
-	kfree(tmp_fb);
-	data->fb_bpp      = info->var.bits_per_pixel;
-	return 0;
-}
-
-/* Do refcounting on our FB and cleanup per worker if FB is
- * closed after unplug of our device
- * (fb_release holds info->lock and still touches info after
- *  we return so we can't release it immediately.
- */
-struct picolcd_fb_cleanup_item {
-	struct fb_info *info;
-	struct picolcd_fb_cleanup_item *next;
-};
-static struct picolcd_fb_cleanup_item *fb_pending;
-static DEFINE_SPINLOCK(fb_pending_lock);
-
-static void picolcd_fb_do_cleanup(struct work_struct *data)
-{
-	struct picolcd_fb_cleanup_item *item;
-	unsigned long flags;
-
-	do {
-		spin_lock_irqsave(&fb_pending_lock, flags);
-		item = fb_pending;
-		fb_pending = item ? item->next : NULL;
-		spin_unlock_irqrestore(&fb_pending_lock, flags);
-
-		if (item) {
-			u8 *fb = (u8 *)item->info->fix.smem_start;
-			/* make sure we do not race against fb core when
-			 * releasing */
-			mutex_lock(&item->info->lock);
-			mutex_unlock(&item->info->lock);
-			framebuffer_release(item->info);
-			vfree(fb);
-		}
-	} while (item);
-}
-
-static DECLARE_WORK(picolcd_fb_cleanup, picolcd_fb_do_cleanup);
-
-static int picolcd_fb_open(struct fb_info *info, int u)
-{
-	u32 *ref_cnt = info->pseudo_palette;
-	ref_cnt--;
-
-	(*ref_cnt)++;
-	return 0;
-}
-
-static int picolcd_fb_release(struct fb_info *info, int u)
-{
-	u32 *ref_cnt = info->pseudo_palette;
-	ref_cnt--;
-
-	(*ref_cnt)++;
-	if (!*ref_cnt) {
-		unsigned long flags;
-		struct picolcd_fb_cleanup_item *item = (struct picolcd_fb_cleanup_item *)ref_cnt;
-		item--;
-		spin_lock_irqsave(&fb_pending_lock, flags);
-		item->next = fb_pending;
-		fb_pending = item;
-		spin_unlock_irqrestore(&fb_pending_lock, flags);
-		schedule_work(&picolcd_fb_cleanup);
-	}
-	return 0;
-}
-
-/* Note this can't be const because of struct fb_info definition */
-static struct fb_ops picolcdfb_ops = {
-	.owner        = THIS_MODULE,
-	.fb_destroy   = picolcd_fb_destroy,
-	.fb_open      = picolcd_fb_open,
-	.fb_release   = picolcd_fb_release,
-	.fb_read      = fb_sys_read,
-	.fb_write     = picolcd_fb_write,
-	.fb_blank     = picolcd_fb_blank,
-	.fb_fillrect  = picolcd_fb_fillrect,
-	.fb_copyarea  = picolcd_fb_copyarea,
-	.fb_imageblit = picolcd_fb_imageblit,
-	.fb_check_var = picolcd_fb_check_var,
-	.fb_set_par   = picolcd_set_par,
-};
-
-
-/* Callback from deferred IO workqueue */
-static void picolcd_fb_deferred_io(struct fb_info *info, struct list_head *pagelist)
-{
-	picolcd_fb_update(info->par);
-}
-
-static const struct fb_deferred_io picolcd_fb_defio = {
-	.delay = HZ / PICOLCDFB_UPDATE_RATE_DEFAULT,
-	.deferred_io = picolcd_fb_deferred_io,
-};
-
-
-/*
- * The "fb_update_rate" sysfs attribute
- */
-static ssize_t picolcd_fb_update_rate_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-	unsigned i, fb_update_rate = data->fb_update_rate;
-	size_t ret = 0;
-
-	for (i = 1; i <= PICOLCDFB_UPDATE_RATE_LIMIT; i++)
-		if (ret >= PAGE_SIZE)
-			break;
-		else if (i == fb_update_rate)
-			ret += snprintf(buf+ret, PAGE_SIZE-ret, "[%u] ", i);
-		else
-			ret += snprintf(buf+ret, PAGE_SIZE-ret, "%u ", i);
-	if (ret > 0)
-		buf[min(ret, (size_t)PAGE_SIZE)-1] = '\n';
-	return ret;
-}
-
-static ssize_t picolcd_fb_update_rate_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-	int i;
-	unsigned u;
-
-	if (count < 1 || count > 10)
-		return -EINVAL;
-
-	i = sscanf(buf, "%u", &u);
-	if (i != 1)
-		return -EINVAL;
-
-	if (u > PICOLCDFB_UPDATE_RATE_LIMIT)
-		return -ERANGE;
-	else if (u == 0)
-		u = PICOLCDFB_UPDATE_RATE_DEFAULT;
-
-	data->fb_update_rate = u;
-	data->fb_defio.delay = HZ / data->fb_update_rate;
-	return count;
-}
-
-static DEVICE_ATTR(fb_update_rate, 0666, picolcd_fb_update_rate_show,
-		picolcd_fb_update_rate_store);
-
-/* initialize Framebuffer device */
-static int picolcd_init_framebuffer(struct picolcd_data *data)
-{
-	struct device *dev = &data->hdev->dev;
-	struct fb_info *info = NULL;
-	int i, error = -ENOMEM;
-	u8 *fb_vbitmap = NULL;
-	u8 *fb_bitmap  = NULL;
-	u32 *palette;
-
-	fb_bitmap = vmalloc(PICOLCDFB_SIZE*8);
-	if (fb_bitmap == NULL) {
-		dev_err(dev, "can't get a free page for framebuffer\n");
-		goto err_nomem;
-	}
-
-	fb_vbitmap = kmalloc(PICOLCDFB_SIZE, GFP_KERNEL);
-	if (fb_vbitmap == NULL) {
-		dev_err(dev, "can't alloc vbitmap image buffer\n");
-		goto err_nomem;
-	}
-
-	data->fb_update_rate = PICOLCDFB_UPDATE_RATE_DEFAULT;
-	data->fb_defio = picolcd_fb_defio;
-	/* The extra memory is:
-	 * - struct picolcd_fb_cleanup_item
-	 * - u32 for ref_count
-	 * - 256*u32 for pseudo_palette
-	 */
-	info = framebuffer_alloc(257 * sizeof(u32) + sizeof(struct picolcd_fb_cleanup_item), dev);
-	if (info == NULL) {
-		dev_err(dev, "failed to allocate a framebuffer\n");
-		goto err_nomem;
-	}
-
-	palette  = info->par + sizeof(struct picolcd_fb_cleanup_item);
-	*palette = 1;
-	palette++;
-	for (i = 0; i < 256; i++)
-		palette[i] = i > 0 && i < 16 ? 0xff : 0;
-	info->pseudo_palette = palette;
-	info->fbdefio = &data->fb_defio;
-	info->screen_base = (char __force __iomem *)fb_bitmap;
-	info->fbops = &picolcdfb_ops;
-	info->var = picolcdfb_var;
-	info->fix = picolcdfb_fix;
-	info->fix.smem_len   = PICOLCDFB_SIZE*8;
-	info->fix.smem_start = (unsigned long)fb_bitmap;
-	info->par = data;
-	info->flags = FBINFO_FLAG_DEFAULT;
-
-	data->fb_vbitmap = fb_vbitmap;
-	data->fb_bitmap  = fb_bitmap;
-	data->fb_bpp     = picolcdfb_var.bits_per_pixel;
-	error = picolcd_fb_reset(data, 1);
-	if (error) {
-		dev_err(dev, "failed to configure display\n");
-		goto err_cleanup;
-	}
-	error = device_create_file(dev, &dev_attr_fb_update_rate);
-	if (error) {
-		dev_err(dev, "failed to create sysfs attributes\n");
-		goto err_cleanup;
-	}
-	fb_deferred_io_init(info);
-	data->fb_info    = info;
-	error = register_framebuffer(info);
-	if (error) {
-		dev_err(dev, "failed to register framebuffer\n");
-		goto err_sysfs;
-	}
-	/* schedule first output of framebuffer */
-	data->fb_force = 1;
-	schedule_delayed_work(&info->deferred_work, 0);
-	return 0;
-
-err_sysfs:
-	fb_deferred_io_cleanup(info);
-	device_remove_file(dev, &dev_attr_fb_update_rate);
-err_cleanup:
-	data->fb_vbitmap = NULL;
-	data->fb_bitmap  = NULL;
-	data->fb_bpp     = 0;
-	data->fb_info    = NULL;
-
-err_nomem:
-	framebuffer_release(info);
-	vfree(fb_bitmap);
-	kfree(fb_vbitmap);
-	return error;
-}
-
-static void picolcd_exit_framebuffer(struct picolcd_data *data)
-{
-	struct fb_info *info = data->fb_info;
-	u8 *fb_vbitmap = data->fb_vbitmap;
-
-	if (!info)
-		return;
-
-	info->par = NULL;
-	device_remove_file(&data->hdev->dev, &dev_attr_fb_update_rate);
-	unregister_framebuffer(info);
-	data->fb_vbitmap = NULL;
-	data->fb_bitmap  = NULL;
-	data->fb_bpp     = 0;
-	data->fb_info    = NULL;
-	kfree(fb_vbitmap);
-}
-
-#define picolcd_fbinfo(d) ((d)->fb_info)
-#else
-static inline int picolcd_fb_reset(struct picolcd_data *data, int clear)
-{
-	return 0;
-}
-static inline int picolcd_init_framebuffer(struct picolcd_data *data)
-{
-	return 0;
-}
-static inline void picolcd_exit_framebuffer(struct picolcd_data *data)
-{
-}
-#define picolcd_fbinfo(d) NULL
-#endif /* CONFIG_HID_PICOLCD_FB */
-
-#ifdef CONFIG_HID_PICOLCD_BACKLIGHT
-/*
- * backlight class device
- */
-static int picolcd_get_brightness(struct backlight_device *bdev)
-{
-	struct picolcd_data *data = bl_get_data(bdev);
-	return data->lcd_brightness;
-}
-
-static int picolcd_set_brightness(struct backlight_device *bdev)
-{
-	struct picolcd_data *data = bl_get_data(bdev);
-	struct hid_report *report = picolcd_out_report(REPORT_BRIGHTNESS, data->hdev);
-	unsigned long flags;
-
-	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
-		return -ENODEV;
-
-	data->lcd_brightness = bdev->props.brightness & 0x0ff;
-	data->lcd_power      = bdev->props.power;
-	spin_lock_irqsave(&data->lock, flags);
-	hid_set_field(report->field[0], 0, data->lcd_power == FB_BLANK_UNBLANK ? data->lcd_brightness : 0);
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-	return 0;
-}
-
-static int picolcd_check_bl_fb(struct backlight_device *bdev, struct fb_info *fb)
-{
-	return fb && fb == picolcd_fbinfo((struct picolcd_data *)bl_get_data(bdev));
-}
-
-static const struct backlight_ops picolcd_blops = {
-	.update_status  = picolcd_set_brightness,
-	.get_brightness = picolcd_get_brightness,
-	.check_fb       = picolcd_check_bl_fb,
-};
-
-static int picolcd_init_backlight(struct picolcd_data *data, struct hid_report *report)
-{
-	struct device *dev = &data->hdev->dev;
-	struct backlight_device *bdev;
-	struct backlight_properties props;
-	if (!report)
-		return -ENODEV;
-	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
-			report->field[0]->report_size != 8) {
-		dev_err(dev, "unsupported BRIGHTNESS report");
-		return -EINVAL;
-	}
-
-	memset(&props, 0, sizeof(props));
-	props.type = BACKLIGHT_RAW;
-	props.max_brightness = 0xff;
-	bdev = backlight_device_register(dev_name(dev), dev, data,
-			&picolcd_blops, &props);
-	if (IS_ERR(bdev)) {
-		dev_err(dev, "failed to register backlight\n");
-		return PTR_ERR(bdev);
-	}
-	bdev->props.brightness     = 0xff;
-	data->lcd_brightness       = 0xff;
-	data->backlight            = bdev;
-	picolcd_set_brightness(bdev);
-	return 0;
-}
-
-static void picolcd_exit_backlight(struct picolcd_data *data)
-{
-	struct backlight_device *bdev = data->backlight;
-
-	data->backlight = NULL;
-	if (bdev)
-		backlight_device_unregister(bdev);
-}
-
-static inline int picolcd_resume_backlight(struct picolcd_data *data)
-{
-	if (!data->backlight)
-		return 0;
-	return picolcd_set_brightness(data->backlight);
-}
-
-#ifdef CONFIG_PM
-static void picolcd_suspend_backlight(struct picolcd_data *data)
-{
-	int bl_power = data->lcd_power;
-	if (!data->backlight)
-		return;
-
-	data->backlight->props.power = FB_BLANK_POWERDOWN;
-	picolcd_set_brightness(data->backlight);
-	data->lcd_power = data->backlight->props.power = bl_power;
-}
-#endif /* CONFIG_PM */
-#else
-static inline int picolcd_init_backlight(struct picolcd_data *data,
-		struct hid_report *report)
-{
-	return 0;
-}
-static inline void picolcd_exit_backlight(struct picolcd_data *data)
-{
-}
-static inline int picolcd_resume_backlight(struct picolcd_data *data)
-{
-	return 0;
-}
-static inline void picolcd_suspend_backlight(struct picolcd_data *data)
-{
-}
-#endif /* CONFIG_HID_PICOLCD_BACKLIGHT */
-
-#ifdef CONFIG_HID_PICOLCD_LCD
-/*
- * lcd class device
- */
-static int picolcd_get_contrast(struct lcd_device *ldev)
-{
-	struct picolcd_data *data = lcd_get_data(ldev);
-	return data->lcd_contrast;
-}
-
-static int picolcd_set_contrast(struct lcd_device *ldev, int contrast)
-{
-	struct picolcd_data *data = lcd_get_data(ldev);
-	struct hid_report *report = picolcd_out_report(REPORT_CONTRAST, data->hdev);
-	unsigned long flags;
-
-	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
-		return -ENODEV;
-
-	data->lcd_contrast = contrast & 0x0ff;
-	spin_lock_irqsave(&data->lock, flags);
-	hid_set_field(report->field[0], 0, data->lcd_contrast);
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-	return 0;
-}
-
-static int picolcd_check_lcd_fb(struct lcd_device *ldev, struct fb_info *fb)
-{
-	return fb && fb == picolcd_fbinfo((struct picolcd_data *)lcd_get_data(ldev));
-}
-
-static struct lcd_ops picolcd_lcdops = {
-	.get_contrast   = picolcd_get_contrast,
-	.set_contrast   = picolcd_set_contrast,
-	.check_fb       = picolcd_check_lcd_fb,
-};
-
-static int picolcd_init_lcd(struct picolcd_data *data, struct hid_report *report)
-{
-	struct device *dev = &data->hdev->dev;
-	struct lcd_device *ldev;
-
-	if (!report)
-		return -ENODEV;
-	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
-			report->field[0]->report_size != 8) {
-		dev_err(dev, "unsupported CONTRAST report");
-		return -EINVAL;
-	}
-
-	ldev = lcd_device_register(dev_name(dev), dev, data, &picolcd_lcdops);
-	if (IS_ERR(ldev)) {
-		dev_err(dev, "failed to register LCD\n");
-		return PTR_ERR(ldev);
-	}
-	ldev->props.max_contrast = 0x0ff;
-	data->lcd_contrast = 0xe5;
-	data->lcd = ldev;
-	picolcd_set_contrast(ldev, 0xe5);
-	return 0;
-}
-
-static void picolcd_exit_lcd(struct picolcd_data *data)
-{
-	struct lcd_device *ldev = data->lcd;
-
-	data->lcd = NULL;
-	if (ldev)
-		lcd_device_unregister(ldev);
-}
-
-static inline int picolcd_resume_lcd(struct picolcd_data *data)
-{
-	if (!data->lcd)
-		return 0;
-	return picolcd_set_contrast(data->lcd, data->lcd_contrast);
-}
-#else
-static inline int picolcd_init_lcd(struct picolcd_data *data,
-		struct hid_report *report)
-{
-	return 0;
-}
-static inline void picolcd_exit_lcd(struct picolcd_data *data)
-{
-}
-static inline int picolcd_resume_lcd(struct picolcd_data *data)
-{
-	return 0;
-}
-#endif /* CONFIG_HID_PICOLCD_LCD */
-
-#ifdef CONFIG_HID_PICOLCD_LEDS
-/**
- * LED class device
- */
-static void picolcd_leds_set(struct picolcd_data *data)
-{
-	struct hid_report *report;
-	unsigned long flags;
-
-	if (!data->led[0])
-		return;
-	report = picolcd_out_report(REPORT_LED_STATE, data->hdev);
-	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
-		return;
-
-	spin_lock_irqsave(&data->lock, flags);
-	hid_set_field(report->field[0], 0, data->led_state);
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-}
-
-static void picolcd_led_set_brightness(struct led_classdev *led_cdev,
-			enum led_brightness value)
-{
-	struct device *dev;
-	struct hid_device *hdev;
-	struct picolcd_data *data;
-	int i, state = 0;
-
-	dev  = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
-	data = hid_get_drvdata(hdev);
-	for (i = 0; i < 8; i++) {
-		if (led_cdev != data->led[i])
-			continue;
-		state = (data->led_state >> i) & 1;
-		if (value == LED_OFF && state) {
-			data->led_state &= ~(1 << i);
-			picolcd_leds_set(data);
-		} else if (value != LED_OFF && !state) {
-			data->led_state |= 1 << i;
-			picolcd_leds_set(data);
-		}
-		break;
-	}
-}
-
-static enum led_brightness picolcd_led_get_brightness(struct led_classdev *led_cdev)
-{
-	struct device *dev;
-	struct hid_device *hdev;
-	struct picolcd_data *data;
-	int i, value = 0;
-
-	dev  = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
-	data = hid_get_drvdata(hdev);
-	for (i = 0; i < 8; i++)
-		if (led_cdev == data->led[i]) {
-			value = (data->led_state >> i) & 1;
-			break;
-		}
-	return value ? LED_FULL : LED_OFF;
-}
-
-static int picolcd_init_leds(struct picolcd_data *data, struct hid_report *report)
-{
-	struct device *dev = &data->hdev->dev;
-	struct led_classdev *led;
-	size_t name_sz = strlen(dev_name(dev)) + 8;
-	char *name;
-	int i, ret = 0;
-
-	if (!report)
-		return -ENODEV;
-	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
-			report->field[0]->report_size != 8) {
-		dev_err(dev, "unsupported LED_STATE report");
-		return -EINVAL;
-	}
-
-	for (i = 0; i < 8; i++) {
-		led = kzalloc(sizeof(struct led_classdev)+name_sz, GFP_KERNEL);
-		if (!led) {
-			dev_err(dev, "can't allocate memory for LED %d\n", i);
-			ret = -ENOMEM;
-			goto err;
-		}
-		name = (void *)(&led[1]);
-		snprintf(name, name_sz, "%s::GPO%d", dev_name(dev), i);
-		led->name = name;
-		led->brightness = 0;
-		led->max_brightness = 1;
-		led->brightness_get = picolcd_led_get_brightness;
-		led->brightness_set = picolcd_led_set_brightness;
-
-		data->led[i] = led;
-		ret = led_classdev_register(dev, data->led[i]);
-		if (ret) {
-			data->led[i] = NULL;
-			kfree(led);
-			dev_err(dev, "can't register LED %d\n", i);
-			goto err;
-		}
-	}
-	return 0;
-err:
-	for (i = 0; i < 8; i++)
-		if (data->led[i]) {
-			led = data->led[i];
-			data->led[i] = NULL;
-			led_classdev_unregister(led);
-			kfree(led);
-		}
-	return ret;
-}
-
-static void picolcd_exit_leds(struct picolcd_data *data)
-{
-	struct led_classdev *led;
-	int i;
-
-	for (i = 0; i < 8; i++) {
-		led = data->led[i];
-		data->led[i] = NULL;
-		if (!led)
-			continue;
-		led_classdev_unregister(led);
-		kfree(led);
-	}
-}
-
-#else
-static inline int picolcd_init_leds(struct picolcd_data *data,
-		struct hid_report *report)
-{
-	return 0;
-}
-static inline void picolcd_exit_leds(struct picolcd_data *data)
-{
-}
-static inline int picolcd_leds_set(struct picolcd_data *data)
-{
-	return 0;
-}
-#endif /* CONFIG_HID_PICOLCD_LEDS */
-
-/*
- * input class device
- */
-static int picolcd_raw_keypad(struct picolcd_data *data,
-		struct hid_report *report, u8 *raw_data, int size)
-{
-	/*
-	 * Keypad event
-	 * First and second data bytes list currently pressed keys,
-	 * 0x00 means no key and at most 2 keys may be pressed at same time
-	 */
-	int i, j;
-
-	/* determine newly pressed keys */
-	for (i = 0; i < size; i++) {
-		unsigned int key_code;
-		if (raw_data[i] == 0)
-			continue;
-		for (j = 0; j < sizeof(data->pressed_keys); j++)
-			if (data->pressed_keys[j] == raw_data[i])
-				goto key_already_down;
-		for (j = 0; j < sizeof(data->pressed_keys); j++)
-			if (data->pressed_keys[j] == 0) {
-				data->pressed_keys[j] = raw_data[i];
-				break;
-			}
-		input_event(data->input_keys, EV_MSC, MSC_SCAN, raw_data[i]);
-		if (raw_data[i] < PICOLCD_KEYS)
-			key_code = data->keycode[raw_data[i]];
-		else
-			key_code = KEY_UNKNOWN;
-		if (key_code != KEY_UNKNOWN) {
-			dbg_hid(PICOLCD_NAME " got key press for %u:%d",
-					raw_data[i], key_code);
-			input_report_key(data->input_keys, key_code, 1);
-		}
-		input_sync(data->input_keys);
-key_already_down:
-		continue;
-	}
-
-	/* determine newly released keys */
-	for (j = 0; j < sizeof(data->pressed_keys); j++) {
-		unsigned int key_code;
-		if (data->pressed_keys[j] == 0)
-			continue;
-		for (i = 0; i < size; i++)
-			if (data->pressed_keys[j] == raw_data[i])
-				goto key_still_down;
-		input_event(data->input_keys, EV_MSC, MSC_SCAN, data->pressed_keys[j]);
-		if (data->pressed_keys[j] < PICOLCD_KEYS)
-			key_code = data->keycode[data->pressed_keys[j]];
-		else
-			key_code = KEY_UNKNOWN;
-		if (key_code != KEY_UNKNOWN) {
-			dbg_hid(PICOLCD_NAME " got key release for %u:%d",
-					data->pressed_keys[j], key_code);
-			input_report_key(data->input_keys, key_code, 0);
-		}
-		input_sync(data->input_keys);
-		data->pressed_keys[j] = 0;
-key_still_down:
-		continue;
-	}
-	return 1;
-}
-
-static int picolcd_raw_cir(struct picolcd_data *data,
-		struct hid_report *report, u8 *raw_data, int size)
-{
-	/* Need understanding of CIR data format to implement ... */
-	return 1;
-}
-
-static int picolcd_check_version(struct hid_device *hdev)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	struct picolcd_pending *verinfo;
-	int ret = 0;
-
-	if (!data)
-		return -ENODEV;
-
-	verinfo = picolcd_send_and_wait(hdev, REPORT_VERSION, NULL, 0);
-	if (!verinfo) {
-		hid_err(hdev, "no version response from PicoLCD\n");
-		return -ENODEV;
-	}
-
-	if (verinfo->raw_size == 2) {
-		data->version[0] = verinfo->raw_data[1];
-		data->version[1] = verinfo->raw_data[0];
-		if (data->status & PICOLCD_BOOTLOADER) {
-			hid_info(hdev, "PicoLCD, bootloader version %d.%d\n",
-				 verinfo->raw_data[1], verinfo->raw_data[0]);
-		} else {
-			hid_info(hdev, "PicoLCD, firmware version %d.%d\n",
-				 verinfo->raw_data[1], verinfo->raw_data[0]);
-		}
-	} else {
-		hid_err(hdev, "confused, got unexpected version response from PicoLCD\n");
-		ret = -EINVAL;
-	}
-	kfree(verinfo);
-	return ret;
-}
-
-/*
- * Reset our device and wait for answer to VERSION request
- */
-static int picolcd_reset(struct hid_device *hdev)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	struct hid_report *report = picolcd_out_report(REPORT_RESET, hdev);
-	unsigned long flags;
-	int error;
-
-	if (!data || !report || report->maxfield != 1)
-		return -ENODEV;
-
-	spin_lock_irqsave(&data->lock, flags);
-	if (hdev->product == USB_DEVICE_ID_PICOLCD_BOOTLOADER)
-		data->status |= PICOLCD_BOOTLOADER;
-
-	/* perform the reset */
-	hid_set_field(report->field[0], 0, 1);
-	usbhid_submit_report(hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-
-	error = picolcd_check_version(hdev);
-	if (error)
-		return error;
-
-	picolcd_resume_lcd(data);
-	picolcd_resume_backlight(data);
-#ifdef CONFIG_HID_PICOLCD_FB
-	if (data->fb_info)
-		schedule_delayed_work(&data->fb_info->deferred_work, 0);
-#endif /* CONFIG_HID_PICOLCD_FB */
-
-	picolcd_leds_set(data);
-	return 0;
-}
-
-/*
- * The "operation_mode" sysfs attribute
- */
-static ssize_t picolcd_operation_mode_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-
-	if (data->status & PICOLCD_BOOTLOADER)
-		return snprintf(buf, PAGE_SIZE, "[bootloader] lcd\n");
-	else
-		return snprintf(buf, PAGE_SIZE, "bootloader [lcd]\n");
-}
-
-static ssize_t picolcd_operation_mode_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-	struct hid_report *report = NULL;
-	size_t cnt = count;
-	int timeout = data->opmode_delay;
-	unsigned long flags;
-
-	if (cnt >= 3 && strncmp("lcd", buf, 3) == 0) {
-		if (data->status & PICOLCD_BOOTLOADER)
-			report = picolcd_out_report(REPORT_EXIT_FLASHER, data->hdev);
-		buf += 3;
-		cnt -= 3;
-	} else if (cnt >= 10 && strncmp("bootloader", buf, 10) == 0) {
-		if (!(data->status & PICOLCD_BOOTLOADER))
-			report = picolcd_out_report(REPORT_EXIT_KEYBOARD, data->hdev);
-		buf += 10;
-		cnt -= 10;
-	}
-	if (!report)
-		return -EINVAL;
-
-	while (cnt > 0 && (buf[cnt-1] == '\n' || buf[cnt-1] == '\r'))
-		cnt--;
-	if (cnt != 0)
-		return -EINVAL;
-
-	spin_lock_irqsave(&data->lock, flags);
-	hid_set_field(report->field[0], 0, timeout & 0xff);
-	hid_set_field(report->field[0], 1, (timeout >> 8) & 0xff);
-	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
-	spin_unlock_irqrestore(&data->lock, flags);
-	return count;
-}
-
-static DEVICE_ATTR(operation_mode, 0644, picolcd_operation_mode_show,
-		picolcd_operation_mode_store);
-
-/*
- * The "operation_mode_delay" sysfs attribute
- */
-static ssize_t picolcd_operation_mode_delay_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-
-	return snprintf(buf, PAGE_SIZE, "%hu\n", data->opmode_delay);
-}
-
-static ssize_t picolcd_operation_mode_delay_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct picolcd_data *data = dev_get_drvdata(dev);
-	unsigned u;
-	if (sscanf(buf, "%u", &u) != 1)
-		return -EINVAL;
-	if (u > 30000)
-		return -EINVAL;
-	else
-		data->opmode_delay = u;
-	return count;
-}
-
-static DEVICE_ATTR(operation_mode_delay, 0644, picolcd_operation_mode_delay_show,
-		picolcd_operation_mode_delay_store);
-
-
-#ifdef CONFIG_DEBUG_FS
-/*
- * The "reset" file
- */
-static int picolcd_debug_reset_show(struct seq_file *f, void *p)
-{
-	if (picolcd_fbinfo((struct picolcd_data *)f->private))
-		seq_printf(f, "all fb\n");
-	else
-		seq_printf(f, "all\n");
-	return 0;
-}
-
-static int picolcd_debug_reset_open(struct inode *inode, struct file *f)
-{
-	return single_open(f, picolcd_debug_reset_show, inode->i_private);
-}
-
-static ssize_t picolcd_debug_reset_write(struct file *f, const char __user *user_buf,
-		size_t count, loff_t *ppos)
-{
-	struct picolcd_data *data = ((struct seq_file *)f->private_data)->private;
-	char buf[32];
-	size_t cnt = min(count, sizeof(buf)-1);
-	if (copy_from_user(buf, user_buf, cnt))
-		return -EFAULT;
-
-	while (cnt > 0 && (buf[cnt-1] == ' ' || buf[cnt-1] == '\n'))
-		cnt--;
-	buf[cnt] = '\0';
-	if (strcmp(buf, "all") == 0) {
-		picolcd_reset(data->hdev);
-		picolcd_fb_reset(data, 1);
-	} else if (strcmp(buf, "fb") == 0) {
-		picolcd_fb_reset(data, 1);
-	} else {
-		return -EINVAL;
-	}
-	return count;
-}
-
-static const struct file_operations picolcd_debug_reset_fops = {
-	.owner    = THIS_MODULE,
-	.open     = picolcd_debug_reset_open,
-	.read     = seq_read,
-	.llseek   = seq_lseek,
-	.write    = picolcd_debug_reset_write,
-	.release  = single_release,
-};
-
-/*
- * The "eeprom" file
- */
-static ssize_t picolcd_debug_eeprom_read(struct file *f, char __user *u,
-		size_t s, loff_t *off)
-{
-	struct picolcd_data *data = f->private_data;
-	struct picolcd_pending *resp;
-	u8 raw_data[3];
-	ssize_t ret = -EIO;
-
-	if (s == 0)
-		return -EINVAL;
-	if (*off > 0x0ff)
-		return 0;
-
-	/* prepare buffer with info about what we want to read (addr & len) */
-	raw_data[0] = *off & 0xff;
-	raw_data[1] = (*off >> 8) & 0xff;
-	raw_data[2] = s < 20 ? s : 20;
-	if (*off + raw_data[2] > 0xff)
-		raw_data[2] = 0x100 - *off;
-	resp = picolcd_send_and_wait(data->hdev, REPORT_EE_READ, raw_data,
-			sizeof(raw_data));
-	if (!resp)
-		return -EIO;
-
-	if (resp->in_report && resp->in_report->id == REPORT_EE_DATA) {
-		/* successful read :) */
-		ret = resp->raw_data[2];
-		if (ret > s)
-			ret = s;
-		if (copy_to_user(u, resp->raw_data+3, ret))
-			ret = -EFAULT;
-		else
-			*off += ret;
-	} /* anything else is some kind of IO error */
-
-	kfree(resp);
-	return ret;
-}
-
-static ssize_t picolcd_debug_eeprom_write(struct file *f, const char __user *u,
-		size_t s, loff_t *off)
-{
-	struct picolcd_data *data = f->private_data;
-	struct picolcd_pending *resp;
-	ssize_t ret = -EIO;
-	u8 raw_data[23];
-
-	if (s == 0)
-		return -EINVAL;
-	if (*off > 0x0ff)
-		return -ENOSPC;
-
-	memset(raw_data, 0, sizeof(raw_data));
-	raw_data[0] = *off & 0xff;
-	raw_data[1] = (*off >> 8) & 0xff;
-	raw_data[2] = min((size_t)20, s);
-	if (*off + raw_data[2] > 0xff)
-		raw_data[2] = 0x100 - *off;
-
-	if (copy_from_user(raw_data+3, u, min((u8)20, raw_data[2])))
-		return -EFAULT;
-	resp = picolcd_send_and_wait(data->hdev, REPORT_EE_WRITE, raw_data,
-			sizeof(raw_data));
-
-	if (!resp)
-		return -EIO;
-
-	if (resp->in_report && resp->in_report->id == REPORT_EE_DATA) {
-		/* check if written data matches */
-		if (memcmp(raw_data, resp->raw_data, 3+raw_data[2]) == 0) {
-			*off += raw_data[2];
-			ret = raw_data[2];
-		}
-	}
-	kfree(resp);
-	return ret;
-}
-
-/*
- * Notes:
- * - read/write happens in chunks of at most 20 bytes, it's up to userspace
- *   to loop in order to get more data.
- * - on write errors on otherwise correct write request the bytes
- *   that should have been written are in undefined state.
- */
-static const struct file_operations picolcd_debug_eeprom_fops = {
-	.owner    = THIS_MODULE,
-	.open     = simple_open,
-	.read     = picolcd_debug_eeprom_read,
-	.write    = picolcd_debug_eeprom_write,
-	.llseek   = generic_file_llseek,
-};
-
-/*
- * The "flash" file
- */
-/* record a flash address to buf (bounds check to be done by caller) */
-static int _picolcd_flash_setaddr(struct picolcd_data *data, u8 *buf, long off)
-{
-	buf[0] = off & 0xff;
-	buf[1] = (off >> 8) & 0xff;
-	if (data->addr_sz == 3)
-		buf[2] = (off >> 16) & 0xff;
-	return data->addr_sz == 2 ? 2 : 3;
-}
-
-/* read a given size of data (bounds check to be done by caller) */
-static ssize_t _picolcd_flash_read(struct picolcd_data *data, int report_id,
-		char __user *u, size_t s, loff_t *off)
-{
-	struct picolcd_pending *resp;
-	u8 raw_data[4];
-	ssize_t ret = 0;
-	int len_off, err = -EIO;
-
-	while (s > 0) {
-		err = -EIO;
-		len_off = _picolcd_flash_setaddr(data, raw_data, *off);
-		raw_data[len_off] = s > 32 ? 32 : s;
-		resp = picolcd_send_and_wait(data->hdev, report_id, raw_data, len_off+1);
-		if (!resp || !resp->in_report)
-			goto skip;
-		if (resp->in_report->id == REPORT_MEMORY ||
-			resp->in_report->id == REPORT_BL_READ_MEMORY) {
-			if (memcmp(raw_data, resp->raw_data, len_off+1) != 0)
-				goto skip;
-			if (copy_to_user(u+ret, resp->raw_data+len_off+1, raw_data[len_off])) {
-				err = -EFAULT;
-				goto skip;
-			}
-			*off += raw_data[len_off];
-			s    -= raw_data[len_off];
-			ret  += raw_data[len_off];
-			err   = 0;
-		}
-skip:
-		kfree(resp);
-		if (err)
-			return ret > 0 ? ret : err;
-	}
-	return ret;
-}
-
-static ssize_t picolcd_debug_flash_read(struct file *f, char __user *u,
-		size_t s, loff_t *off)
-{
-	struct picolcd_data *data = f->private_data;
-
-	if (s == 0)
-		return -EINVAL;
-	if (*off > 0x05fff)
-		return 0;
-	if (*off + s > 0x05fff)
-		s = 0x06000 - *off;
-
-	if (data->status & PICOLCD_BOOTLOADER)
-		return _picolcd_flash_read(data, REPORT_BL_READ_MEMORY, u, s, off);
-	else
-		return _picolcd_flash_read(data, REPORT_READ_MEMORY, u, s, off);
-}
-
-/* erase block aligned to 64bytes boundary */
-static ssize_t _picolcd_flash_erase64(struct picolcd_data *data, int report_id,
-		loff_t *off)
-{
-	struct picolcd_pending *resp;
-	u8 raw_data[3];
-	int len_off;
-	ssize_t ret = -EIO;
-
-	if (*off & 0x3f)
-		return -EINVAL;
-
-	len_off = _picolcd_flash_setaddr(data, raw_data, *off);
-	resp = picolcd_send_and_wait(data->hdev, report_id, raw_data, len_off);
-	if (!resp || !resp->in_report)
-		goto skip;
-	if (resp->in_report->id == REPORT_MEMORY ||
-		resp->in_report->id == REPORT_BL_ERASE_MEMORY) {
-		if (memcmp(raw_data, resp->raw_data, len_off) != 0)
-			goto skip;
-		ret = 0;
-	}
-skip:
-	kfree(resp);
-	return ret;
-}
-
-/* write a given size of data (bounds check to be done by caller) */
-static ssize_t _picolcd_flash_write(struct picolcd_data *data, int report_id,
-		const char __user *u, size_t s, loff_t *off)
-{
-	struct picolcd_pending *resp;
-	u8 raw_data[36];
-	ssize_t ret = 0;
-	int len_off, err = -EIO;
-
-	while (s > 0) {
-		err = -EIO;
-		len_off = _picolcd_flash_setaddr(data, raw_data, *off);
-		raw_data[len_off] = s > 32 ? 32 : s;
-		if (copy_from_user(raw_data+len_off+1, u, raw_data[len_off])) {
-			err = -EFAULT;
-			break;
-		}
-		resp = picolcd_send_and_wait(data->hdev, report_id, raw_data,
-				len_off+1+raw_data[len_off]);
-		if (!resp || !resp->in_report)
-			goto skip;
-		if (resp->in_report->id == REPORT_MEMORY ||
-			resp->in_report->id == REPORT_BL_WRITE_MEMORY) {
-			if (memcmp(raw_data, resp->raw_data, len_off+1+raw_data[len_off]) != 0)
-				goto skip;
-			*off += raw_data[len_off];
-			s    -= raw_data[len_off];
-			ret  += raw_data[len_off];
-			err   = 0;
-		}
-skip:
-		kfree(resp);
-		if (err)
-			break;
-	}
-	return ret > 0 ? ret : err;
-}
-
-static ssize_t picolcd_debug_flash_write(struct file *f, const char __user *u,
-		size_t s, loff_t *off)
-{
-	struct picolcd_data *data = f->private_data;
-	ssize_t err, ret = 0;
-	int report_erase, report_write;
-
-	if (s == 0)
-		return -EINVAL;
-	if (*off > 0x5fff)
-		return -ENOSPC;
-	if (s & 0x3f)
-		return -EINVAL;
-	if (*off & 0x3f)
-		return -EINVAL;
-
-	if (data->status & PICOLCD_BOOTLOADER) {
-		report_erase = REPORT_BL_ERASE_MEMORY;
-		report_write = REPORT_BL_WRITE_MEMORY;
-	} else {
-		report_erase = REPORT_ERASE_MEMORY;
-		report_write = REPORT_WRITE_MEMORY;
-	}
-	mutex_lock(&data->mutex_flash);
-	while (s > 0) {
-		err = _picolcd_flash_erase64(data, report_erase, off);
-		if (err)
-			break;
-		err = _picolcd_flash_write(data, report_write, u, 64, off);
-		if (err < 0)
-			break;
-		ret += err;
-		*off += err;
-		s -= err;
-		if (err != 64)
-			break;
-	}
-	mutex_unlock(&data->mutex_flash);
-	return ret > 0 ? ret : err;
-}
-
-/*
- * Notes:
- * - concurrent writing is prevented by mutex and all writes must be
- *   n*64 bytes and 64-byte aligned, each write being preceded by an
- *   ERASE which erases a 64byte block.
- *   If less than requested was written or an error is returned for an
- *   otherwise correct write request the next 64-byte block which should
- *   have been written is in undefined state (mostly: original, erased,
- *   (half-)written with write error)
- * - reading can happen without special restriction
- */
-static const struct file_operations picolcd_debug_flash_fops = {
-	.owner    = THIS_MODULE,
-	.open     = simple_open,
-	.read     = picolcd_debug_flash_read,
-	.write    = picolcd_debug_flash_write,
-	.llseek   = generic_file_llseek,
-};
-
-
-/*
- * Helper code for HID report level dumping/debugging
- */
-static const char *error_codes[] = {
-	"success", "parameter missing", "data_missing", "block readonly",
-	"block not erasable", "block too big", "section overflow",
-	"invalid command length", "invalid data length",
-};
-
-static void dump_buff_as_hex(char *dst, size_t dst_sz, const u8 *data,
-		const size_t data_len)
-{
-	int i, j;
-	for (i = j = 0; i < data_len && j + 3 < dst_sz; i++) {
-		dst[j++] = hex_asc[(data[i] >> 4) & 0x0f];
-		dst[j++] = hex_asc[data[i] & 0x0f];
-		dst[j++] = ' ';
-	}
-	if (j < dst_sz) {
-		dst[j--] = '\0';
-		dst[j] = '\n';
-	} else
-		dst[j] = '\0';
-}
-
-static void picolcd_debug_out_report(struct picolcd_data *data,
-		struct hid_device *hdev, struct hid_report *report)
-{
-	u8 raw_data[70];
-	int raw_size = (report->size >> 3) + 1;
-	char *buff;
-#define BUFF_SZ 256
-
-	/* Avoid unnecessary overhead if debugfs is disabled */
-	if (list_empty(&hdev->debug_list))
-		return;
-
-	buff = kmalloc(BUFF_SZ, GFP_ATOMIC);
-	if (!buff)
-		return;
-
-	snprintf(buff, BUFF_SZ, "\nout report %d (size %d) =  ",
-			report->id, raw_size);
-	hid_debug_event(hdev, buff);
-	if (raw_size + 5 > sizeof(raw_data)) {
-		kfree(buff);
-		hid_debug_event(hdev, " TOO BIG\n");
-		return;
-	} else {
-		raw_data[0] = report->id;
-		hid_output_report(report, raw_data);
-		dump_buff_as_hex(buff, BUFF_SZ, raw_data, raw_size);
-		hid_debug_event(hdev, buff);
-	}
-
-	switch (report->id) {
-	case REPORT_LED_STATE:
-		/* 1 data byte with GPO state */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_LED_STATE", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tGPO state: 0x%02x\n", raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_BRIGHTNESS:
-		/* 1 data byte with brightness */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_BRIGHTNESS", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tBrightness: 0x%02x\n", raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_CONTRAST:
-		/* 1 data byte with contrast */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_CONTRAST", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tContrast: 0x%02x\n", raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_RESET:
-		/* 2 data bytes with reset duration in ms */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_RESET", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tDuration: 0x%02x%02x (%dms)\n",
-				raw_data[2], raw_data[1], raw_data[2] << 8 | raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_LCD_CMD:
-		/* 63 data bytes with LCD commands */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_LCD_CMD", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		/* TODO: format decoding */
-		break;
-	case REPORT_LCD_DATA:
-		/* 63 data bytes with LCD data */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_LCD_CMD", report->id, raw_size-1);
-		/* TODO: format decoding */
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_LCD_CMD_DATA:
-		/* 63 data bytes with LCD commands and data */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_LCD_CMD", report->id, raw_size-1);
-		/* TODO: format decoding */
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_EE_READ:
-		/* 3 data bytes with read area description */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_EE_READ", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_EE_WRITE:
-		/* 3+1..20 data bytes with write area description */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_EE_WRITE", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-		hid_debug_event(hdev, buff);
-		if (raw_data[3] == 0) {
-			snprintf(buff, BUFF_SZ, "\tNo data\n");
-		} else if (raw_data[3] + 4 <= raw_size) {
-			snprintf(buff, BUFF_SZ, "\tData: ");
-			hid_debug_event(hdev, buff);
-			dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
-		} else {
-			snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-		}
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_ERASE_MEMORY:
-	case REPORT_BL_ERASE_MEMORY:
-		/* 3 data bytes with pointer inside erase block */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_ERASE_MEMORY", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		switch (data->addr_sz) {
-		case 2:
-			snprintf(buff, BUFF_SZ, "\tAddress inside 64 byte block: 0x%02x%02x\n",
-					raw_data[2], raw_data[1]);
-			break;
-		case 3:
-			snprintf(buff, BUFF_SZ, "\tAddress inside 64 byte block: 0x%02x%02x%02x\n",
-					raw_data[3], raw_data[2], raw_data[1]);
-			break;
-		default:
-			snprintf(buff, BUFF_SZ, "\tNot supported\n");
-		}
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_READ_MEMORY:
-	case REPORT_BL_READ_MEMORY:
-		/* 4 data bytes with read area description */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_READ_MEMORY", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		switch (data->addr_sz) {
-		case 2:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-					raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-			break;
-		case 3:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
-					raw_data[3], raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
-			break;
-		default:
-			snprintf(buff, BUFF_SZ, "\tNot supported\n");
-		}
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_WRITE_MEMORY:
-	case REPORT_BL_WRITE_MEMORY:
-		/* 4+1..32 data bytes with write adrea description */
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_WRITE_MEMORY", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		switch (data->addr_sz) {
-		case 2:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-					raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-			hid_debug_event(hdev, buff);
-			if (raw_data[3] == 0) {
-				snprintf(buff, BUFF_SZ, "\tNo data\n");
-			} else if (raw_data[3] + 4 <= raw_size) {
-				snprintf(buff, BUFF_SZ, "\tData: ");
-				hid_debug_event(hdev, buff);
-				dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
-			} else {
-				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-			}
-			break;
-		case 3:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
-					raw_data[3], raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
-			hid_debug_event(hdev, buff);
-			if (raw_data[4] == 0) {
-				snprintf(buff, BUFF_SZ, "\tNo data\n");
-			} else if (raw_data[4] + 5 <= raw_size) {
-				snprintf(buff, BUFF_SZ, "\tData: ");
-				hid_debug_event(hdev, buff);
-				dump_buff_as_hex(buff, BUFF_SZ, raw_data+5, raw_data[4]);
-			} else {
-				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-			}
-			break;
-		default:
-			snprintf(buff, BUFF_SZ, "\tNot supported\n");
-		}
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_SPLASH_RESTART:
-		/* TODO */
-		break;
-	case REPORT_EXIT_KEYBOARD:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_EXIT_KEYBOARD", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tRestart delay: %dms (0x%02x%02x)\n",
-				raw_data[1] | (raw_data[2] << 8),
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_VERSION:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_VERSION", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_DEVID:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_DEVID", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_SPLASH_SIZE:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_SPLASH_SIZE", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_HOOK_VERSION:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_HOOK_VERSION", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_EXIT_FLASHER:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"REPORT_VERSION", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tRestart delay: %dms (0x%02x%02x)\n",
-				raw_data[1] | (raw_data[2] << 8),
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	default:
-		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
-			"<unknown>", report->id, raw_size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	}
-	wake_up_interruptible(&hdev->debug_wait);
-	kfree(buff);
-}
-
-static void picolcd_debug_raw_event(struct picolcd_data *data,
-		struct hid_device *hdev, struct hid_report *report,
-		u8 *raw_data, int size)
-{
-	char *buff;
-
-#define BUFF_SZ 256
-	/* Avoid unnecessary overhead if debugfs is disabled */
-	if (!hdev->debug_events)
-		return;
-
-	buff = kmalloc(BUFF_SZ, GFP_ATOMIC);
-	if (!buff)
-		return;
-
-	switch (report->id) {
-	case REPORT_ERROR_CODE:
-		/* 2 data bytes with affected report and error code */
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_ERROR_CODE", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		if (raw_data[2] < ARRAY_SIZE(error_codes))
-			snprintf(buff, BUFF_SZ, "\tError code 0x%02x (%s) in reply to report 0x%02x\n",
-					raw_data[2], error_codes[raw_data[2]], raw_data[1]);
-		else
-			snprintf(buff, BUFF_SZ, "\tError code 0x%02x in reply to report 0x%02x\n",
-					raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_KEY_STATE:
-		/* 2 data bytes with key state */
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_KEY_STATE", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		if (raw_data[1] == 0)
-			snprintf(buff, BUFF_SZ, "\tNo key pressed\n");
-		else if (raw_data[2] == 0)
-			snprintf(buff, BUFF_SZ, "\tOne key pressed: 0x%02x (%d)\n",
-					raw_data[1], raw_data[1]);
-		else
-			snprintf(buff, BUFF_SZ, "\tTwo keys pressed: 0x%02x (%d), 0x%02x (%d)\n",
-					raw_data[1], raw_data[1], raw_data[2], raw_data[2]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_IR_DATA:
-		/* Up to 20 byes of IR scancode data */
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_IR_DATA", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		if (raw_data[1] == 0) {
-			snprintf(buff, BUFF_SZ, "\tUnexpectedly 0 data length\n");
-			hid_debug_event(hdev, buff);
-		} else if (raw_data[1] + 1 <= size) {
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n\tIR Data: ",
-					raw_data[1]-1);
-			hid_debug_event(hdev, buff);
-			dump_buff_as_hex(buff, BUFF_SZ, raw_data+2, raw_data[1]-1);
-			hid_debug_event(hdev, buff);
-		} else {
-			snprintf(buff, BUFF_SZ, "\tOverflowing data length: %d\n",
-					raw_data[1]-1);
-			hid_debug_event(hdev, buff);
-		}
-		break;
-	case REPORT_EE_DATA:
-		/* Data buffer in response to REPORT_EE_READ or REPORT_EE_WRITE */
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_EE_DATA", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-		hid_debug_event(hdev, buff);
-		if (raw_data[3] == 0) {
-			snprintf(buff, BUFF_SZ, "\tNo data\n");
-			hid_debug_event(hdev, buff);
-		} else if (raw_data[3] + 4 <= size) {
-			snprintf(buff, BUFF_SZ, "\tData: ");
-			hid_debug_event(hdev, buff);
-			dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
-			hid_debug_event(hdev, buff);
-		} else {
-			snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-			hid_debug_event(hdev, buff);
-		}
-		break;
-	case REPORT_MEMORY:
-		/* Data buffer in response to REPORT_READ_MEMORY or REPORT_WRTIE_MEMORY */
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_MEMORY", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		switch (data->addr_sz) {
-		case 2:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
-					raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
-			hid_debug_event(hdev, buff);
-			if (raw_data[3] == 0) {
-				snprintf(buff, BUFF_SZ, "\tNo data\n");
-			} else if (raw_data[3] + 4 <= size) {
-				snprintf(buff, BUFF_SZ, "\tData: ");
-				hid_debug_event(hdev, buff);
-				dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
-			} else {
-				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-			}
-			break;
-		case 3:
-			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
-					raw_data[3], raw_data[2], raw_data[1]);
-			hid_debug_event(hdev, buff);
-			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
-			hid_debug_event(hdev, buff);
-			if (raw_data[4] == 0) {
-				snprintf(buff, BUFF_SZ, "\tNo data\n");
-			} else if (raw_data[4] + 5 <= size) {
-				snprintf(buff, BUFF_SZ, "\tData: ");
-				hid_debug_event(hdev, buff);
-				dump_buff_as_hex(buff, BUFF_SZ, raw_data+5, raw_data[4]);
-			} else {
-				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
-			}
-			break;
-		default:
-			snprintf(buff, BUFF_SZ, "\tNot supported\n");
-		}
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_VERSION:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_VERSION", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tFirmware version: %d.%d\n",
-				raw_data[2], raw_data[1]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_BL_ERASE_MEMORY:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_BL_ERASE_MEMORY", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		/* TODO */
-		break;
-	case REPORT_BL_READ_MEMORY:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_BL_READ_MEMORY", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		/* TODO */
-		break;
-	case REPORT_BL_WRITE_MEMORY:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_BL_WRITE_MEMORY", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		/* TODO */
-		break;
-	case REPORT_DEVID:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_DEVID", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tSerial: 0x%02x%02x%02x%02x\n",
-				raw_data[1], raw_data[2], raw_data[3], raw_data[4]);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tType: 0x%02x\n",
-				raw_data[5]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_SPLASH_SIZE:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_SPLASH_SIZE", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tTotal splash space: %d\n",
-				(raw_data[2] << 8) | raw_data[1]);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tUsed splash space: %d\n",
-				(raw_data[4] << 8) | raw_data[3]);
-		hid_debug_event(hdev, buff);
-		break;
-	case REPORT_HOOK_VERSION:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"REPORT_HOOK_VERSION", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		snprintf(buff, BUFF_SZ, "\tFirmware version: %d.%d\n",
-				raw_data[1], raw_data[2]);
-		hid_debug_event(hdev, buff);
-		break;
-	default:
-		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
-			"<unknown>", report->id, size-1);
-		hid_debug_event(hdev, buff);
-		break;
-	}
-	wake_up_interruptible(&hdev->debug_wait);
-	kfree(buff);
-}
-
-static void picolcd_init_devfs(struct picolcd_data *data,
-		struct hid_report *eeprom_r, struct hid_report *eeprom_w,
-		struct hid_report *flash_r, struct hid_report *flash_w,
-		struct hid_report *reset)
-{
-	struct hid_device *hdev = data->hdev;
-
-	mutex_init(&data->mutex_flash);
-
-	/* reset */
-	if (reset)
-		data->debug_reset = debugfs_create_file("reset", 0600,
-				hdev->debug_dir, data, &picolcd_debug_reset_fops);
-
-	/* eeprom */
-	if (eeprom_r || eeprom_w)
-		data->debug_eeprom = debugfs_create_file("eeprom",
-			(eeprom_w ? S_IWUSR : 0) | (eeprom_r ? S_IRUSR : 0),
-			hdev->debug_dir, data, &picolcd_debug_eeprom_fops);
-
-	/* flash */
-	if (flash_r && flash_r->maxfield == 1 && flash_r->field[0]->report_size == 8)
-		data->addr_sz = flash_r->field[0]->report_count - 1;
-	else
-		data->addr_sz = -1;
-	if (data->addr_sz == 2 || data->addr_sz == 3) {
-		data->debug_flash = debugfs_create_file("flash",
-			(flash_w ? S_IWUSR : 0) | (flash_r ? S_IRUSR : 0),
-			hdev->debug_dir, data, &picolcd_debug_flash_fops);
-	} else if (flash_r || flash_w)
-		hid_warn(hdev, "Unexpected FLASH access reports, please submit rdesc for review\n");
-}
-
-static void picolcd_exit_devfs(struct picolcd_data *data)
-{
-	struct dentry *dent;
-
-	dent = data->debug_reset;
-	data->debug_reset = NULL;
-	if (dent)
-		debugfs_remove(dent);
-	dent = data->debug_eeprom;
-	data->debug_eeprom = NULL;
-	if (dent)
-		debugfs_remove(dent);
-	dent = data->debug_flash;
-	data->debug_flash = NULL;
-	if (dent)
-		debugfs_remove(dent);
-	mutex_destroy(&data->mutex_flash);
-}
-#else
-static inline void picolcd_debug_raw_event(struct picolcd_data *data,
-		struct hid_device *hdev, struct hid_report *report,
-		u8 *raw_data, int size)
-{
-}
-static inline void picolcd_init_devfs(struct picolcd_data *data,
-		struct hid_report *eeprom_r, struct hid_report *eeprom_w,
-		struct hid_report *flash_r, struct hid_report *flash_w,
-		struct hid_report *reset)
-{
-}
-static inline void picolcd_exit_devfs(struct picolcd_data *data)
-{
-}
-#endif /* CONFIG_DEBUG_FS */
-
-/*
- * Handle raw report as sent by device
- */
-static int picolcd_raw_event(struct hid_device *hdev,
-		struct hid_report *report, u8 *raw_data, int size)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	unsigned long flags;
-	int ret = 0;
-
-	if (!data)
-		return 1;
-
-	if (report->id == REPORT_KEY_STATE) {
-		if (data->input_keys)
-			ret = picolcd_raw_keypad(data, report, raw_data+1, size-1);
-	} else if (report->id == REPORT_IR_DATA) {
-		if (data->input_cir)
-			ret = picolcd_raw_cir(data, report, raw_data+1, size-1);
-	} else {
-		spin_lock_irqsave(&data->lock, flags);
-		/*
-		 * We let the caller of picolcd_send_and_wait() check if the
-		 * report we got is one of the expected ones or not.
-		 */
-		if (data->pending) {
-			memcpy(data->pending->raw_data, raw_data+1, size-1);
-			data->pending->raw_size  = size-1;
-			data->pending->in_report = report;
-			complete(&data->pending->ready);
-		}
-		spin_unlock_irqrestore(&data->lock, flags);
-	}
-
-	picolcd_debug_raw_event(data, hdev, report, raw_data, size);
-	return 1;
-}
-
-#ifdef CONFIG_PM
-static int picolcd_suspend(struct hid_device *hdev, pm_message_t message)
-{
-	if (PMSG_IS_AUTO(message))
-		return 0;
-
-	picolcd_suspend_backlight(hid_get_drvdata(hdev));
-	dbg_hid(PICOLCD_NAME " device ready for suspend\n");
-	return 0;
-}
-
-static int picolcd_resume(struct hid_device *hdev)
-{
-	int ret;
-	ret = picolcd_resume_backlight(hid_get_drvdata(hdev));
-	if (ret)
-		dbg_hid(PICOLCD_NAME " restoring backlight failed: %d\n", ret);
-	return 0;
-}
-
-static int picolcd_reset_resume(struct hid_device *hdev)
-{
-	int ret;
-	ret = picolcd_reset(hdev);
-	if (ret)
-		dbg_hid(PICOLCD_NAME " resetting our device failed: %d\n", ret);
-	ret = picolcd_fb_reset(hid_get_drvdata(hdev), 0);
-	if (ret)
-		dbg_hid(PICOLCD_NAME " restoring framebuffer content failed: %d\n", ret);
-	ret = picolcd_resume_lcd(hid_get_drvdata(hdev));
-	if (ret)
-		dbg_hid(PICOLCD_NAME " restoring lcd failed: %d\n", ret);
-	ret = picolcd_resume_backlight(hid_get_drvdata(hdev));
-	if (ret)
-		dbg_hid(PICOLCD_NAME " restoring backlight failed: %d\n", ret);
-	picolcd_leds_set(hid_get_drvdata(hdev));
-	return 0;
-}
-#endif
-
-/* initialize keypad input device */
-static int picolcd_init_keys(struct picolcd_data *data,
-		struct hid_report *report)
-{
-	struct hid_device *hdev = data->hdev;
-	struct input_dev *idev;
-	int error, i;
-
-	if (!report)
-		return -ENODEV;
-	if (report->maxfield != 1 || report->field[0]->report_count != 2 ||
-			report->field[0]->report_size != 8) {
-		hid_err(hdev, "unsupported KEY_STATE report\n");
-		return -EINVAL;
-	}
-
-	idev = input_allocate_device();
-	if (idev == NULL) {
-		hid_err(hdev, "failed to allocate input device\n");
-		return -ENOMEM;
-	}
-	input_set_drvdata(idev, hdev);
-	memcpy(data->keycode, def_keymap, sizeof(def_keymap));
-	idev->name = hdev->name;
-	idev->phys = hdev->phys;
-	idev->uniq = hdev->uniq;
-	idev->id.bustype = hdev->bus;
-	idev->id.vendor  = hdev->vendor;
-	idev->id.product = hdev->product;
-	idev->id.version = hdev->version;
-	idev->dev.parent = hdev->dev.parent;
-	idev->keycode     = &data->keycode;
-	idev->keycodemax  = PICOLCD_KEYS;
-	idev->keycodesize = sizeof(data->keycode[0]);
-	input_set_capability(idev, EV_MSC, MSC_SCAN);
-	set_bit(EV_REP, idev->evbit);
-	for (i = 0; i < PICOLCD_KEYS; i++)
-		input_set_capability(idev, EV_KEY, data->keycode[i]);
-	error = input_register_device(idev);
-	if (error) {
-		hid_err(hdev, "error registering the input device\n");
-		input_free_device(idev);
-		return error;
-	}
-	data->input_keys = idev;
-	return 0;
-}
-
-static void picolcd_exit_keys(struct picolcd_data *data)
-{
-	struct input_dev *idev = data->input_keys;
-
-	data->input_keys = NULL;
-	if (idev)
-		input_unregister_device(idev);
-}
-
-/* initialize CIR input device */
-static inline int picolcd_init_cir(struct picolcd_data *data, struct hid_report *report)
-{
-	/* support not implemented yet */
-	return 0;
-}
-
-static inline void picolcd_exit_cir(struct picolcd_data *data)
-{
-}
-
-static int picolcd_probe_lcd(struct hid_device *hdev, struct picolcd_data *data)
-{
-	int error;
-
-	error = picolcd_check_version(hdev);
-	if (error)
-		return error;
-
-	if (data->version[0] != 0 && data->version[1] != 3)
-		hid_info(hdev, "Device with untested firmware revision, please submit /sys/kernel/debug/hid/%s/rdesc for this device.\n",
-			 dev_name(&hdev->dev));
-
-	/* Setup keypad input device */
-	error = picolcd_init_keys(data, picolcd_in_report(REPORT_KEY_STATE, hdev));
-	if (error)
-		goto err;
-
-	/* Setup CIR input device */
-	error = picolcd_init_cir(data, picolcd_in_report(REPORT_IR_DATA, hdev));
-	if (error)
-		goto err;
-
-	/* Set up the framebuffer device */
-	error = picolcd_init_framebuffer(data);
-	if (error)
-		goto err;
-
-	/* Setup lcd class device */
-	error = picolcd_init_lcd(data, picolcd_out_report(REPORT_CONTRAST, hdev));
-	if (error)
-		goto err;
-
-	/* Setup backlight class device */
-	error = picolcd_init_backlight(data, picolcd_out_report(REPORT_BRIGHTNESS, hdev));
-	if (error)
-		goto err;
-
-	/* Setup the LED class devices */
-	error = picolcd_init_leds(data, picolcd_out_report(REPORT_LED_STATE, hdev));
-	if (error)
-		goto err;
-
-	picolcd_init_devfs(data, picolcd_out_report(REPORT_EE_READ, hdev),
-			picolcd_out_report(REPORT_EE_WRITE, hdev),
-			picolcd_out_report(REPORT_READ_MEMORY, hdev),
-			picolcd_out_report(REPORT_WRITE_MEMORY, hdev),
-			picolcd_out_report(REPORT_RESET, hdev));
-	return 0;
-err:
-	picolcd_exit_leds(data);
-	picolcd_exit_backlight(data);
-	picolcd_exit_lcd(data);
-	picolcd_exit_framebuffer(data);
-	picolcd_exit_cir(data);
-	picolcd_exit_keys(data);
-	return error;
-}
-
-static int picolcd_probe_bootloader(struct hid_device *hdev, struct picolcd_data *data)
-{
-	int error;
-
-	error = picolcd_check_version(hdev);
-	if (error)
-		return error;
-
-	if (data->version[0] != 1 && data->version[1] != 0)
-		hid_info(hdev, "Device with untested bootloader revision, please submit /sys/kernel/debug/hid/%s/rdesc for this device.\n",
-			 dev_name(&hdev->dev));
-
-	picolcd_init_devfs(data, NULL, NULL,
-			picolcd_out_report(REPORT_BL_READ_MEMORY, hdev),
-			picolcd_out_report(REPORT_BL_WRITE_MEMORY, hdev), NULL);
-	return 0;
-}
-
-static int picolcd_probe(struct hid_device *hdev,
-		     const struct hid_device_id *id)
-{
-	struct picolcd_data *data;
-	int error = -ENOMEM;
-
-	dbg_hid(PICOLCD_NAME " hardware probe...\n");
-
-	/*
-	 * Let's allocate the picolcd data structure, set some reasonable
-	 * defaults, and associate it with the device
-	 */
-	data = kzalloc(sizeof(struct picolcd_data), GFP_KERNEL);
-	if (data == NULL) {
-		hid_err(hdev, "can't allocate space for Minibox PicoLCD device data\n");
-		error = -ENOMEM;
-		goto err_no_cleanup;
-	}
-
-	spin_lock_init(&data->lock);
-	mutex_init(&data->mutex);
-	data->hdev = hdev;
-	data->opmode_delay = 5000;
-	if (hdev->product == USB_DEVICE_ID_PICOLCD_BOOTLOADER)
-		data->status |= PICOLCD_BOOTLOADER;
-	hid_set_drvdata(hdev, data);
-
-	/* Parse the device reports and start it up */
-	error = hid_parse(hdev);
-	if (error) {
-		hid_err(hdev, "device report parse failed\n");
-		goto err_cleanup_data;
-	}
-
-	error = hid_hw_start(hdev, 0);
-	if (error) {
-		hid_err(hdev, "hardware start failed\n");
-		goto err_cleanup_data;
-	}
-
-	error = hid_hw_open(hdev);
-	if (error) {
-		hid_err(hdev, "failed to open input interrupt pipe for key and IR events\n");
-		goto err_cleanup_hid_hw;
-	}
-
-	error = device_create_file(&hdev->dev, &dev_attr_operation_mode_delay);
-	if (error) {
-		hid_err(hdev, "failed to create sysfs attributes\n");
-		goto err_cleanup_hid_ll;
-	}
-
-	error = device_create_file(&hdev->dev, &dev_attr_operation_mode);
-	if (error) {
-		hid_err(hdev, "failed to create sysfs attributes\n");
-		goto err_cleanup_sysfs1;
-	}
-
-	if (data->status & PICOLCD_BOOTLOADER)
-		error = picolcd_probe_bootloader(hdev, data);
-	else
-		error = picolcd_probe_lcd(hdev, data);
-	if (error)
-		goto err_cleanup_sysfs2;
-
-	dbg_hid(PICOLCD_NAME " activated and initialized\n");
-	return 0;
-
-err_cleanup_sysfs2:
-	device_remove_file(&hdev->dev, &dev_attr_operation_mode);
-err_cleanup_sysfs1:
-	device_remove_file(&hdev->dev, &dev_attr_operation_mode_delay);
-err_cleanup_hid_ll:
-	hid_hw_close(hdev);
-err_cleanup_hid_hw:
-	hid_hw_stop(hdev);
-err_cleanup_data:
-	kfree(data);
-err_no_cleanup:
-	hid_set_drvdata(hdev, NULL);
-
-	return error;
-}
-
-static void picolcd_remove(struct hid_device *hdev)
-{
-	struct picolcd_data *data = hid_get_drvdata(hdev);
-	unsigned long flags;
-
-	dbg_hid(PICOLCD_NAME " hardware remove...\n");
-	spin_lock_irqsave(&data->lock, flags);
-	data->status |= PICOLCD_FAILED;
-	spin_unlock_irqrestore(&data->lock, flags);
-#ifdef CONFIG_HID_PICOLCD_FB
-	/* short-circuit FB as early as possible in order to
-	 * avoid long delays if we host console.
-	 */
-	if (data->fb_info)
-		data->fb_info->par = NULL;
-#endif
-
-	picolcd_exit_devfs(data);
-	device_remove_file(&hdev->dev, &dev_attr_operation_mode);
-	device_remove_file(&hdev->dev, &dev_attr_operation_mode_delay);
-	hid_hw_close(hdev);
-	hid_hw_stop(hdev);
-	hid_set_drvdata(hdev, NULL);
-
-	/* Shortcut potential pending reply that will never arrive */
-	spin_lock_irqsave(&data->lock, flags);
-	if (data->pending)
-		complete(&data->pending->ready);
-	spin_unlock_irqrestore(&data->lock, flags);
-
-	/* Cleanup LED */
-	picolcd_exit_leds(data);
-	/* Clean up the framebuffer */
-	picolcd_exit_backlight(data);
-	picolcd_exit_lcd(data);
-	picolcd_exit_framebuffer(data);
-	/* Cleanup input */
-	picolcd_exit_cir(data);
-	picolcd_exit_keys(data);
-
-	mutex_destroy(&data->mutex);
-	/* Finally, clean up the picolcd data itself */
-	kfree(data);
-}
-
-static const struct hid_device_id picolcd_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD_BOOTLOADER) },
-	{ }
-};
-MODULE_DEVICE_TABLE(hid, picolcd_devices);
-
-static struct hid_driver picolcd_driver = {
-	.name =          "hid-picolcd",
-	.id_table =      picolcd_devices,
-	.probe =         picolcd_probe,
-	.remove =        picolcd_remove,
-	.raw_event =     picolcd_raw_event,
-#ifdef CONFIG_PM
-	.suspend =       picolcd_suspend,
-	.resume =        picolcd_resume,
-	.reset_resume =  picolcd_reset_resume,
-#endif
-};
-
-static int __init picolcd_init(void)
-{
-	return hid_register_driver(&picolcd_driver);
-}
-
-static void __exit picolcd_exit(void)
-{
-	hid_unregister_driver(&picolcd_driver);
-#ifdef CONFIG_HID_PICOLCD_FB
-	flush_work_sync(&picolcd_fb_cleanup);
-	WARN_ON(fb_pending);
-#endif
-}
-
-module_init(picolcd_init);
-module_exit(picolcd_exit);
-MODULE_DESCRIPTION("Minibox graphics PicoLCD Driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-picolcd.h b/drivers/hid/hid-picolcd.h
new file mode 100644
index 000000000000..020cef69f6a1
--- /dev/null
+++ b/drivers/hid/hid-picolcd.h
@@ -0,0 +1,309 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#define PICOLCD_NAME "PicoLCD (graphic)"
+
+/* Report numbers */
+#define REPORT_ERROR_CODE      0x10 /* LCD: IN[16]  */
+#define   ERR_SUCCESS            0x00
+#define   ERR_PARAMETER_MISSING  0x01
+#define   ERR_DATA_MISSING       0x02
+#define   ERR_BLOCK_READ_ONLY    0x03
+#define   ERR_BLOCK_NOT_ERASABLE 0x04
+#define   ERR_BLOCK_TOO_BIG      0x05
+#define   ERR_SECTION_OVERFLOW   0x06
+#define   ERR_INVALID_CMD_LEN    0x07
+#define   ERR_INVALID_DATA_LEN   0x08
+#define REPORT_KEY_STATE       0x11 /* LCD: IN[2]   */
+#define REPORT_IR_DATA         0x21 /* LCD: IN[63]  */
+#define REPORT_EE_DATA         0x32 /* LCD: IN[63]  */
+#define REPORT_MEMORY          0x41 /* LCD: IN[63]  */
+#define REPORT_LED_STATE       0x81 /* LCD: OUT[1]  */
+#define REPORT_BRIGHTNESS      0x91 /* LCD: OUT[1]  */
+#define REPORT_CONTRAST        0x92 /* LCD: OUT[1]  */
+#define REPORT_RESET           0x93 /* LCD: OUT[2]  */
+#define REPORT_LCD_CMD         0x94 /* LCD: OUT[63] */
+#define REPORT_LCD_DATA        0x95 /* LCD: OUT[63] */
+#define REPORT_LCD_CMD_DATA    0x96 /* LCD: OUT[63] */
+#define	REPORT_EE_READ         0xa3 /* LCD: OUT[63] */
+#define REPORT_EE_WRITE        0xa4 /* LCD: OUT[63] */
+#define REPORT_ERASE_MEMORY    0xb2 /* LCD: OUT[2]  */
+#define REPORT_READ_MEMORY     0xb3 /* LCD: OUT[3]  */
+#define REPORT_WRITE_MEMORY    0xb4 /* LCD: OUT[63] */
+#define REPORT_SPLASH_RESTART  0xc1 /* LCD: OUT[1]  */
+#define REPORT_EXIT_KEYBOARD   0xef /* LCD: OUT[2]  */
+#define REPORT_VERSION         0xf1 /* LCD: IN[2],OUT[1]    Bootloader: IN[2],OUT[1]   */
+#define REPORT_BL_ERASE_MEMORY 0xf2 /*                      Bootloader: IN[36],OUT[4]  */
+#define REPORT_BL_READ_MEMORY  0xf3 /*                      Bootloader: IN[36],OUT[4]  */
+#define REPORT_BL_WRITE_MEMORY 0xf4 /*                      Bootloader: IN[36],OUT[36] */
+#define REPORT_DEVID           0xf5 /* LCD: IN[5], OUT[1]   Bootloader: IN[5],OUT[1]   */
+#define REPORT_SPLASH_SIZE     0xf6 /* LCD: IN[4], OUT[1]   */
+#define REPORT_HOOK_VERSION    0xf7 /* LCD: IN[2], OUT[1]   */
+#define REPORT_EXIT_FLASHER    0xff /*                      Bootloader: OUT[2]         */
+
+/* Description of in-progress IO operation, used for operations
+ * that trigger response from device */
+struct picolcd_pending {
+	struct hid_report *out_report;
+	struct hid_report *in_report;
+	struct completion ready;
+	int raw_size;
+	u8 raw_data[64];
+};
+
+
+#define PICOLCD_KEYS 17
+
+/* Per device data structure */
+struct picolcd_data {
+	struct hid_device *hdev;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debug_reset;
+	struct dentry *debug_eeprom;
+	struct dentry *debug_flash;
+	struct mutex mutex_flash;
+	int addr_sz;
+#endif
+	u8 version[2];
+	unsigned short opmode_delay;
+	/* input stuff */
+	u8 pressed_keys[2];
+	struct input_dev *input_keys;
+#ifdef CONFIG_HID_PICOLCD_CIR
+	struct rc_dev *rc_dev;
+#endif
+	unsigned short keycode[PICOLCD_KEYS];
+
+#ifdef CONFIG_HID_PICOLCD_FB
+	/* Framebuffer stuff */
+	struct fb_info *fb_info;
+#endif /* CONFIG_HID_PICOLCD_FB */
+#ifdef CONFIG_HID_PICOLCD_LCD
+	struct lcd_device *lcd;
+	u8 lcd_contrast;
+#endif /* CONFIG_HID_PICOLCD_LCD */
+#ifdef CONFIG_HID_PICOLCD_BACKLIGHT
+	struct backlight_device *backlight;
+	u8 lcd_brightness;
+	u8 lcd_power;
+#endif /* CONFIG_HID_PICOLCD_BACKLIGHT */
+#ifdef CONFIG_HID_PICOLCD_LEDS
+	/* LED stuff */
+	u8 led_state;
+	struct led_classdev *led[8];
+#endif /* CONFIG_HID_PICOLCD_LEDS */
+
+	/* Housekeeping stuff */
+	spinlock_t lock;
+	struct mutex mutex;
+	struct picolcd_pending *pending;
+	int status;
+#define PICOLCD_BOOTLOADER 1
+#define PICOLCD_FAILED 2
+#define PICOLCD_CIR_SHUN 4
+};
+
+#ifdef CONFIG_HID_PICOLCD_FB
+struct picolcd_fb_data {
+	/* Framebuffer stuff */
+	spinlock_t lock;
+	struct picolcd_data *picolcd;
+	u8 update_rate;
+	u8 bpp;
+	u8 force;
+	u8 ready;
+	u8 *vbitmap;		/* local copy of what was sent to PicoLCD */
+	u8 *bitmap;		/* framebuffer */
+};
+#endif /* CONFIG_HID_PICOLCD_FB */
+
+/* Find a given report */
+#define picolcd_in_report(id, dev) picolcd_report(id, dev, HID_INPUT_REPORT)
+#define picolcd_out_report(id, dev) picolcd_report(id, dev, HID_OUTPUT_REPORT)
+
+struct hid_report *picolcd_report(int id, struct hid_device *hdev, int dir);
+
+#ifdef CONFIG_DEBUG_FS
+void picolcd_debug_out_report(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report);
+#define usbhid_submit_report(a, b, c) \
+	do { \
+		picolcd_debug_out_report(hid_get_drvdata(a), a, b); \
+		usbhid_submit_report(a, b, c); \
+	} while (0)
+
+void picolcd_debug_raw_event(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report,
+		u8 *raw_data, int size);
+
+void picolcd_init_devfs(struct picolcd_data *data,
+		struct hid_report *eeprom_r, struct hid_report *eeprom_w,
+		struct hid_report *flash_r, struct hid_report *flash_w,
+		struct hid_report *reset);
+
+void picolcd_exit_devfs(struct picolcd_data *data);
+#else
+static inline void picolcd_debug_out_report(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report)
+{
+}
+static inline void picolcd_debug_raw_event(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report,
+		u8 *raw_data, int size)
+{
+}
+static inline void picolcd_init_devfs(struct picolcd_data *data,
+		struct hid_report *eeprom_r, struct hid_report *eeprom_w,
+		struct hid_report *flash_r, struct hid_report *flash_w,
+		struct hid_report *reset)
+{
+}
+static inline void picolcd_exit_devfs(struct picolcd_data *data)
+{
+}
+#endif /* CONFIG_DEBUG_FS */
+
+
+#ifdef CONFIG_HID_PICOLCD_FB
+int picolcd_fb_reset(struct picolcd_data *data, int clear);
+
+int picolcd_init_framebuffer(struct picolcd_data *data);
+
+void picolcd_exit_framebuffer(struct picolcd_data *data);
+
+void picolcd_fb_refresh(struct picolcd_data *data);
+#define picolcd_fbinfo(d) ((d)->fb_info)
+#else
+static inline int picolcd_fb_reset(struct picolcd_data *data, int clear)
+{
+	return 0;
+}
+static inline int picolcd_init_framebuffer(struct picolcd_data *data)
+{
+	return 0;
+}
+static inline void picolcd_exit_framebuffer(struct picolcd_data *data)
+{
+}
+static inline void picolcd_fb_refresh(struct picolcd_data *data)
+{
+}
+#define picolcd_fbinfo(d) NULL
+#endif /* CONFIG_HID_PICOLCD_FB */
+
+
+#ifdef CONFIG_HID_PICOLCD_BACKLIGHT
+int picolcd_init_backlight(struct picolcd_data *data,
+		struct hid_report *report);
+
+void picolcd_exit_backlight(struct picolcd_data *data);
+
+int picolcd_resume_backlight(struct picolcd_data *data);
+
+void picolcd_suspend_backlight(struct picolcd_data *data);
+#else
+static inline int picolcd_init_backlight(struct picolcd_data *data,
+		struct hid_report *report)
+{
+	return 0;
+}
+static inline void picolcd_exit_backlight(struct picolcd_data *data)
+{
+}
+static inline int picolcd_resume_backlight(struct picolcd_data *data)
+{
+	return 0;
+}
+static inline void picolcd_suspend_backlight(struct picolcd_data *data)
+{
+}
+
+#endif /* CONFIG_HID_PICOLCD_BACKLIGHT */
+
+
+#ifdef CONFIG_HID_PICOLCD_LCD
+int picolcd_init_lcd(struct picolcd_data *data,
+		struct hid_report *report);
+
+void picolcd_exit_lcd(struct picolcd_data *data);
+
+int picolcd_resume_lcd(struct picolcd_data *data);
+#else
+static inline int picolcd_init_lcd(struct picolcd_data *data,
+		struct hid_report *report)
+{
+	return 0;
+}
+static inline void picolcd_exit_lcd(struct picolcd_data *data)
+{
+}
+static inline int picolcd_resume_lcd(struct picolcd_data *data)
+{
+	return 0;
+}
+#endif /* CONFIG_HID_PICOLCD_LCD */
+
+
+#ifdef CONFIG_HID_PICOLCD_LEDS
+int picolcd_init_leds(struct picolcd_data *data,
+		struct hid_report *report);
+
+void picolcd_exit_leds(struct picolcd_data *data);
+
+void picolcd_leds_set(struct picolcd_data *data);
+#else
+static inline int picolcd_init_leds(struct picolcd_data *data,
+		struct hid_report *report)
+{
+	return 0;
+}
+static inline void picolcd_exit_leds(struct picolcd_data *data)
+{
+}
+static inline void picolcd_leds_set(struct picolcd_data *data)
+{
+}
+#endif /* CONFIG_HID_PICOLCD_LEDS */
+
+
+#ifdef CONFIG_HID_PICOLCD_CIR
+int picolcd_raw_cir(struct picolcd_data *data,
+		struct hid_report *report, u8 *raw_data, int size);
+
+int picolcd_init_cir(struct picolcd_data *data, struct hid_report *report);
+
+void picolcd_exit_cir(struct picolcd_data *data);
+#else
+static inline int picolcd_raw_cir(struct picolcd_data *data,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	return 1;
+}
+static inline int picolcd_init_cir(struct picolcd_data *data, struct hid_report *report)
+{
+	return 0;
+}
+static inline void picolcd_exit_cir(struct picolcd_data *data)
+{
+}
+#endif /* CONFIG_HID_PICOLCD_LIRC */
+
+int picolcd_reset(struct hid_device *hdev);
+struct picolcd_pending *picolcd_send_and_wait(struct hid_device *hdev,
+			int report_id, const u8 *raw_data, int size);
diff --git a/drivers/hid/hid-picolcd_backlight.c b/drivers/hid/hid-picolcd_backlight.c
new file mode 100644
index 000000000000..b91f30945f9c
--- /dev/null
+++ b/drivers/hid/hid-picolcd_backlight.c
@@ -0,0 +1,122 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/backlight.h>
+
+#include "hid-picolcd.h"
+
+static int picolcd_get_brightness(struct backlight_device *bdev)
+{
+	struct picolcd_data *data = bl_get_data(bdev);
+	return data->lcd_brightness;
+}
+
+static int picolcd_set_brightness(struct backlight_device *bdev)
+{
+	struct picolcd_data *data = bl_get_data(bdev);
+	struct hid_report *report = picolcd_out_report(REPORT_BRIGHTNESS, data->hdev);
+	unsigned long flags;
+
+	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
+		return -ENODEV;
+
+	data->lcd_brightness = bdev->props.brightness & 0x0ff;
+	data->lcd_power      = bdev->props.power;
+	spin_lock_irqsave(&data->lock, flags);
+	hid_set_field(report->field[0], 0, data->lcd_power == FB_BLANK_UNBLANK ? data->lcd_brightness : 0);
+	if (!(data->status & PICOLCD_FAILED))
+		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+	return 0;
+}
+
+static int picolcd_check_bl_fb(struct backlight_device *bdev, struct fb_info *fb)
+{
+	return fb && fb == picolcd_fbinfo((struct picolcd_data *)bl_get_data(bdev));
+}
+
+static const struct backlight_ops picolcd_blops = {
+	.update_status  = picolcd_set_brightness,
+	.get_brightness = picolcd_get_brightness,
+	.check_fb       = picolcd_check_bl_fb,
+};
+
+int picolcd_init_backlight(struct picolcd_data *data, struct hid_report *report)
+{
+	struct device *dev = &data->hdev->dev;
+	struct backlight_device *bdev;
+	struct backlight_properties props;
+	if (!report)
+		return -ENODEV;
+	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
+			report->field[0]->report_size != 8) {
+		dev_err(dev, "unsupported BRIGHTNESS report");
+		return -EINVAL;
+	}
+
+	memset(&props, 0, sizeof(props));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = 0xff;
+	bdev = backlight_device_register(dev_name(dev), dev, data,
+			&picolcd_blops, &props);
+	if (IS_ERR(bdev)) {
+		dev_err(dev, "failed to register backlight\n");
+		return PTR_ERR(bdev);
+	}
+	bdev->props.brightness     = 0xff;
+	data->lcd_brightness       = 0xff;
+	data->backlight            = bdev;
+	picolcd_set_brightness(bdev);
+	return 0;
+}
+
+void picolcd_exit_backlight(struct picolcd_data *data)
+{
+	struct backlight_device *bdev = data->backlight;
+
+	data->backlight = NULL;
+	if (bdev)
+		backlight_device_unregister(bdev);
+}
+
+int picolcd_resume_backlight(struct picolcd_data *data)
+{
+	if (!data->backlight)
+		return 0;
+	return picolcd_set_brightness(data->backlight);
+}
+
+#ifdef CONFIG_PM
+void picolcd_suspend_backlight(struct picolcd_data *data)
+{
+	int bl_power = data->lcd_power;
+	if (!data->backlight)
+		return;
+
+	data->backlight->props.power = FB_BLANK_POWERDOWN;
+	picolcd_set_brightness(data->backlight);
+	data->lcd_power = data->backlight->props.power = bl_power;
+}
+#endif /* CONFIG_PM */
+
diff --git a/drivers/hid/hid-picolcd_cir.c b/drivers/hid/hid-picolcd_cir.c
new file mode 100644
index 000000000000..13ca9191b630
--- /dev/null
+++ b/drivers/hid/hid-picolcd_cir.c
@@ -0,0 +1,152 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include <linux/hid-debug.h>
+#include <linux/input.h>
+#include "hid-ids.h"
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/vmalloc.h>
+#include <linux/backlight.h>
+#include <linux/lcd.h>
+
+#include <linux/leds.h>
+
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+
+#include <linux/completion.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <media/rc-core.h>
+
+#include "hid-picolcd.h"
+
+
+int picolcd_raw_cir(struct picolcd_data *data,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	unsigned long flags;
+	int i, w, sz;
+	DEFINE_IR_RAW_EVENT(rawir);
+
+	/* ignore if rc_dev is NULL or status is shunned */
+	spin_lock_irqsave(&data->lock, flags);
+	if (!data->rc_dev || (data->status & PICOLCD_CIR_SHUN)) {
+		spin_unlock_irqrestore(&data->lock, flags);
+		return 1;
+	}
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	/* PicoLCD USB packets contain 16-bit intervals in network order,
+	 * with value negated for pulse. Intervals are in microseconds.
+	 *
+	 * Note: some userspace LIRC code for PicoLCD says negated values
+	 * for space - is it a matter of IR chip? (pulse for my TSOP2236)
+	 *
+	 * In addition, the first interval seems to be around 15000 + base
+	 * interval for non-first report of IR data - thus the quirk below
+	 * to get RC_CODE to understand Sony and JVC remotes I have at hand
+	 */
+	sz = size > 0 ? min((int)raw_data[0], size-1) : 0;
+	for (i = 0; i+1 < sz; i += 2) {
+		init_ir_raw_event(&rawir);
+		w = (raw_data[i] << 8) | (raw_data[i+1]);
+		rawir.pulse = !!(w & 0x8000);
+		rawir.duration = US_TO_NS(rawir.pulse ? (65536 - w) : w);
+		/* Quirk!! - see above */
+		if (i == 0 && rawir.duration > 15000000)
+			rawir.duration -= 15000000;
+		ir_raw_event_store(data->rc_dev, &rawir);
+	}
+	ir_raw_event_handle(data->rc_dev);
+
+	return 1;
+}
+
+static int picolcd_cir_open(struct rc_dev *dev)
+{
+	struct picolcd_data *data = dev->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&data->lock, flags);
+	data->status &= ~PICOLCD_CIR_SHUN;
+	spin_unlock_irqrestore(&data->lock, flags);
+	return 0;
+}
+
+static void picolcd_cir_close(struct rc_dev *dev)
+{
+	struct picolcd_data *data = dev->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&data->lock, flags);
+	data->status |= PICOLCD_CIR_SHUN;
+	spin_unlock_irqrestore(&data->lock, flags);
+}
+
+/* initialize CIR input device */
+int picolcd_init_cir(struct picolcd_data *data, struct hid_report *report)
+{
+	struct rc_dev *rdev;
+	int ret = 0;
+
+	rdev = rc_allocate_device();
+	if (!rdev)
+		return -ENOMEM;
+
+	rdev->priv             = data;
+	rdev->driver_type      = RC_DRIVER_IR_RAW;
+	rdev->allowed_protos   = RC_TYPE_ALL;
+	rdev->open             = picolcd_cir_open;
+	rdev->close            = picolcd_cir_close;
+	rdev->input_name       = data->hdev->name;
+	rdev->input_phys       = data->hdev->phys;
+	rdev->input_id.bustype = data->hdev->bus;
+	rdev->input_id.vendor  = data->hdev->vendor;
+	rdev->input_id.product = data->hdev->product;
+	rdev->input_id.version = data->hdev->version;
+	rdev->dev.parent       = &data->hdev->dev;
+	rdev->driver_name      = PICOLCD_NAME;
+	rdev->map_name         = RC_MAP_RC6_MCE;
+	rdev->timeout          = MS_TO_NS(100);
+	rdev->rx_resolution    = US_TO_NS(1);
+
+	ret = rc_register_device(rdev);
+	if (ret)
+		goto err;
+	data->rc_dev = rdev;
+	return 0;
+
+err:
+	rc_free_device(rdev);
+	return ret;
+}
+
+void picolcd_exit_cir(struct picolcd_data *data)
+{
+	struct rc_dev *rdev = data->rc_dev;
+
+	data->rc_dev = NULL;
+	rc_unregister_device(rdev);
+}
+
diff --git a/drivers/hid/hid-picolcd_core.c b/drivers/hid/hid-picolcd_core.c
new file mode 100644
index 000000000000..86df26e58aba
--- /dev/null
+++ b/drivers/hid/hid-picolcd_core.c
@@ -0,0 +1,689 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include <linux/hid-debug.h>
+#include <linux/input.h>
+#include "hid-ids.h"
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/vmalloc.h>
+
+#include <linux/completion.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+
+#include "hid-picolcd.h"
+
+
+/* Input device
+ *
+ * The PicoLCD has an IR receiver header, a built-in keypad with 5 keys
+ * and header for 4x4 key matrix. The built-in keys are part of the matrix.
+ */
+static const unsigned short def_keymap[PICOLCD_KEYS] = {
+	KEY_RESERVED,	/* none */
+	KEY_BACK,	/* col 4 + row 1 */
+	KEY_HOMEPAGE,	/* col 3 + row 1 */
+	KEY_RESERVED,	/* col 2 + row 1 */
+	KEY_RESERVED,	/* col 1 + row 1 */
+	KEY_SCROLLUP,	/* col 4 + row 2 */
+	KEY_OK,		/* col 3 + row 2 */
+	KEY_SCROLLDOWN,	/* col 2 + row 2 */
+	KEY_RESERVED,	/* col 1 + row 2 */
+	KEY_RESERVED,	/* col 4 + row 3 */
+	KEY_RESERVED,	/* col 3 + row 3 */
+	KEY_RESERVED,	/* col 2 + row 3 */
+	KEY_RESERVED,	/* col 1 + row 3 */
+	KEY_RESERVED,	/* col 4 + row 4 */
+	KEY_RESERVED,	/* col 3 + row 4 */
+	KEY_RESERVED,	/* col 2 + row 4 */
+	KEY_RESERVED,	/* col 1 + row 4 */
+};
+
+
+/* Find a given report */
+struct hid_report *picolcd_report(int id, struct hid_device *hdev, int dir)
+{
+	struct list_head *feature_report_list = &hdev->report_enum[dir].report_list;
+	struct hid_report *report = NULL;
+
+	list_for_each_entry(report, feature_report_list, list) {
+		if (report->id == id)
+			return report;
+	}
+	hid_warn(hdev, "No report with id 0x%x found\n", id);
+	return NULL;
+}
+
+/* Submit a report and wait for a reply from device - if device fades away
+ * or does not respond in time, return NULL */
+struct picolcd_pending *picolcd_send_and_wait(struct hid_device *hdev,
+		int report_id, const u8 *raw_data, int size)
+{
+	struct picolcd_data *data = hid_get_drvdata(hdev);
+	struct picolcd_pending *work;
+	struct hid_report *report = picolcd_out_report(report_id, hdev);
+	unsigned long flags;
+	int i, j, k;
+
+	if (!report || !data)
+		return NULL;
+	if (data->status & PICOLCD_FAILED)
+		return NULL;
+	work = kzalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return NULL;
+
+	init_completion(&work->ready);
+	work->out_report = report;
+	work->in_report  = NULL;
+	work->raw_size   = 0;
+
+	mutex_lock(&data->mutex);
+	spin_lock_irqsave(&data->lock, flags);
+	for (i = k = 0; i < report->maxfield; i++)
+		for (j = 0; j < report->field[i]->report_count; j++) {
+			hid_set_field(report->field[i], j, k < size ? raw_data[k] : 0);
+			k++;
+		}
+	if (data->status & PICOLCD_FAILED) {
+		kfree(work);
+		work = NULL;
+	} else {
+		data->pending = work;
+		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+		spin_unlock_irqrestore(&data->lock, flags);
+		wait_for_completion_interruptible_timeout(&work->ready, HZ*2);
+		spin_lock_irqsave(&data->lock, flags);
+		data->pending = NULL;
+	}
+	spin_unlock_irqrestore(&data->lock, flags);
+	mutex_unlock(&data->mutex);
+	return work;
+}
+
+/*
+ * input class device
+ */
+static int picolcd_raw_keypad(struct picolcd_data *data,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	/*
+	 * Keypad event
+	 * First and second data bytes list currently pressed keys,
+	 * 0x00 means no key and at most 2 keys may be pressed at same time
+	 */
+	int i, j;
+
+	/* determine newly pressed keys */
+	for (i = 0; i < size; i++) {
+		unsigned int key_code;
+		if (raw_data[i] == 0)
+			continue;
+		for (j = 0; j < sizeof(data->pressed_keys); j++)
+			if (data->pressed_keys[j] == raw_data[i])
+				goto key_already_down;
+		for (j = 0; j < sizeof(data->pressed_keys); j++)
+			if (data->pressed_keys[j] == 0) {
+				data->pressed_keys[j] = raw_data[i];
+				break;
+			}
+		input_event(data->input_keys, EV_MSC, MSC_SCAN, raw_data[i]);
+		if (raw_data[i] < PICOLCD_KEYS)
+			key_code = data->keycode[raw_data[i]];
+		else
+			key_code = KEY_UNKNOWN;
+		if (key_code != KEY_UNKNOWN) {
+			dbg_hid(PICOLCD_NAME " got key press for %u:%d",
+					raw_data[i], key_code);
+			input_report_key(data->input_keys, key_code, 1);
+		}
+		input_sync(data->input_keys);
+key_already_down:
+		continue;
+	}
+
+	/* determine newly released keys */
+	for (j = 0; j < sizeof(data->pressed_keys); j++) {
+		unsigned int key_code;
+		if (data->pressed_keys[j] == 0)
+			continue;
+		for (i = 0; i < size; i++)
+			if (data->pressed_keys[j] == raw_data[i])
+				goto key_still_down;
+		input_event(data->input_keys, EV_MSC, MSC_SCAN, data->pressed_keys[j]);
+		if (data->pressed_keys[j] < PICOLCD_KEYS)
+			key_code = data->keycode[data->pressed_keys[j]];
+		else
+			key_code = KEY_UNKNOWN;
+		if (key_code != KEY_UNKNOWN) {
+			dbg_hid(PICOLCD_NAME " got key release for %u:%d",
+					data->pressed_keys[j], key_code);
+			input_report_key(data->input_keys, key_code, 0);
+		}
+		input_sync(data->input_keys);
+		data->pressed_keys[j] = 0;
+key_still_down:
+		continue;
+	}
+	return 1;
+}
+
+static int picolcd_check_version(struct hid_device *hdev)
+{
+	struct picolcd_data *data = hid_get_drvdata(hdev);
+	struct picolcd_pending *verinfo;
+	int ret = 0;
+
+	if (!data)
+		return -ENODEV;
+
+	verinfo = picolcd_send_and_wait(hdev, REPORT_VERSION, NULL, 0);
+	if (!verinfo) {
+		hid_err(hdev, "no version response from PicoLCD\n");
+		return -ENODEV;
+	}
+
+	if (verinfo->raw_size == 2) {
+		data->version[0] = verinfo->raw_data[1];
+		data->version[1] = verinfo->raw_data[0];
+		if (data->status & PICOLCD_BOOTLOADER) {
+			hid_info(hdev, "PicoLCD, bootloader version %d.%d\n",
+				 verinfo->raw_data[1], verinfo->raw_data[0]);
+		} else {
+			hid_info(hdev, "PicoLCD, firmware version %d.%d\n",
+				 verinfo->raw_data[1], verinfo->raw_data[0]);
+		}
+	} else {
+		hid_err(hdev, "confused, got unexpected version response from PicoLCD\n");
+		ret = -EINVAL;
+	}
+	kfree(verinfo);
+	return ret;
+}
+
+/*
+ * Reset our device and wait for answer to VERSION request
+ */
+int picolcd_reset(struct hid_device *hdev)
+{
+	struct picolcd_data *data = hid_get_drvdata(hdev);
+	struct hid_report *report = picolcd_out_report(REPORT_RESET, hdev);
+	unsigned long flags;
+	int error;
+
+	if (!data || !report || report->maxfield != 1)
+		return -ENODEV;
+
+	spin_lock_irqsave(&data->lock, flags);
+	if (hdev->product == USB_DEVICE_ID_PICOLCD_BOOTLOADER)
+		data->status |= PICOLCD_BOOTLOADER;
+
+	/* perform the reset */
+	hid_set_field(report->field[0], 0, 1);
+	if (data->status & PICOLCD_FAILED) {
+		spin_unlock_irqrestore(&data->lock, flags);
+		return -ENODEV;
+	}
+	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	error = picolcd_check_version(hdev);
+	if (error)
+		return error;
+
+	picolcd_resume_lcd(data);
+	picolcd_resume_backlight(data);
+	picolcd_fb_refresh(data);
+	picolcd_leds_set(data);
+	return 0;
+}
+
+/*
+ * The "operation_mode" sysfs attribute
+ */
+static ssize_t picolcd_operation_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+
+	if (data->status & PICOLCD_BOOTLOADER)
+		return snprintf(buf, PAGE_SIZE, "[bootloader] lcd\n");
+	else
+		return snprintf(buf, PAGE_SIZE, "bootloader [lcd]\n");
+}
+
+static ssize_t picolcd_operation_mode_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+	struct hid_report *report = NULL;
+	size_t cnt = count;
+	int timeout = data->opmode_delay;
+	unsigned long flags;
+
+	if (cnt >= 3 && strncmp("lcd", buf, 3) == 0) {
+		if (data->status & PICOLCD_BOOTLOADER)
+			report = picolcd_out_report(REPORT_EXIT_FLASHER, data->hdev);
+		buf += 3;
+		cnt -= 3;
+	} else if (cnt >= 10 && strncmp("bootloader", buf, 10) == 0) {
+		if (!(data->status & PICOLCD_BOOTLOADER))
+			report = picolcd_out_report(REPORT_EXIT_KEYBOARD, data->hdev);
+		buf += 10;
+		cnt -= 10;
+	}
+	if (!report)
+		return -EINVAL;
+
+	while (cnt > 0 && (buf[cnt-1] == '\n' || buf[cnt-1] == '\r'))
+		cnt--;
+	if (cnt != 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&data->lock, flags);
+	hid_set_field(report->field[0], 0, timeout & 0xff);
+	hid_set_field(report->field[0], 1, (timeout >> 8) & 0xff);
+	usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+	return count;
+}
+
+static DEVICE_ATTR(operation_mode, 0644, picolcd_operation_mode_show,
+		picolcd_operation_mode_store);
+
+/*
+ * The "operation_mode_delay" sysfs attribute
+ */
+static ssize_t picolcd_operation_mode_delay_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%hu\n", data->opmode_delay);
+}
+
+static ssize_t picolcd_operation_mode_delay_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+	unsigned u;
+	if (sscanf(buf, "%u", &u) != 1)
+		return -EINVAL;
+	if (u > 30000)
+		return -EINVAL;
+	else
+		data->opmode_delay = u;
+	return count;
+}
+
+static DEVICE_ATTR(operation_mode_delay, 0644, picolcd_operation_mode_delay_show,
+		picolcd_operation_mode_delay_store);
+
+/*
+ * Handle raw report as sent by device
+ */
+static int picolcd_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	struct picolcd_data *data = hid_get_drvdata(hdev);
+	unsigned long flags;
+	int ret = 0;
+
+	if (!data)
+		return 1;
+
+	if (report->id == REPORT_KEY_STATE) {
+		if (data->input_keys)
+			ret = picolcd_raw_keypad(data, report, raw_data+1, size-1);
+	} else if (report->id == REPORT_IR_DATA) {
+		ret = picolcd_raw_cir(data, report, raw_data+1, size-1);
+	} else {
+		spin_lock_irqsave(&data->lock, flags);
+		/*
+		 * We let the caller of picolcd_send_and_wait() check if the
+		 * report we got is one of the expected ones or not.
+		 */
+		if (data->pending) {
+			memcpy(data->pending->raw_data, raw_data+1, size-1);
+			data->pending->raw_size  = size-1;
+			data->pending->in_report = report;
+			complete(&data->pending->ready);
+		}
+		spin_unlock_irqrestore(&data->lock, flags);
+	}
+
+	picolcd_debug_raw_event(data, hdev, report, raw_data, size);
+	return 1;
+}
+
+#ifdef CONFIG_PM
+static int picolcd_suspend(struct hid_device *hdev, pm_message_t message)
+{
+	if (PMSG_IS_AUTO(message))
+		return 0;
+
+	picolcd_suspend_backlight(hid_get_drvdata(hdev));
+	dbg_hid(PICOLCD_NAME " device ready for suspend\n");
+	return 0;
+}
+
+static int picolcd_resume(struct hid_device *hdev)
+{
+	int ret;
+	ret = picolcd_resume_backlight(hid_get_drvdata(hdev));
+	if (ret)
+		dbg_hid(PICOLCD_NAME " restoring backlight failed: %d\n", ret);
+	return 0;
+}
+
+static int picolcd_reset_resume(struct hid_device *hdev)
+{
+	int ret;
+	ret = picolcd_reset(hdev);
+	if (ret)
+		dbg_hid(PICOLCD_NAME " resetting our device failed: %d\n", ret);
+	ret = picolcd_fb_reset(hid_get_drvdata(hdev), 0);
+	if (ret)
+		dbg_hid(PICOLCD_NAME " restoring framebuffer content failed: %d\n", ret);
+	ret = picolcd_resume_lcd(hid_get_drvdata(hdev));
+	if (ret)
+		dbg_hid(PICOLCD_NAME " restoring lcd failed: %d\n", ret);
+	ret = picolcd_resume_backlight(hid_get_drvdata(hdev));
+	if (ret)
+		dbg_hid(PICOLCD_NAME " restoring backlight failed: %d\n", ret);
+	picolcd_leds_set(hid_get_drvdata(hdev));
+	return 0;
+}
+#endif
+
+/* initialize keypad input device */
+static int picolcd_init_keys(struct picolcd_data *data,
+		struct hid_report *report)
+{
+	struct hid_device *hdev = data->hdev;
+	struct input_dev *idev;
+	int error, i;
+
+	if (!report)
+		return -ENODEV;
+	if (report->maxfield != 1 || report->field[0]->report_count != 2 ||
+			report->field[0]->report_size != 8) {
+		hid_err(hdev, "unsupported KEY_STATE report\n");
+		return -EINVAL;
+	}
+
+	idev = input_allocate_device();
+	if (idev == NULL) {
+		hid_err(hdev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+	input_set_drvdata(idev, hdev);
+	memcpy(data->keycode, def_keymap, sizeof(def_keymap));
+	idev->name = hdev->name;
+	idev->phys = hdev->phys;
+	idev->uniq = hdev->uniq;
+	idev->id.bustype = hdev->bus;
+	idev->id.vendor  = hdev->vendor;
+	idev->id.product = hdev->product;
+	idev->id.version = hdev->version;
+	idev->dev.parent = &hdev->dev;
+	idev->keycode     = &data->keycode;
+	idev->keycodemax  = PICOLCD_KEYS;
+	idev->keycodesize = sizeof(data->keycode[0]);
+	input_set_capability(idev, EV_MSC, MSC_SCAN);
+	set_bit(EV_REP, idev->evbit);
+	for (i = 0; i < PICOLCD_KEYS; i++)
+		input_set_capability(idev, EV_KEY, data->keycode[i]);
+	error = input_register_device(idev);
+	if (error) {
+		hid_err(hdev, "error registering the input device\n");
+		input_free_device(idev);
+		return error;
+	}
+	data->input_keys = idev;
+	return 0;
+}
+
+static void picolcd_exit_keys(struct picolcd_data *data)
+{
+	struct input_dev *idev = data->input_keys;
+
+	data->input_keys = NULL;
+	if (idev)
+		input_unregister_device(idev);
+}
+
+static int picolcd_probe_lcd(struct hid_device *hdev, struct picolcd_data *data)
+{
+	int error;
+
+	/* Setup keypad input device */
+	error = picolcd_init_keys(data, picolcd_in_report(REPORT_KEY_STATE, hdev));
+	if (error)
+		goto err;
+
+	/* Setup CIR input device */
+	error = picolcd_init_cir(data, picolcd_in_report(REPORT_IR_DATA, hdev));
+	if (error)
+		goto err;
+
+	/* Set up the framebuffer device */
+	error = picolcd_init_framebuffer(data);
+	if (error)
+		goto err;
+
+	/* Setup lcd class device */
+	error = picolcd_init_lcd(data, picolcd_out_report(REPORT_CONTRAST, hdev));
+	if (error)
+		goto err;
+
+	/* Setup backlight class device */
+	error = picolcd_init_backlight(data, picolcd_out_report(REPORT_BRIGHTNESS, hdev));
+	if (error)
+		goto err;
+
+	/* Setup the LED class devices */
+	error = picolcd_init_leds(data, picolcd_out_report(REPORT_LED_STATE, hdev));
+	if (error)
+		goto err;
+
+	picolcd_init_devfs(data, picolcd_out_report(REPORT_EE_READ, hdev),
+			picolcd_out_report(REPORT_EE_WRITE, hdev),
+			picolcd_out_report(REPORT_READ_MEMORY, hdev),
+			picolcd_out_report(REPORT_WRITE_MEMORY, hdev),
+			picolcd_out_report(REPORT_RESET, hdev));
+	return 0;
+err:
+	picolcd_exit_leds(data);
+	picolcd_exit_backlight(data);
+	picolcd_exit_lcd(data);
+	picolcd_exit_framebuffer(data);
+	picolcd_exit_cir(data);
+	picolcd_exit_keys(data);
+	return error;
+}
+
+static int picolcd_probe_bootloader(struct hid_device *hdev, struct picolcd_data *data)
+{
+	picolcd_init_devfs(data, NULL, NULL,
+			picolcd_out_report(REPORT_BL_READ_MEMORY, hdev),
+			picolcd_out_report(REPORT_BL_WRITE_MEMORY, hdev), NULL);
+	return 0;
+}
+
+static int picolcd_probe(struct hid_device *hdev,
+		     const struct hid_device_id *id)
+{
+	struct picolcd_data *data;
+	int error = -ENOMEM;
+
+	dbg_hid(PICOLCD_NAME " hardware probe...\n");
+
+	/*
+	 * Let's allocate the picolcd data structure, set some reasonable
+	 * defaults, and associate it with the device
+	 */
+	data = kzalloc(sizeof(struct picolcd_data), GFP_KERNEL);
+	if (data == NULL) {
+		hid_err(hdev, "can't allocate space for Minibox PicoLCD device data\n");
+		error = -ENOMEM;
+		goto err_no_cleanup;
+	}
+
+	spin_lock_init(&data->lock);
+	mutex_init(&data->mutex);
+	data->hdev = hdev;
+	data->opmode_delay = 5000;
+	if (hdev->product == USB_DEVICE_ID_PICOLCD_BOOTLOADER)
+		data->status |= PICOLCD_BOOTLOADER;
+	hid_set_drvdata(hdev, data);
+
+	/* Parse the device reports and start it up */
+	error = hid_parse(hdev);
+	if (error) {
+		hid_err(hdev, "device report parse failed\n");
+		goto err_cleanup_data;
+	}
+
+	error = hid_hw_start(hdev, 0);
+	if (error) {
+		hid_err(hdev, "hardware start failed\n");
+		goto err_cleanup_data;
+	}
+
+	error = hid_hw_open(hdev);
+	if (error) {
+		hid_err(hdev, "failed to open input interrupt pipe for key and IR events\n");
+		goto err_cleanup_hid_hw;
+	}
+
+	error = device_create_file(&hdev->dev, &dev_attr_operation_mode_delay);
+	if (error) {
+		hid_err(hdev, "failed to create sysfs attributes\n");
+		goto err_cleanup_hid_ll;
+	}
+
+	error = device_create_file(&hdev->dev, &dev_attr_operation_mode);
+	if (error) {
+		hid_err(hdev, "failed to create sysfs attributes\n");
+		goto err_cleanup_sysfs1;
+	}
+
+	if (data->status & PICOLCD_BOOTLOADER)
+		error = picolcd_probe_bootloader(hdev, data);
+	else
+		error = picolcd_probe_lcd(hdev, data);
+	if (error)
+		goto err_cleanup_sysfs2;
+
+	dbg_hid(PICOLCD_NAME " activated and initialized\n");
+	return 0;
+
+err_cleanup_sysfs2:
+	device_remove_file(&hdev->dev, &dev_attr_operation_mode);
+err_cleanup_sysfs1:
+	device_remove_file(&hdev->dev, &dev_attr_operation_mode_delay);
+err_cleanup_hid_ll:
+	hid_hw_close(hdev);
+err_cleanup_hid_hw:
+	hid_hw_stop(hdev);
+err_cleanup_data:
+	kfree(data);
+err_no_cleanup:
+	hid_set_drvdata(hdev, NULL);
+
+	return error;
+}
+
+static void picolcd_remove(struct hid_device *hdev)
+{
+	struct picolcd_data *data = hid_get_drvdata(hdev);
+	unsigned long flags;
+
+	dbg_hid(PICOLCD_NAME " hardware remove...\n");
+	spin_lock_irqsave(&data->lock, flags);
+	data->status |= PICOLCD_FAILED;
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	picolcd_exit_devfs(data);
+	device_remove_file(&hdev->dev, &dev_attr_operation_mode);
+	device_remove_file(&hdev->dev, &dev_attr_operation_mode_delay);
+	hid_hw_close(hdev);
+	hid_hw_stop(hdev);
+
+	/* Shortcut potential pending reply that will never arrive */
+	spin_lock_irqsave(&data->lock, flags);
+	if (data->pending)
+		complete(&data->pending->ready);
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	/* Cleanup LED */
+	picolcd_exit_leds(data);
+	/* Clean up the framebuffer */
+	picolcd_exit_backlight(data);
+	picolcd_exit_lcd(data);
+	picolcd_exit_framebuffer(data);
+	/* Cleanup input */
+	picolcd_exit_cir(data);
+	picolcd_exit_keys(data);
+
+	hid_set_drvdata(hdev, NULL);
+	mutex_destroy(&data->mutex);
+	/* Finally, clean up the picolcd data itself */
+	kfree(data);
+}
+
+static const struct hid_device_id picolcd_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD_BOOTLOADER) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, picolcd_devices);
+
+static struct hid_driver picolcd_driver = {
+	.name =          "hid-picolcd",
+	.id_table =      picolcd_devices,
+	.probe =         picolcd_probe,
+	.remove =        picolcd_remove,
+	.raw_event =     picolcd_raw_event,
+#ifdef CONFIG_PM
+	.suspend =       picolcd_suspend,
+	.resume =        picolcd_resume,
+	.reset_resume =  picolcd_reset_resume,
+#endif
+};
+
+static int __init picolcd_init(void)
+{
+	return hid_register_driver(&picolcd_driver);
+}
+
+static void __exit picolcd_exit(void)
+{
+	hid_unregister_driver(&picolcd_driver);
+}
+
+module_init(picolcd_init);
+module_exit(picolcd_exit);
+MODULE_DESCRIPTION("Minibox graphics PicoLCD Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-picolcd_debugfs.c b/drivers/hid/hid-picolcd_debugfs.c
new file mode 100644
index 000000000000..4809aa1bdb9c
--- /dev/null
+++ b/drivers/hid/hid-picolcd_debugfs.c
@@ -0,0 +1,899 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include <linux/hid-debug.h>
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+
+#include "hid-picolcd.h"
+
+
+static int picolcd_debug_reset_show(struct seq_file *f, void *p)
+{
+	if (picolcd_fbinfo((struct picolcd_data *)f->private))
+		seq_printf(f, "all fb\n");
+	else
+		seq_printf(f, "all\n");
+	return 0;
+}
+
+static int picolcd_debug_reset_open(struct inode *inode, struct file *f)
+{
+	return single_open(f, picolcd_debug_reset_show, inode->i_private);
+}
+
+static ssize_t picolcd_debug_reset_write(struct file *f, const char __user *user_buf,
+		size_t count, loff_t *ppos)
+{
+	struct picolcd_data *data = ((struct seq_file *)f->private_data)->private;
+	char buf[32];
+	size_t cnt = min(count, sizeof(buf)-1);
+	if (copy_from_user(buf, user_buf, cnt))
+		return -EFAULT;
+
+	while (cnt > 0 && (buf[cnt-1] == ' ' || buf[cnt-1] == '\n'))
+		cnt--;
+	buf[cnt] = '\0';
+	if (strcmp(buf, "all") == 0) {
+		picolcd_reset(data->hdev);
+		picolcd_fb_reset(data, 1);
+	} else if (strcmp(buf, "fb") == 0) {
+		picolcd_fb_reset(data, 1);
+	} else {
+		return -EINVAL;
+	}
+	return count;
+}
+
+static const struct file_operations picolcd_debug_reset_fops = {
+	.owner    = THIS_MODULE,
+	.open     = picolcd_debug_reset_open,
+	.read     = seq_read,
+	.llseek   = seq_lseek,
+	.write    = picolcd_debug_reset_write,
+	.release  = single_release,
+};
+
+/*
+ * The "eeprom" file
+ */
+static ssize_t picolcd_debug_eeprom_read(struct file *f, char __user *u,
+		size_t s, loff_t *off)
+{
+	struct picolcd_data *data = f->private_data;
+	struct picolcd_pending *resp;
+	u8 raw_data[3];
+	ssize_t ret = -EIO;
+
+	if (s == 0)
+		return -EINVAL;
+	if (*off > 0x0ff)
+		return 0;
+
+	/* prepare buffer with info about what we want to read (addr & len) */
+	raw_data[0] = *off & 0xff;
+	raw_data[1] = (*off >> 8) & 0xff;
+	raw_data[2] = s < 20 ? s : 20;
+	if (*off + raw_data[2] > 0xff)
+		raw_data[2] = 0x100 - *off;
+	resp = picolcd_send_and_wait(data->hdev, REPORT_EE_READ, raw_data,
+			sizeof(raw_data));
+	if (!resp)
+		return -EIO;
+
+	if (resp->in_report && resp->in_report->id == REPORT_EE_DATA) {
+		/* successful read :) */
+		ret = resp->raw_data[2];
+		if (ret > s)
+			ret = s;
+		if (copy_to_user(u, resp->raw_data+3, ret))
+			ret = -EFAULT;
+		else
+			*off += ret;
+	} /* anything else is some kind of IO error */
+
+	kfree(resp);
+	return ret;
+}
+
+static ssize_t picolcd_debug_eeprom_write(struct file *f, const char __user *u,
+		size_t s, loff_t *off)
+{
+	struct picolcd_data *data = f->private_data;
+	struct picolcd_pending *resp;
+	ssize_t ret = -EIO;
+	u8 raw_data[23];
+
+	if (s == 0)
+		return -EINVAL;
+	if (*off > 0x0ff)
+		return -ENOSPC;
+
+	memset(raw_data, 0, sizeof(raw_data));
+	raw_data[0] = *off & 0xff;
+	raw_data[1] = (*off >> 8) & 0xff;
+	raw_data[2] = min_t(size_t, 20, s);
+	if (*off + raw_data[2] > 0xff)
+		raw_data[2] = 0x100 - *off;
+
+	if (copy_from_user(raw_data+3, u, min((u8)20, raw_data[2])))
+		return -EFAULT;
+	resp = picolcd_send_and_wait(data->hdev, REPORT_EE_WRITE, raw_data,
+			sizeof(raw_data));
+
+	if (!resp)
+		return -EIO;
+
+	if (resp->in_report && resp->in_report->id == REPORT_EE_DATA) {
+		/* check if written data matches */
+		if (memcmp(raw_data, resp->raw_data, 3+raw_data[2]) == 0) {
+			*off += raw_data[2];
+			ret = raw_data[2];
+		}
+	}
+	kfree(resp);
+	return ret;
+}
+
+/*
+ * Notes:
+ * - read/write happens in chunks of at most 20 bytes, it's up to userspace
+ *   to loop in order to get more data.
+ * - on write errors on otherwise correct write request the bytes
+ *   that should have been written are in undefined state.
+ */
+static const struct file_operations picolcd_debug_eeprom_fops = {
+	.owner    = THIS_MODULE,
+	.open     = simple_open,
+	.read     = picolcd_debug_eeprom_read,
+	.write    = picolcd_debug_eeprom_write,
+	.llseek   = generic_file_llseek,
+};
+
+/*
+ * The "flash" file
+ */
+/* record a flash address to buf (bounds check to be done by caller) */
+static int _picolcd_flash_setaddr(struct picolcd_data *data, u8 *buf, long off)
+{
+	buf[0] = off & 0xff;
+	buf[1] = (off >> 8) & 0xff;
+	if (data->addr_sz == 3)
+		buf[2] = (off >> 16) & 0xff;
+	return data->addr_sz == 2 ? 2 : 3;
+}
+
+/* read a given size of data (bounds check to be done by caller) */
+static ssize_t _picolcd_flash_read(struct picolcd_data *data, int report_id,
+		char __user *u, size_t s, loff_t *off)
+{
+	struct picolcd_pending *resp;
+	u8 raw_data[4];
+	ssize_t ret = 0;
+	int len_off, err = -EIO;
+
+	while (s > 0) {
+		err = -EIO;
+		len_off = _picolcd_flash_setaddr(data, raw_data, *off);
+		raw_data[len_off] = s > 32 ? 32 : s;
+		resp = picolcd_send_and_wait(data->hdev, report_id, raw_data, len_off+1);
+		if (!resp || !resp->in_report)
+			goto skip;
+		if (resp->in_report->id == REPORT_MEMORY ||
+			resp->in_report->id == REPORT_BL_READ_MEMORY) {
+			if (memcmp(raw_data, resp->raw_data, len_off+1) != 0)
+				goto skip;
+			if (copy_to_user(u+ret, resp->raw_data+len_off+1, raw_data[len_off])) {
+				err = -EFAULT;
+				goto skip;
+			}
+			*off += raw_data[len_off];
+			s    -= raw_data[len_off];
+			ret  += raw_data[len_off];
+			err   = 0;
+		}
+skip:
+		kfree(resp);
+		if (err)
+			return ret > 0 ? ret : err;
+	}
+	return ret;
+}
+
+static ssize_t picolcd_debug_flash_read(struct file *f, char __user *u,
+		size_t s, loff_t *off)
+{
+	struct picolcd_data *data = f->private_data;
+
+	if (s == 0)
+		return -EINVAL;
+	if (*off > 0x05fff)
+		return 0;
+	if (*off + s > 0x05fff)
+		s = 0x06000 - *off;
+
+	if (data->status & PICOLCD_BOOTLOADER)
+		return _picolcd_flash_read(data, REPORT_BL_READ_MEMORY, u, s, off);
+	else
+		return _picolcd_flash_read(data, REPORT_READ_MEMORY, u, s, off);
+}
+
+/* erase block aligned to 64bytes boundary */
+static ssize_t _picolcd_flash_erase64(struct picolcd_data *data, int report_id,
+		loff_t *off)
+{
+	struct picolcd_pending *resp;
+	u8 raw_data[3];
+	int len_off;
+	ssize_t ret = -EIO;
+
+	if (*off & 0x3f)
+		return -EINVAL;
+
+	len_off = _picolcd_flash_setaddr(data, raw_data, *off);
+	resp = picolcd_send_and_wait(data->hdev, report_id, raw_data, len_off);
+	if (!resp || !resp->in_report)
+		goto skip;
+	if (resp->in_report->id == REPORT_MEMORY ||
+		resp->in_report->id == REPORT_BL_ERASE_MEMORY) {
+		if (memcmp(raw_data, resp->raw_data, len_off) != 0)
+			goto skip;
+		ret = 0;
+	}
+skip:
+	kfree(resp);
+	return ret;
+}
+
+/* write a given size of data (bounds check to be done by caller) */
+static ssize_t _picolcd_flash_write(struct picolcd_data *data, int report_id,
+		const char __user *u, size_t s, loff_t *off)
+{
+	struct picolcd_pending *resp;
+	u8 raw_data[36];
+	ssize_t ret = 0;
+	int len_off, err = -EIO;
+
+	while (s > 0) {
+		err = -EIO;
+		len_off = _picolcd_flash_setaddr(data, raw_data, *off);
+		raw_data[len_off] = s > 32 ? 32 : s;
+		if (copy_from_user(raw_data+len_off+1, u, raw_data[len_off])) {
+			err = -EFAULT;
+			break;
+		}
+		resp = picolcd_send_and_wait(data->hdev, report_id, raw_data,
+				len_off+1+raw_data[len_off]);
+		if (!resp || !resp->in_report)
+			goto skip;
+		if (resp->in_report->id == REPORT_MEMORY ||
+			resp->in_report->id == REPORT_BL_WRITE_MEMORY) {
+			if (memcmp(raw_data, resp->raw_data, len_off+1+raw_data[len_off]) != 0)
+				goto skip;
+			*off += raw_data[len_off];
+			s    -= raw_data[len_off];
+			ret  += raw_data[len_off];
+			err   = 0;
+		}
+skip:
+		kfree(resp);
+		if (err)
+			break;
+	}
+	return ret > 0 ? ret : err;
+}
+
+static ssize_t picolcd_debug_flash_write(struct file *f, const char __user *u,
+		size_t s, loff_t *off)
+{
+	struct picolcd_data *data = f->private_data;
+	ssize_t err, ret = 0;
+	int report_erase, report_write;
+
+	if (s == 0)
+		return -EINVAL;
+	if (*off > 0x5fff)
+		return -ENOSPC;
+	if (s & 0x3f)
+		return -EINVAL;
+	if (*off & 0x3f)
+		return -EINVAL;
+
+	if (data->status & PICOLCD_BOOTLOADER) {
+		report_erase = REPORT_BL_ERASE_MEMORY;
+		report_write = REPORT_BL_WRITE_MEMORY;
+	} else {
+		report_erase = REPORT_ERASE_MEMORY;
+		report_write = REPORT_WRITE_MEMORY;
+	}
+	mutex_lock(&data->mutex_flash);
+	while (s > 0) {
+		err = _picolcd_flash_erase64(data, report_erase, off);
+		if (err)
+			break;
+		err = _picolcd_flash_write(data, report_write, u, 64, off);
+		if (err < 0)
+			break;
+		ret += err;
+		*off += err;
+		s -= err;
+		if (err != 64)
+			break;
+	}
+	mutex_unlock(&data->mutex_flash);
+	return ret > 0 ? ret : err;
+}
+
+/*
+ * Notes:
+ * - concurrent writing is prevented by mutex and all writes must be
+ *   n*64 bytes and 64-byte aligned, each write being preceded by an
+ *   ERASE which erases a 64byte block.
+ *   If less than requested was written or an error is returned for an
+ *   otherwise correct write request the next 64-byte block which should
+ *   have been written is in undefined state (mostly: original, erased,
+ *   (half-)written with write error)
+ * - reading can happen without special restriction
+ */
+static const struct file_operations picolcd_debug_flash_fops = {
+	.owner    = THIS_MODULE,
+	.open     = simple_open,
+	.read     = picolcd_debug_flash_read,
+	.write    = picolcd_debug_flash_write,
+	.llseek   = generic_file_llseek,
+};
+
+
+/*
+ * Helper code for HID report level dumping/debugging
+ */
+static const char * const error_codes[] = {
+	"success", "parameter missing", "data_missing", "block readonly",
+	"block not erasable", "block too big", "section overflow",
+	"invalid command length", "invalid data length",
+};
+
+static void dump_buff_as_hex(char *dst, size_t dst_sz, const u8 *data,
+		const size_t data_len)
+{
+	int i, j;
+	for (i = j = 0; i < data_len && j + 4 < dst_sz; i++) {
+		dst[j++] = hex_asc[(data[i] >> 4) & 0x0f];
+		dst[j++] = hex_asc[data[i] & 0x0f];
+		dst[j++] = ' ';
+	}
+	dst[j]   = '\0';
+	if (j > 0)
+		dst[j-1] = '\n';
+	if (i < data_len && j > 2)
+		dst[j-2] = dst[j-3] = '.';
+}
+
+void picolcd_debug_out_report(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report)
+{
+	u8 raw_data[70];
+	int raw_size = (report->size >> 3) + 1;
+	char *buff;
+#define BUFF_SZ 256
+
+	/* Avoid unnecessary overhead if debugfs is disabled */
+	if (list_empty(&hdev->debug_list))
+		return;
+
+	buff = kmalloc(BUFF_SZ, GFP_ATOMIC);
+	if (!buff)
+		return;
+
+	snprintf(buff, BUFF_SZ, "\nout report %d (size %d) =  ",
+			report->id, raw_size);
+	hid_debug_event(hdev, buff);
+	if (raw_size + 5 > sizeof(raw_data)) {
+		kfree(buff);
+		hid_debug_event(hdev, " TOO BIG\n");
+		return;
+	} else {
+		raw_data[0] = report->id;
+		hid_output_report(report, raw_data);
+		dump_buff_as_hex(buff, BUFF_SZ, raw_data, raw_size);
+		hid_debug_event(hdev, buff);
+	}
+
+	switch (report->id) {
+	case REPORT_LED_STATE:
+		/* 1 data byte with GPO state */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_LED_STATE", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tGPO state: 0x%02x\n", raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_BRIGHTNESS:
+		/* 1 data byte with brightness */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_BRIGHTNESS", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tBrightness: 0x%02x\n", raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_CONTRAST:
+		/* 1 data byte with contrast */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_CONTRAST", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tContrast: 0x%02x\n", raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_RESET:
+		/* 2 data bytes with reset duration in ms */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_RESET", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tDuration: 0x%02x%02x (%dms)\n",
+				raw_data[2], raw_data[1], raw_data[2] << 8 | raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_LCD_CMD:
+		/* 63 data bytes with LCD commands */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_LCD_CMD", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		/* TODO: format decoding */
+		break;
+	case REPORT_LCD_DATA:
+		/* 63 data bytes with LCD data */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_LCD_CMD", report->id, raw_size-1);
+		/* TODO: format decoding */
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_LCD_CMD_DATA:
+		/* 63 data bytes with LCD commands and data */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_LCD_CMD", report->id, raw_size-1);
+		/* TODO: format decoding */
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_EE_READ:
+		/* 3 data bytes with read area description */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_EE_READ", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_EE_WRITE:
+		/* 3+1..20 data bytes with write area description */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_EE_WRITE", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+		hid_debug_event(hdev, buff);
+		if (raw_data[3] == 0) {
+			snprintf(buff, BUFF_SZ, "\tNo data\n");
+		} else if (raw_data[3] + 4 <= raw_size) {
+			snprintf(buff, BUFF_SZ, "\tData: ");
+			hid_debug_event(hdev, buff);
+			dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
+		} else {
+			snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+		}
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_ERASE_MEMORY:
+	case REPORT_BL_ERASE_MEMORY:
+		/* 3 data bytes with pointer inside erase block */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_ERASE_MEMORY", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		switch (data->addr_sz) {
+		case 2:
+			snprintf(buff, BUFF_SZ, "\tAddress inside 64 byte block: 0x%02x%02x\n",
+					raw_data[2], raw_data[1]);
+			break;
+		case 3:
+			snprintf(buff, BUFF_SZ, "\tAddress inside 64 byte block: 0x%02x%02x%02x\n",
+					raw_data[3], raw_data[2], raw_data[1]);
+			break;
+		default:
+			snprintf(buff, BUFF_SZ, "\tNot supported\n");
+		}
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_READ_MEMORY:
+	case REPORT_BL_READ_MEMORY:
+		/* 4 data bytes with read area description */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_READ_MEMORY", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		switch (data->addr_sz) {
+		case 2:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+					raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+			break;
+		case 3:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
+					raw_data[3], raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
+			break;
+		default:
+			snprintf(buff, BUFF_SZ, "\tNot supported\n");
+		}
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_WRITE_MEMORY:
+	case REPORT_BL_WRITE_MEMORY:
+		/* 4+1..32 data bytes with write adrea description */
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_WRITE_MEMORY", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		switch (data->addr_sz) {
+		case 2:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+					raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+			hid_debug_event(hdev, buff);
+			if (raw_data[3] == 0) {
+				snprintf(buff, BUFF_SZ, "\tNo data\n");
+			} else if (raw_data[3] + 4 <= raw_size) {
+				snprintf(buff, BUFF_SZ, "\tData: ");
+				hid_debug_event(hdev, buff);
+				dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
+			} else {
+				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+			}
+			break;
+		case 3:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
+					raw_data[3], raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
+			hid_debug_event(hdev, buff);
+			if (raw_data[4] == 0) {
+				snprintf(buff, BUFF_SZ, "\tNo data\n");
+			} else if (raw_data[4] + 5 <= raw_size) {
+				snprintf(buff, BUFF_SZ, "\tData: ");
+				hid_debug_event(hdev, buff);
+				dump_buff_as_hex(buff, BUFF_SZ, raw_data+5, raw_data[4]);
+			} else {
+				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+			}
+			break;
+		default:
+			snprintf(buff, BUFF_SZ, "\tNot supported\n");
+		}
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_SPLASH_RESTART:
+		/* TODO */
+		break;
+	case REPORT_EXIT_KEYBOARD:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_EXIT_KEYBOARD", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tRestart delay: %dms (0x%02x%02x)\n",
+				raw_data[1] | (raw_data[2] << 8),
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_VERSION:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_VERSION", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_DEVID:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_DEVID", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_SPLASH_SIZE:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_SPLASH_SIZE", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_HOOK_VERSION:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_HOOK_VERSION", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_EXIT_FLASHER:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"REPORT_VERSION", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tRestart delay: %dms (0x%02x%02x)\n",
+				raw_data[1] | (raw_data[2] << 8),
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	default:
+		snprintf(buff, BUFF_SZ, "out report %s (%d, size=%d)\n",
+			"<unknown>", report->id, raw_size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	}
+	wake_up_interruptible(&hdev->debug_wait);
+	kfree(buff);
+}
+
+void picolcd_debug_raw_event(struct picolcd_data *data,
+		struct hid_device *hdev, struct hid_report *report,
+		u8 *raw_data, int size)
+{
+	char *buff;
+
+#define BUFF_SZ 256
+	/* Avoid unnecessary overhead if debugfs is disabled */
+	if (list_empty(&hdev->debug_list))
+		return;
+
+	buff = kmalloc(BUFF_SZ, GFP_ATOMIC);
+	if (!buff)
+		return;
+
+	switch (report->id) {
+	case REPORT_ERROR_CODE:
+		/* 2 data bytes with affected report and error code */
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_ERROR_CODE", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		if (raw_data[2] < ARRAY_SIZE(error_codes))
+			snprintf(buff, BUFF_SZ, "\tError code 0x%02x (%s) in reply to report 0x%02x\n",
+					raw_data[2], error_codes[raw_data[2]], raw_data[1]);
+		else
+			snprintf(buff, BUFF_SZ, "\tError code 0x%02x in reply to report 0x%02x\n",
+					raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_KEY_STATE:
+		/* 2 data bytes with key state */
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_KEY_STATE", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		if (raw_data[1] == 0)
+			snprintf(buff, BUFF_SZ, "\tNo key pressed\n");
+		else if (raw_data[2] == 0)
+			snprintf(buff, BUFF_SZ, "\tOne key pressed: 0x%02x (%d)\n",
+					raw_data[1], raw_data[1]);
+		else
+			snprintf(buff, BUFF_SZ, "\tTwo keys pressed: 0x%02x (%d), 0x%02x (%d)\n",
+					raw_data[1], raw_data[1], raw_data[2], raw_data[2]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_IR_DATA:
+		/* Up to 20 byes of IR scancode data */
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_IR_DATA", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		if (raw_data[1] == 0) {
+			snprintf(buff, BUFF_SZ, "\tUnexpectedly 0 data length\n");
+			hid_debug_event(hdev, buff);
+		} else if (raw_data[1] + 1 <= size) {
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n\tIR Data: ",
+					raw_data[1]);
+			hid_debug_event(hdev, buff);
+			dump_buff_as_hex(buff, BUFF_SZ, raw_data+2, raw_data[1]);
+			hid_debug_event(hdev, buff);
+		} else {
+			snprintf(buff, BUFF_SZ, "\tOverflowing data length: %d\n",
+					raw_data[1]-1);
+			hid_debug_event(hdev, buff);
+		}
+		break;
+	case REPORT_EE_DATA:
+		/* Data buffer in response to REPORT_EE_READ or REPORT_EE_WRITE */
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_EE_DATA", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+		hid_debug_event(hdev, buff);
+		if (raw_data[3] == 0) {
+			snprintf(buff, BUFF_SZ, "\tNo data\n");
+			hid_debug_event(hdev, buff);
+		} else if (raw_data[3] + 4 <= size) {
+			snprintf(buff, BUFF_SZ, "\tData: ");
+			hid_debug_event(hdev, buff);
+			dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
+			hid_debug_event(hdev, buff);
+		} else {
+			snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+			hid_debug_event(hdev, buff);
+		}
+		break;
+	case REPORT_MEMORY:
+		/* Data buffer in response to REPORT_READ_MEMORY or REPORT_WRTIE_MEMORY */
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_MEMORY", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		switch (data->addr_sz) {
+		case 2:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x\n",
+					raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[3]);
+			hid_debug_event(hdev, buff);
+			if (raw_data[3] == 0) {
+				snprintf(buff, BUFF_SZ, "\tNo data\n");
+			} else if (raw_data[3] + 4 <= size) {
+				snprintf(buff, BUFF_SZ, "\tData: ");
+				hid_debug_event(hdev, buff);
+				dump_buff_as_hex(buff, BUFF_SZ, raw_data+4, raw_data[3]);
+			} else {
+				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+			}
+			break;
+		case 3:
+			snprintf(buff, BUFF_SZ, "\tData address: 0x%02x%02x%02x\n",
+					raw_data[3], raw_data[2], raw_data[1]);
+			hid_debug_event(hdev, buff);
+			snprintf(buff, BUFF_SZ, "\tData length: %d\n", raw_data[4]);
+			hid_debug_event(hdev, buff);
+			if (raw_data[4] == 0) {
+				snprintf(buff, BUFF_SZ, "\tNo data\n");
+			} else if (raw_data[4] + 5 <= size) {
+				snprintf(buff, BUFF_SZ, "\tData: ");
+				hid_debug_event(hdev, buff);
+				dump_buff_as_hex(buff, BUFF_SZ, raw_data+5, raw_data[4]);
+			} else {
+				snprintf(buff, BUFF_SZ, "\tData overflowed\n");
+			}
+			break;
+		default:
+			snprintf(buff, BUFF_SZ, "\tNot supported\n");
+		}
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_VERSION:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_VERSION", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tFirmware version: %d.%d\n",
+				raw_data[2], raw_data[1]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_BL_ERASE_MEMORY:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_BL_ERASE_MEMORY", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		/* TODO */
+		break;
+	case REPORT_BL_READ_MEMORY:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_BL_READ_MEMORY", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		/* TODO */
+		break;
+	case REPORT_BL_WRITE_MEMORY:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_BL_WRITE_MEMORY", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		/* TODO */
+		break;
+	case REPORT_DEVID:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_DEVID", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tSerial: 0x%02x%02x%02x%02x\n",
+				raw_data[1], raw_data[2], raw_data[3], raw_data[4]);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tType: 0x%02x\n",
+				raw_data[5]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_SPLASH_SIZE:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_SPLASH_SIZE", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tTotal splash space: %d\n",
+				(raw_data[2] << 8) | raw_data[1]);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tUsed splash space: %d\n",
+				(raw_data[4] << 8) | raw_data[3]);
+		hid_debug_event(hdev, buff);
+		break;
+	case REPORT_HOOK_VERSION:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"REPORT_HOOK_VERSION", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		snprintf(buff, BUFF_SZ, "\tFirmware version: %d.%d\n",
+				raw_data[1], raw_data[2]);
+		hid_debug_event(hdev, buff);
+		break;
+	default:
+		snprintf(buff, BUFF_SZ, "report %s (%d, size=%d)\n",
+			"<unknown>", report->id, size-1);
+		hid_debug_event(hdev, buff);
+		break;
+	}
+	wake_up_interruptible(&hdev->debug_wait);
+	kfree(buff);
+}
+
+void picolcd_init_devfs(struct picolcd_data *data,
+		struct hid_report *eeprom_r, struct hid_report *eeprom_w,
+		struct hid_report *flash_r, struct hid_report *flash_w,
+		struct hid_report *reset)
+{
+	struct hid_device *hdev = data->hdev;
+
+	mutex_init(&data->mutex_flash);
+
+	/* reset */
+	if (reset)
+		data->debug_reset = debugfs_create_file("reset", 0600,
+				hdev->debug_dir, data, &picolcd_debug_reset_fops);
+
+	/* eeprom */
+	if (eeprom_r || eeprom_w)
+		data->debug_eeprom = debugfs_create_file("eeprom",
+			(eeprom_w ? S_IWUSR : 0) | (eeprom_r ? S_IRUSR : 0),
+			hdev->debug_dir, data, &picolcd_debug_eeprom_fops);
+
+	/* flash */
+	if (flash_r && flash_r->maxfield == 1 && flash_r->field[0]->report_size == 8)
+		data->addr_sz = flash_r->field[0]->report_count - 1;
+	else
+		data->addr_sz = -1;
+	if (data->addr_sz == 2 || data->addr_sz == 3) {
+		data->debug_flash = debugfs_create_file("flash",
+			(flash_w ? S_IWUSR : 0) | (flash_r ? S_IRUSR : 0),
+			hdev->debug_dir, data, &picolcd_debug_flash_fops);
+	} else if (flash_r || flash_w)
+		hid_warn(hdev, "Unexpected FLASH access reports, please submit rdesc for review\n");
+}
+
+void picolcd_exit_devfs(struct picolcd_data *data)
+{
+	struct dentry *dent;
+
+	dent = data->debug_reset;
+	data->debug_reset = NULL;
+	if (dent)
+		debugfs_remove(dent);
+	dent = data->debug_eeprom;
+	data->debug_eeprom = NULL;
+	if (dent)
+		debugfs_remove(dent);
+	dent = data->debug_flash;
+	data->debug_flash = NULL;
+	if (dent)
+		debugfs_remove(dent);
+	mutex_destroy(&data->mutex_flash);
+}
+
diff --git a/drivers/hid/hid-picolcd_fb.c b/drivers/hid/hid-picolcd_fb.c
new file mode 100644
index 000000000000..0008a512211d
--- /dev/null
+++ b/drivers/hid/hid-picolcd_fb.c
@@ -0,0 +1,615 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include <linux/vmalloc.h>
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/module.h>
+
+#include "hid-picolcd.h"
+
+/* Framebuffer
+ *
+ * The PicoLCD use a Topway LCD module of 256x64 pixel
+ * This display area is tiled over 4 controllers with 8 tiles
+ * each. Each tile has 8x64 pixel, each data byte representing
+ * a 1-bit wide vertical line of the tile.
+ *
+ * The display can be updated at a tile granularity.
+ *
+ *       Chip 1           Chip 2           Chip 3           Chip 4
+ * +----------------+----------------+----------------+----------------+
+ * |     Tile 1     |     Tile 1     |     Tile 1     |     Tile 1     |
+ * +----------------+----------------+----------------+----------------+
+ * |     Tile 2     |     Tile 2     |     Tile 2     |     Tile 2     |
+ * +----------------+----------------+----------------+----------------+
+ *                                  ...
+ * +----------------+----------------+----------------+----------------+
+ * |     Tile 8     |     Tile 8     |     Tile 8     |     Tile 8     |
+ * +----------------+----------------+----------------+----------------+
+ */
+#define PICOLCDFB_NAME "picolcdfb"
+#define PICOLCDFB_WIDTH (256)
+#define PICOLCDFB_HEIGHT (64)
+#define PICOLCDFB_SIZE (PICOLCDFB_WIDTH * PICOLCDFB_HEIGHT / 8)
+
+#define PICOLCDFB_UPDATE_RATE_LIMIT   10
+#define PICOLCDFB_UPDATE_RATE_DEFAULT  2
+
+/* Framebuffer visual structures */
+static const struct fb_fix_screeninfo picolcdfb_fix = {
+	.id          = PICOLCDFB_NAME,
+	.type        = FB_TYPE_PACKED_PIXELS,
+	.visual      = FB_VISUAL_MONO01,
+	.xpanstep    = 0,
+	.ypanstep    = 0,
+	.ywrapstep   = 0,
+	.line_length = PICOLCDFB_WIDTH / 8,
+	.accel       = FB_ACCEL_NONE,
+};
+
+static const struct fb_var_screeninfo picolcdfb_var = {
+	.xres           = PICOLCDFB_WIDTH,
+	.yres           = PICOLCDFB_HEIGHT,
+	.xres_virtual   = PICOLCDFB_WIDTH,
+	.yres_virtual   = PICOLCDFB_HEIGHT,
+	.width          = 103,
+	.height         = 26,
+	.bits_per_pixel = 1,
+	.grayscale      = 1,
+	.red            = {
+		.offset = 0,
+		.length = 1,
+		.msb_right = 0,
+	},
+	.green          = {
+		.offset = 0,
+		.length = 1,
+		.msb_right = 0,
+	},
+	.blue           = {
+		.offset = 0,
+		.length = 1,
+		.msb_right = 0,
+	},
+	.transp         = {
+		.offset = 0,
+		.length = 0,
+		.msb_right = 0,
+	},
+};
+
+/* Send a given tile to PicoLCD */
+static int picolcd_fb_send_tile(struct picolcd_data *data, u8 *vbitmap,
+		int chip, int tile)
+{
+	struct hid_report *report1, *report2;
+	unsigned long flags;
+	u8 *tdata;
+	int i;
+
+	report1 = picolcd_out_report(REPORT_LCD_CMD_DATA, data->hdev);
+	if (!report1 || report1->maxfield != 1)
+		return -ENODEV;
+	report2 = picolcd_out_report(REPORT_LCD_DATA, data->hdev);
+	if (!report2 || report2->maxfield != 1)
+		return -ENODEV;
+
+	spin_lock_irqsave(&data->lock, flags);
+	if ((data->status & PICOLCD_FAILED)) {
+		spin_unlock_irqrestore(&data->lock, flags);
+		return -ENODEV;
+	}
+	hid_set_field(report1->field[0],  0, chip << 2);
+	hid_set_field(report1->field[0],  1, 0x02);
+	hid_set_field(report1->field[0],  2, 0x00);
+	hid_set_field(report1->field[0],  3, 0x00);
+	hid_set_field(report1->field[0],  4, 0xb8 | tile);
+	hid_set_field(report1->field[0],  5, 0x00);
+	hid_set_field(report1->field[0],  6, 0x00);
+	hid_set_field(report1->field[0],  7, 0x40);
+	hid_set_field(report1->field[0],  8, 0x00);
+	hid_set_field(report1->field[0],  9, 0x00);
+	hid_set_field(report1->field[0], 10,   32);
+
+	hid_set_field(report2->field[0],  0, (chip << 2) | 0x01);
+	hid_set_field(report2->field[0],  1, 0x00);
+	hid_set_field(report2->field[0],  2, 0x00);
+	hid_set_field(report2->field[0],  3,   32);
+
+	tdata = vbitmap + (tile * 4 + chip) * 64;
+	for (i = 0; i < 64; i++)
+		if (i < 32)
+			hid_set_field(report1->field[0], 11 + i, tdata[i]);
+		else
+			hid_set_field(report2->field[0], 4 + i - 32, tdata[i]);
+
+	usbhid_submit_report(data->hdev, report1, USB_DIR_OUT);
+	usbhid_submit_report(data->hdev, report2, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+	return 0;
+}
+
+/* Translate a single tile*/
+static int picolcd_fb_update_tile(u8 *vbitmap, const u8 *bitmap, int bpp,
+		int chip, int tile)
+{
+	int i, b, changed = 0;
+	u8 tdata[64];
+	u8 *vdata = vbitmap + (tile * 4 + chip) * 64;
+
+	if (bpp == 1) {
+		for (b = 7; b >= 0; b--) {
+			const u8 *bdata = bitmap + tile * 256 + chip * 8 + b * 32;
+			for (i = 0; i < 64; i++) {
+				tdata[i] <<= 1;
+				tdata[i] |= (bdata[i/8] >> (i % 8)) & 0x01;
+			}
+		}
+	} else if (bpp == 8) {
+		for (b = 7; b >= 0; b--) {
+			const u8 *bdata = bitmap + (tile * 256 + chip * 8 + b * 32) * 8;
+			for (i = 0; i < 64; i++) {
+				tdata[i] <<= 1;
+				tdata[i] |= (bdata[i] & 0x80) ? 0x01 : 0x00;
+			}
+		}
+	} else {
+		/* Oops, we should never get here! */
+		WARN_ON(1);
+		return 0;
+	}
+
+	for (i = 0; i < 64; i++)
+		if (tdata[i] != vdata[i]) {
+			changed = 1;
+			vdata[i] = tdata[i];
+		}
+	return changed;
+}
+
+void picolcd_fb_refresh(struct picolcd_data *data)
+{
+	if (data->fb_info)
+		schedule_delayed_work(&data->fb_info->deferred_work, 0);
+}
+
+/* Reconfigure LCD display */
+int picolcd_fb_reset(struct picolcd_data *data, int clear)
+{
+	struct hid_report *report = picolcd_out_report(REPORT_LCD_CMD, data->hdev);
+	struct picolcd_fb_data *fbdata = data->fb_info->par;
+	int i, j;
+	unsigned long flags;
+	static const u8 mapcmd[8] = { 0x00, 0x02, 0x00, 0x64, 0x3f, 0x00, 0x64, 0xc0 };
+
+	if (!report || report->maxfield != 1)
+		return -ENODEV;
+
+	spin_lock_irqsave(&data->lock, flags);
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < report->field[0]->maxusage; j++)
+			if (j == 0)
+				hid_set_field(report->field[0], j, i << 2);
+			else if (j < sizeof(mapcmd))
+				hid_set_field(report->field[0], j, mapcmd[j]);
+			else
+				hid_set_field(report->field[0], j, 0);
+		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	}
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	if (clear) {
+		memset(fbdata->vbitmap, 0, PICOLCDFB_SIZE);
+		memset(fbdata->bitmap, 0, PICOLCDFB_SIZE*fbdata->bpp);
+	}
+	fbdata->force = 1;
+
+	/* schedule first output of framebuffer */
+	if (fbdata->ready)
+		schedule_delayed_work(&data->fb_info->deferred_work, 0);
+	else
+		fbdata->ready = 1;
+
+	return 0;
+}
+
+/* Update fb_vbitmap from the screen_base and send changed tiles to device */
+static void picolcd_fb_update(struct fb_info *info)
+{
+	int chip, tile, n;
+	unsigned long flags;
+	struct picolcd_fb_data *fbdata = info->par;
+	struct picolcd_data *data;
+
+	mutex_lock(&info->lock);
+
+	spin_lock_irqsave(&fbdata->lock, flags);
+	if (!fbdata->ready && fbdata->picolcd)
+		picolcd_fb_reset(fbdata->picolcd, 0);
+	spin_unlock_irqrestore(&fbdata->lock, flags);
+
+	/*
+	 * Translate the framebuffer into the format needed by the PicoLCD.
+	 * See display layout above.
+	 * Do this one tile after the other and push those tiles that changed.
+	 *
+	 * Wait for our IO to complete as otherwise we might flood the queue!
+	 */
+	n = 0;
+	for (chip = 0; chip < 4; chip++)
+		for (tile = 0; tile < 8; tile++) {
+			if (!fbdata->force && !picolcd_fb_update_tile(
+					fbdata->vbitmap, fbdata->bitmap,
+					fbdata->bpp, chip, tile))
+				continue;
+			n += 2;
+			if (n >= HID_OUTPUT_FIFO_SIZE / 2) {
+				spin_lock_irqsave(&fbdata->lock, flags);
+				data = fbdata->picolcd;
+				spin_unlock_irqrestore(&fbdata->lock, flags);
+				mutex_unlock(&info->lock);
+				if (!data)
+					return;
+				usbhid_wait_io(data->hdev);
+				mutex_lock(&info->lock);
+				n = 0;
+			}
+			spin_lock_irqsave(&fbdata->lock, flags);
+			data = fbdata->picolcd;
+			spin_unlock_irqrestore(&fbdata->lock, flags);
+			if (!data || picolcd_fb_send_tile(data,
+					fbdata->vbitmap, chip, tile))
+				goto out;
+		}
+	fbdata->force = false;
+	if (n) {
+		spin_lock_irqsave(&fbdata->lock, flags);
+		data = fbdata->picolcd;
+		spin_unlock_irqrestore(&fbdata->lock, flags);
+		mutex_unlock(&info->lock);
+		if (data)
+			usbhid_wait_io(data->hdev);
+		return;
+	}
+out:
+	mutex_unlock(&info->lock);
+}
+
+/* Stub to call the system default and update the image on the picoLCD */
+static void picolcd_fb_fillrect(struct fb_info *info,
+		const struct fb_fillrect *rect)
+{
+	if (!info->par)
+		return;
+	sys_fillrect(info, rect);
+
+	schedule_delayed_work(&info->deferred_work, 0);
+}
+
+/* Stub to call the system default and update the image on the picoLCD */
+static void picolcd_fb_copyarea(struct fb_info *info,
+		const struct fb_copyarea *area)
+{
+	if (!info->par)
+		return;
+	sys_copyarea(info, area);
+
+	schedule_delayed_work(&info->deferred_work, 0);
+}
+
+/* Stub to call the system default and update the image on the picoLCD */
+static void picolcd_fb_imageblit(struct fb_info *info, const struct fb_image *image)
+{
+	if (!info->par)
+		return;
+	sys_imageblit(info, image);
+
+	schedule_delayed_work(&info->deferred_work, 0);
+}
+
+/*
+ * this is the slow path from userspace. they can seek and write to
+ * the fb. it's inefficient to do anything less than a full screen draw
+ */
+static ssize_t picolcd_fb_write(struct fb_info *info, const char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	ssize_t ret;
+	if (!info->par)
+		return -ENODEV;
+	ret = fb_sys_write(info, buf, count, ppos);
+	if (ret >= 0)
+		schedule_delayed_work(&info->deferred_work, 0);
+	return ret;
+}
+
+static int picolcd_fb_blank(int blank, struct fb_info *info)
+{
+	/* We let fb notification do this for us via lcd/backlight device */
+	return 0;
+}
+
+static void picolcd_fb_destroy(struct fb_info *info)
+{
+	struct picolcd_fb_data *fbdata = info->par;
+
+	/* make sure no work is deferred */
+	fb_deferred_io_cleanup(info);
+
+	/* No thridparty should ever unregister our framebuffer! */
+	WARN_ON(fbdata->picolcd != NULL);
+
+	vfree((u8 *)info->fix.smem_start);
+	framebuffer_release(info);
+}
+
+static int picolcd_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	__u32 bpp      = var->bits_per_pixel;
+	__u32 activate = var->activate;
+
+	/* only allow 1/8 bit depth (8-bit is grayscale) */
+	*var = picolcdfb_var;
+	var->activate = activate;
+	if (bpp >= 8) {
+		var->bits_per_pixel = 8;
+		var->red.length     = 8;
+		var->green.length   = 8;
+		var->blue.length    = 8;
+	} else {
+		var->bits_per_pixel = 1;
+		var->red.length     = 1;
+		var->green.length   = 1;
+		var->blue.length    = 1;
+	}
+	return 0;
+}
+
+static int picolcd_set_par(struct fb_info *info)
+{
+	struct picolcd_fb_data *fbdata = info->par;
+	u8 *tmp_fb, *o_fb;
+	if (info->var.bits_per_pixel == fbdata->bpp)
+		return 0;
+	/* switch between 1/8 bit depths */
+	if (info->var.bits_per_pixel != 1 && info->var.bits_per_pixel != 8)
+		return -EINVAL;
+
+	o_fb   = fbdata->bitmap;
+	tmp_fb = kmalloc(PICOLCDFB_SIZE*info->var.bits_per_pixel, GFP_KERNEL);
+	if (!tmp_fb)
+		return -ENOMEM;
+
+	/* translate FB content to new bits-per-pixel */
+	if (info->var.bits_per_pixel == 1) {
+		int i, b;
+		for (i = 0; i < PICOLCDFB_SIZE; i++) {
+			u8 p = 0;
+			for (b = 0; b < 8; b++) {
+				p <<= 1;
+				p |= o_fb[i*8+b] ? 0x01 : 0x00;
+			}
+			tmp_fb[i] = p;
+		}
+		memcpy(o_fb, tmp_fb, PICOLCDFB_SIZE);
+		info->fix.visual = FB_VISUAL_MONO01;
+		info->fix.line_length = PICOLCDFB_WIDTH / 8;
+	} else {
+		int i;
+		memcpy(tmp_fb, o_fb, PICOLCDFB_SIZE);
+		for (i = 0; i < PICOLCDFB_SIZE * 8; i++)
+			o_fb[i] = tmp_fb[i/8] & (0x01 << (7 - i % 8)) ? 0xff : 0x00;
+		info->fix.visual = FB_VISUAL_DIRECTCOLOR;
+		info->fix.line_length = PICOLCDFB_WIDTH;
+	}
+
+	kfree(tmp_fb);
+	fbdata->bpp = info->var.bits_per_pixel;
+	return 0;
+}
+
+/* Note this can't be const because of struct fb_info definition */
+static struct fb_ops picolcdfb_ops = {
+	.owner        = THIS_MODULE,
+	.fb_destroy   = picolcd_fb_destroy,
+	.fb_read      = fb_sys_read,
+	.fb_write     = picolcd_fb_write,
+	.fb_blank     = picolcd_fb_blank,
+	.fb_fillrect  = picolcd_fb_fillrect,
+	.fb_copyarea  = picolcd_fb_copyarea,
+	.fb_imageblit = picolcd_fb_imageblit,
+	.fb_check_var = picolcd_fb_check_var,
+	.fb_set_par   = picolcd_set_par,
+};
+
+
+/* Callback from deferred IO workqueue */
+static void picolcd_fb_deferred_io(struct fb_info *info, struct list_head *pagelist)
+{
+	picolcd_fb_update(info);
+}
+
+static const struct fb_deferred_io picolcd_fb_defio = {
+	.delay = HZ / PICOLCDFB_UPDATE_RATE_DEFAULT,
+	.deferred_io = picolcd_fb_deferred_io,
+};
+
+
+/*
+ * The "fb_update_rate" sysfs attribute
+ */
+static ssize_t picolcd_fb_update_rate_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+	struct picolcd_fb_data *fbdata = data->fb_info->par;
+	unsigned i, fb_update_rate = fbdata->update_rate;
+	size_t ret = 0;
+
+	for (i = 1; i <= PICOLCDFB_UPDATE_RATE_LIMIT; i++)
+		if (ret >= PAGE_SIZE)
+			break;
+		else if (i == fb_update_rate)
+			ret += snprintf(buf+ret, PAGE_SIZE-ret, "[%u] ", i);
+		else
+			ret += snprintf(buf+ret, PAGE_SIZE-ret, "%u ", i);
+	if (ret > 0)
+		buf[min(ret, (size_t)PAGE_SIZE)-1] = '\n';
+	return ret;
+}
+
+static ssize_t picolcd_fb_update_rate_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct picolcd_data *data = dev_get_drvdata(dev);
+	struct picolcd_fb_data *fbdata = data->fb_info->par;
+	int i;
+	unsigned u;
+
+	if (count < 1 || count > 10)
+		return -EINVAL;
+
+	i = sscanf(buf, "%u", &u);
+	if (i != 1)
+		return -EINVAL;
+
+	if (u > PICOLCDFB_UPDATE_RATE_LIMIT)
+		return -ERANGE;
+	else if (u == 0)
+		u = PICOLCDFB_UPDATE_RATE_DEFAULT;
+
+	fbdata->update_rate = u;
+	data->fb_info->fbdefio->delay = HZ / fbdata->update_rate;
+	return count;
+}
+
+static DEVICE_ATTR(fb_update_rate, 0666, picolcd_fb_update_rate_show,
+		picolcd_fb_update_rate_store);
+
+/* initialize Framebuffer device */
+int picolcd_init_framebuffer(struct picolcd_data *data)
+{
+	struct device *dev = &data->hdev->dev;
+	struct fb_info *info = NULL;
+	struct picolcd_fb_data *fbdata = NULL;
+	int i, error = -ENOMEM;
+	u32 *palette;
+
+	/* The extra memory is:
+	 * - 256*u32 for pseudo_palette
+	 * - struct fb_deferred_io
+	 */
+	info = framebuffer_alloc(256 * sizeof(u32) +
+			sizeof(struct fb_deferred_io) +
+			sizeof(struct picolcd_fb_data) +
+			PICOLCDFB_SIZE, dev);
+	if (info == NULL) {
+		dev_err(dev, "failed to allocate a framebuffer\n");
+		goto err_nomem;
+	}
+
+	info->fbdefio = info->par;
+	*info->fbdefio = picolcd_fb_defio;
+	info->par += sizeof(struct fb_deferred_io);
+	palette = info->par;
+	info->par += 256 * sizeof(u32);
+	for (i = 0; i < 256; i++)
+		palette[i] = i > 0 && i < 16 ? 0xff : 0;
+	info->pseudo_palette = palette;
+	info->fbops = &picolcdfb_ops;
+	info->var = picolcdfb_var;
+	info->fix = picolcdfb_fix;
+	info->fix.smem_len   = PICOLCDFB_SIZE*8;
+	info->flags = FBINFO_FLAG_DEFAULT;
+
+	fbdata = info->par;
+	spin_lock_init(&fbdata->lock);
+	fbdata->picolcd = data;
+	fbdata->update_rate = PICOLCDFB_UPDATE_RATE_DEFAULT;
+	fbdata->bpp     = picolcdfb_var.bits_per_pixel;
+	fbdata->force   = 1;
+	fbdata->vbitmap = info->par + sizeof(struct picolcd_fb_data);
+	fbdata->bitmap  = vmalloc(PICOLCDFB_SIZE*8);
+	if (fbdata->bitmap == NULL) {
+		dev_err(dev, "can't get a free page for framebuffer\n");
+		goto err_nomem;
+	}
+	info->screen_base = (char __force __iomem *)fbdata->bitmap;
+	info->fix.smem_start = (unsigned long)fbdata->bitmap;
+	memset(fbdata->vbitmap, 0xff, PICOLCDFB_SIZE);
+	data->fb_info = info;
+
+	error = picolcd_fb_reset(data, 1);
+	if (error) {
+		dev_err(dev, "failed to configure display\n");
+		goto err_cleanup;
+	}
+
+	error = device_create_file(dev, &dev_attr_fb_update_rate);
+	if (error) {
+		dev_err(dev, "failed to create sysfs attributes\n");
+		goto err_cleanup;
+	}
+
+	fb_deferred_io_init(info);
+	error = register_framebuffer(info);
+	if (error) {
+		dev_err(dev, "failed to register framebuffer\n");
+		goto err_sysfs;
+	}
+	return 0;
+
+err_sysfs:
+	device_remove_file(dev, &dev_attr_fb_update_rate);
+	fb_deferred_io_cleanup(info);
+err_cleanup:
+	data->fb_info    = NULL;
+
+err_nomem:
+	if (fbdata)
+		vfree(fbdata->bitmap);
+	framebuffer_release(info);
+	return error;
+}
+
+void picolcd_exit_framebuffer(struct picolcd_data *data)
+{
+	struct fb_info *info = data->fb_info;
+	struct picolcd_fb_data *fbdata = info->par;
+	unsigned long flags;
+
+	device_remove_file(&data->hdev->dev, &dev_attr_fb_update_rate);
+
+	/* disconnect framebuffer from HID dev */
+	spin_lock_irqsave(&fbdata->lock, flags);
+	fbdata->picolcd = NULL;
+	spin_unlock_irqrestore(&fbdata->lock, flags);
+
+	/* make sure there is no running update - thus that fbdata->picolcd
+	 * once obtained under lock is guaranteed not to get free() under
+	 * the feet of the deferred work */
+	flush_delayed_work_sync(&info->deferred_work);
+
+	data->fb_info = NULL;
+	unregister_framebuffer(info);
+}
diff --git a/drivers/hid/hid-picolcd_lcd.c b/drivers/hid/hid-picolcd_lcd.c
new file mode 100644
index 000000000000..2d0ddc5ac65f
--- /dev/null
+++ b/drivers/hid/hid-picolcd_lcd.c
@@ -0,0 +1,107 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/lcd.h>
+
+#include "hid-picolcd.h"
+
+/*
+ * lcd class device
+ */
+static int picolcd_get_contrast(struct lcd_device *ldev)
+{
+	struct picolcd_data *data = lcd_get_data(ldev);
+	return data->lcd_contrast;
+}
+
+static int picolcd_set_contrast(struct lcd_device *ldev, int contrast)
+{
+	struct picolcd_data *data = lcd_get_data(ldev);
+	struct hid_report *report = picolcd_out_report(REPORT_CONTRAST, data->hdev);
+	unsigned long flags;
+
+	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
+		return -ENODEV;
+
+	data->lcd_contrast = contrast & 0x0ff;
+	spin_lock_irqsave(&data->lock, flags);
+	hid_set_field(report->field[0], 0, data->lcd_contrast);
+	if (!(data->status & PICOLCD_FAILED))
+		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+	return 0;
+}
+
+static int picolcd_check_lcd_fb(struct lcd_device *ldev, struct fb_info *fb)
+{
+	return fb && fb == picolcd_fbinfo((struct picolcd_data *)lcd_get_data(ldev));
+}
+
+static struct lcd_ops picolcd_lcdops = {
+	.get_contrast   = picolcd_get_contrast,
+	.set_contrast   = picolcd_set_contrast,
+	.check_fb       = picolcd_check_lcd_fb,
+};
+
+int picolcd_init_lcd(struct picolcd_data *data, struct hid_report *report)
+{
+	struct device *dev = &data->hdev->dev;
+	struct lcd_device *ldev;
+
+	if (!report)
+		return -ENODEV;
+	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
+			report->field[0]->report_size != 8) {
+		dev_err(dev, "unsupported CONTRAST report");
+		return -EINVAL;
+	}
+
+	ldev = lcd_device_register(dev_name(dev), dev, data, &picolcd_lcdops);
+	if (IS_ERR(ldev)) {
+		dev_err(dev, "failed to register LCD\n");
+		return PTR_ERR(ldev);
+	}
+	ldev->props.max_contrast = 0x0ff;
+	data->lcd_contrast = 0xe5;
+	data->lcd = ldev;
+	picolcd_set_contrast(ldev, 0xe5);
+	return 0;
+}
+
+void picolcd_exit_lcd(struct picolcd_data *data)
+{
+	struct lcd_device *ldev = data->lcd;
+
+	data->lcd = NULL;
+	if (ldev)
+		lcd_device_unregister(ldev);
+}
+
+int picolcd_resume_lcd(struct picolcd_data *data)
+{
+	if (!data->lcd)
+		return 0;
+	return picolcd_set_contrast(data->lcd, data->lcd_contrast);
+}
+
diff --git a/drivers/hid/hid-picolcd_leds.c b/drivers/hid/hid-picolcd_leds.c
new file mode 100644
index 000000000000..28cb6a4f9634
--- /dev/null
+++ b/drivers/hid/hid-picolcd_leds.c
@@ -0,0 +1,175 @@
+/***************************************************************************
+ *   Copyright (C) 2010-2012 by Bruno Prémont <bonbons@linux-vserver.org>  *
+ *                                                                         *
+ *   Based on Logitech G13 driver (v0.4)                                   *
+ *     Copyright (C) 2009 by Rick L. Vinyard, Jr. <rvinyard@cs.nmsu.edu>   *
+ *                                                                         *
+ *   This program is free software: 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 driver is distributed in the hope that it will be useful, but    *
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of            *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      *
+ *   General Public License for more details.                              *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this software. If not see <http://www.gnu.org/licenses/>.  *
+ ***************************************************************************/
+
+#include <linux/hid.h>
+#include <linux/hid-debug.h>
+#include <linux/input.h>
+#include "hid-ids.h"
+#include "usbhid/usbhid.h"
+#include <linux/usb.h>
+
+#include <linux/fb.h>
+#include <linux/vmalloc.h>
+#include <linux/backlight.h>
+#include <linux/lcd.h>
+
+#include <linux/leds.h>
+
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+
+#include <linux/completion.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+
+#include "hid-picolcd.h"
+
+
+void picolcd_leds_set(struct picolcd_data *data)
+{
+	struct hid_report *report;
+	unsigned long flags;
+
+	if (!data->led[0])
+		return;
+	report = picolcd_out_report(REPORT_LED_STATE, data->hdev);
+	if (!report || report->maxfield != 1 || report->field[0]->report_count != 1)
+		return;
+
+	spin_lock_irqsave(&data->lock, flags);
+	hid_set_field(report->field[0], 0, data->led_state);
+	if (!(data->status & PICOLCD_FAILED))
+		usbhid_submit_report(data->hdev, report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&data->lock, flags);
+}
+
+static void picolcd_led_set_brightness(struct led_classdev *led_cdev,
+			enum led_brightness value)
+{
+	struct device *dev;
+	struct hid_device *hdev;
+	struct picolcd_data *data;
+	int i, state = 0;
+
+	dev  = led_cdev->dev->parent;
+	hdev = container_of(dev, struct hid_device, dev);
+	data = hid_get_drvdata(hdev);
+	if (!data)
+		return;
+	for (i = 0; i < 8; i++) {
+		if (led_cdev != data->led[i])
+			continue;
+		state = (data->led_state >> i) & 1;
+		if (value == LED_OFF && state) {
+			data->led_state &= ~(1 << i);
+			picolcd_leds_set(data);
+		} else if (value != LED_OFF && !state) {
+			data->led_state |= 1 << i;
+			picolcd_leds_set(data);
+		}
+		break;
+	}
+}
+
+static enum led_brightness picolcd_led_get_brightness(struct led_classdev *led_cdev)
+{
+	struct device *dev;
+	struct hid_device *hdev;
+	struct picolcd_data *data;
+	int i, value = 0;
+
+	dev  = led_cdev->dev->parent;
+	hdev = container_of(dev, struct hid_device, dev);
+	data = hid_get_drvdata(hdev);
+	for (i = 0; i < 8; i++)
+		if (led_cdev == data->led[i]) {
+			value = (data->led_state >> i) & 1;
+			break;
+		}
+	return value ? LED_FULL : LED_OFF;
+}
+
+int picolcd_init_leds(struct picolcd_data *data, struct hid_report *report)
+{
+	struct device *dev = &data->hdev->dev;
+	struct led_classdev *led;
+	size_t name_sz = strlen(dev_name(dev)) + 8;
+	char *name;
+	int i, ret = 0;
+
+	if (!report)
+		return -ENODEV;
+	if (report->maxfield != 1 || report->field[0]->report_count != 1 ||
+			report->field[0]->report_size != 8) {
+		dev_err(dev, "unsupported LED_STATE report");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < 8; i++) {
+		led = kzalloc(sizeof(struct led_classdev)+name_sz, GFP_KERNEL);
+		if (!led) {
+			dev_err(dev, "can't allocate memory for LED %d\n", i);
+			ret = -ENOMEM;
+			goto err;
+		}
+		name = (void *)(&led[1]);
+		snprintf(name, name_sz, "%s::GPO%d", dev_name(dev), i);
+		led->name = name;
+		led->brightness = 0;
+		led->max_brightness = 1;
+		led->brightness_get = picolcd_led_get_brightness;
+		led->brightness_set = picolcd_led_set_brightness;
+
+		data->led[i] = led;
+		ret = led_classdev_register(dev, data->led[i]);
+		if (ret) {
+			data->led[i] = NULL;
+			kfree(led);
+			dev_err(dev, "can't register LED %d\n", i);
+			goto err;
+		}
+	}
+	return 0;
+err:
+	for (i = 0; i < 8; i++)
+		if (data->led[i]) {
+			led = data->led[i];
+			data->led[i] = NULL;
+			led_classdev_unregister(led);
+			kfree(led);
+		}
+	return ret;
+}
+
+void picolcd_exit_leds(struct picolcd_data *data)
+{
+	struct led_classdev *led;
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		led = data->led[i];
+		data->led[i] = NULL;
+		if (!led)
+			continue;
+		led_classdev_unregister(led);
+		kfree(led);
+	}
+}
+
+
diff --git a/drivers/hid/hid-primax.c b/drivers/hid/hid-primax.c
index 4d3c60d88318..c15adb0c98a1 100644
--- a/drivers/hid/hid-primax.c
+++ b/drivers/hid/hid-primax.c
@@ -64,29 +64,6 @@ static int px_raw_event(struct hid_device *hid, struct hid_report *report,
 	return 0;
 }
 
-static int px_probe(struct hid_device *hid, const struct hid_device_id *id)
-{
-	int ret;
-
-	ret = hid_parse(hid);
-	if (ret) {
-		hid_err(hid, "parse failed\n");
-		goto fail;
-	}
-
-	ret = hid_hw_start(hid, HID_CONNECT_DEFAULT);
-	if (ret)
-		hid_err(hid, "hw start failed\n");
-
-fail:
-	return ret;
-}
-
-static void px_remove(struct hid_device *hid)
-{
-	hid_hw_stop(hid);
-}
-
 static const struct hid_device_id px_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
 	{ }
@@ -97,8 +74,6 @@ static struct hid_driver px_driver = {
 	.name = "primax",
 	.id_table = px_devices,
 	.raw_event = px_raw_event,
-	.probe = px_probe,
-	.remove = px_remove,
 };
 
 static int __init px_init(void)
diff --git a/drivers/hid/hid-prodikeys.c b/drivers/hid/hid-prodikeys.c
index b71b77ab0dc7..ec8ca3336315 100644
--- a/drivers/hid/hid-prodikeys.c
+++ b/drivers/hid/hid-prodikeys.c
@@ -105,7 +105,7 @@ static ssize_t show_channel(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read channel=%u\n", pk->pm->midi_channel);
 
@@ -118,7 +118,7 @@ static ssize_t store_channel(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	unsigned channel = 0;
 
@@ -142,7 +142,7 @@ static ssize_t show_sustain(struct device *dev,
  struct device_attribute *attr, char *buf)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read sustain=%u\n", pk->pm->midi_sustain);
 
@@ -155,7 +155,7 @@ static ssize_t store_sustain(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	unsigned sustain = 0;
 
@@ -181,7 +181,7 @@ static ssize_t show_octave(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read octave=%d\n", pk->pm->midi_octave);
 
@@ -194,7 +194,7 @@ static ssize_t store_octave(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	int octave = 0;
 
@@ -759,7 +759,7 @@ static int pk_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 	struct pcmidi_snd *pm;
 
 	pm = pk->pm;
@@ -777,7 +777,7 @@ static int pk_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 static int pk_raw_event(struct hid_device *hdev, struct hid_report *report,
 	u8 *data, int size)
 {
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 	int ret = 0;
 
 	if (1 == pk->pm->ifnum) {
@@ -858,7 +858,7 @@ err_free_pk:
 
 static void pk_remove(struct hid_device *hdev)
 {
-	struct pk_device *pk = (struct pk_device *)hid_get_drvdata(hdev);
+	struct pk_device *pk = hid_get_drvdata(hdev);
 	struct pcmidi_snd *pm;
 
 	pm = pk->pm;
diff --git a/drivers/hid/hid-ps3remote.c b/drivers/hid/hid-ps3remote.c
new file mode 100644
index 000000000000..03811e539d71
--- /dev/null
+++ b/drivers/hid/hid-ps3remote.c
@@ -0,0 +1,215 @@
+/*
+ * HID driver for Sony PS3 BD Remote Control
+ *
+ * Copyright (c) 2012 David Dillow <dave@thedillows.org>
+ * Based on a blend of the bluez fakehid user-space code by Marcel Holtmann
+ * and other kernel HID drivers.
+ */
+
+/*
+ * This program is free software; you can redistribute 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.
+ */
+
+/* NOTE: in order for the Sony PS3 BD Remote Control to be found by
+ * a Bluetooth host, the key combination Start+Enter has to be kept pressed
+ * for about 7 seconds with the Bluetooth Host Controller in discovering mode.
+ *
+ * There will be no PIN request from the device.
+ */
+
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+
+#include "hid-ids.h"
+
+static __u8 ps3remote_rdesc[] = {
+	0x05, 0x01,          /* GUsagePage Generic Desktop */
+	0x09, 0x05,          /* LUsage 0x05 [Game Pad] */
+	0xA1, 0x01,          /* MCollection Application (mouse, keyboard) */
+
+	 /* Use collection 1 for joypad buttons */
+	 0xA1, 0x02,         /* MCollection Logical (interrelated data) */
+
+	  /* Ignore the 1st byte, maybe it is used for a controller
+	   * number but it's not needed for correct operation */
+	  0x75, 0x08,        /* GReportSize 0x08 [8] */
+	  0x95, 0x01,        /* GReportCount 0x01 [1] */
+	  0x81, 0x01,        /* MInput 0x01 (Const[0] Arr[1] Abs[2]) */
+
+	  /* Bytes from 2nd to 4th are a bitmap for joypad buttons, for these
+	   * buttons multiple keypresses are allowed */
+	  0x05, 0x09,        /* GUsagePage Button */
+	  0x19, 0x01,        /* LUsageMinimum 0x01 [Button 1 (primary/trigger)] */
+	  0x29, 0x18,        /* LUsageMaximum 0x18 [Button 24] */
+	  0x14,              /* GLogicalMinimum [0] */
+	  0x25, 0x01,        /* GLogicalMaximum 0x01 [1] */
+	  0x75, 0x01,        /* GReportSize 0x01 [1] */
+	  0x95, 0x18,        /* GReportCount 0x18 [24] */
+	  0x81, 0x02,        /* MInput 0x02 (Data[0] Var[1] Abs[2]) */
+
+	  0xC0,              /* MEndCollection */
+
+	 /* Use collection 2 for remote control buttons */
+	 0xA1, 0x02,         /* MCollection Logical (interrelated data) */
+
+	  /* 5th byte is used for remote control buttons */
+	  0x05, 0x09,        /* GUsagePage Button */
+	  0x18,              /* LUsageMinimum [No button pressed] */
+	  0x29, 0xFE,        /* LUsageMaximum 0xFE [Button 254] */
+	  0x14,              /* GLogicalMinimum [0] */
+	  0x26, 0xFE, 0x00,  /* GLogicalMaximum 0x00FE [254] */
+	  0x75, 0x08,        /* GReportSize 0x08 [8] */
+	  0x95, 0x01,        /* GReportCount 0x01 [1] */
+	  0x80,              /* MInput  */
+
+	  /* Ignore bytes from 6th to 11th, 6th to 10th are always constant at
+	   * 0xff and 11th is for press indication */
+	  0x75, 0x08,        /* GReportSize 0x08 [8] */
+	  0x95, 0x06,        /* GReportCount 0x06 [6] */
+	  0x81, 0x01,        /* MInput 0x01 (Const[0] Arr[1] Abs[2]) */
+
+	  /* 12th byte is for battery strength */
+	  0x05, 0x06,        /* GUsagePage Generic Device Controls */
+	  0x09, 0x20,        /* LUsage 0x20 [Battery Strength] */
+	  0x14,              /* GLogicalMinimum [0] */
+	  0x25, 0x05,        /* GLogicalMaximum 0x05 [5] */
+	  0x75, 0x08,        /* GReportSize 0x08 [8] */
+	  0x95, 0x01,        /* GReportCount 0x01 [1] */
+	  0x81, 0x02,        /* MInput 0x02 (Data[0] Var[1] Abs[2]) */
+
+	  0xC0,              /* MEndCollection */
+
+	 0xC0                /* MEndCollection [Game Pad] */
+};
+
+static const unsigned int ps3remote_keymap_joypad_buttons[] = {
+	[0x01] = KEY_SELECT,
+	[0x02] = BTN_THUMBL,		/* L3 */
+	[0x03] = BTN_THUMBR,		/* R3 */
+	[0x04] = BTN_START,
+	[0x05] = KEY_UP,
+	[0x06] = KEY_RIGHT,
+	[0x07] = KEY_DOWN,
+	[0x08] = KEY_LEFT,
+	[0x09] = BTN_TL2,		/* L2 */
+	[0x0a] = BTN_TR2,		/* R2 */
+	[0x0b] = BTN_TL,		/* L1 */
+	[0x0c] = BTN_TR,		/* R1 */
+	[0x0d] = KEY_OPTION,		/* options/triangle */
+	[0x0e] = KEY_BACK,		/* back/circle */
+	[0x0f] = BTN_0,			/* cross */
+	[0x10] = KEY_SCREEN,		/* view/square */
+	[0x11] = KEY_HOMEPAGE,		/* PS button */
+	[0x14] = KEY_ENTER,
+};
+static const unsigned int ps3remote_keymap_remote_buttons[] = {
+	[0x00] = KEY_1,
+	[0x01] = KEY_2,
+	[0x02] = KEY_3,
+	[0x03] = KEY_4,
+	[0x04] = KEY_5,
+	[0x05] = KEY_6,
+	[0x06] = KEY_7,
+	[0x07] = KEY_8,
+	[0x08] = KEY_9,
+	[0x09] = KEY_0,
+	[0x0e] = KEY_ESC,		/* return */
+	[0x0f] = KEY_CLEAR,
+	[0x16] = KEY_EJECTCD,
+	[0x1a] = KEY_MENU,		/* top menu */
+	[0x28] = KEY_TIME,
+	[0x30] = KEY_PREVIOUS,
+	[0x31] = KEY_NEXT,
+	[0x32] = KEY_PLAY,
+	[0x33] = KEY_REWIND,		/* scan back */
+	[0x34] = KEY_FORWARD,		/* scan forward */
+	[0x38] = KEY_STOP,
+	[0x39] = KEY_PAUSE,
+	[0x40] = KEY_CONTEXT_MENU,	/* pop up/menu */
+	[0x60] = KEY_FRAMEBACK,		/* slow/step back */
+	[0x61] = KEY_FRAMEFORWARD,	/* slow/step forward */
+	[0x63] = KEY_SUBTITLE,
+	[0x64] = KEY_AUDIO,
+	[0x65] = KEY_ANGLE,
+	[0x70] = KEY_INFO,		/* display */
+	[0x80] = KEY_BLUE,
+	[0x81] = KEY_RED,
+	[0x82] = KEY_GREEN,
+	[0x83] = KEY_YELLOW,
+};
+
+static __u8 *ps3remote_fixup(struct hid_device *hdev, __u8 *rdesc,
+			     unsigned int *rsize)
+{
+	*rsize = sizeof(ps3remote_rdesc);
+	return ps3remote_rdesc;
+}
+
+static int ps3remote_mapping(struct hid_device *hdev, struct hid_input *hi,
+			     struct hid_field *field, struct hid_usage *usage,
+			     unsigned long **bit, int *max)
+{
+	unsigned int key = usage->hid & HID_USAGE;
+
+	if ((usage->hid & HID_USAGE_PAGE) != HID_UP_BUTTON)
+		return -1;
+
+	switch (usage->collection_index) {
+	case 1:
+		if (key >= ARRAY_SIZE(ps3remote_keymap_joypad_buttons))
+			return -1;
+
+		key = ps3remote_keymap_joypad_buttons[key];
+		if (!key)
+			return -1;
+		break;
+	case 2:
+		if (key >= ARRAY_SIZE(ps3remote_keymap_remote_buttons))
+			return -1;
+
+		key = ps3remote_keymap_remote_buttons[key];
+		if (!key)
+			return -1;
+		break;
+	default:
+		return -1;
+	}
+
+	hid_map_usage_clear(hi, usage, bit, max, EV_KEY, key);
+	return 1;
+}
+
+static const struct hid_device_id ps3remote_devices[] = {
+	/* PS3 BD Remote Control */
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_BDREMOTE) },
+	/* Logitech Harmony Adapter for PS3 */
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, ps3remote_devices);
+
+static struct hid_driver ps3remote_driver = {
+	.name          = "ps3_remote",
+	.id_table      = ps3remote_devices,
+	.report_fixup  = ps3remote_fixup,
+	.input_mapping = ps3remote_mapping,
+};
+
+static int __init ps3remote_init(void)
+{
+	return hid_register_driver(&ps3remote_driver);
+}
+
+static void __exit ps3remote_exit(void)
+{
+	hid_unregister_driver(&ps3remote_driver);
+}
+
+module_init(ps3remote_init);
+module_exit(ps3remote_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("David Dillow <dave@thedillows.org>, Antonio Ospite <ospite@studenti.unina.it>");
diff --git a/drivers/hid/hid-samsung.c b/drivers/hid/hid-samsung.c
index 3c1fd8af5e0c..a5821d317229 100644
--- a/drivers/hid/hid-samsung.c
+++ b/drivers/hid/hid-samsung.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  *  Copyright (c) 2010 Don Prince <dhprince.devel@yahoo.co.uk>
  *
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
new file mode 100644
index 000000000000..d9d73e9163eb
--- /dev/null
+++ b/drivers/hid/hid-sensor-hub.c
@@ -0,0 +1,680 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/usb.h>
+#include "usbhid/usbhid.h"
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/mfd/core.h>
+#include <linux/list.h>
+#include <linux/hid-sensor-ids.h>
+#include <linux/hid-sensor-hub.h>
+#include "hid-ids.h"
+
+/**
+ * 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.
+ * @hid_sensor_client_cnt: Number of MFD cells, (no of sensors attached).
+ */
+struct sensor_hub_data {
+	struct hid_sensor_hub_device *hsdev;
+	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;
+	int hid_sensor_client_cnt;
+};
+
+/**
+ * struct hid_sensor_hub_callbacks_list - Stores callback list
+ * @list:		list head.
+ * @usage_id:		usage id for a physical device.
+ * @usage_callback:	Stores registered callback functions.
+ * @priv:		Private data for a physical device.
+ */
+struct hid_sensor_hub_callbacks_list {
+	struct list_head list;
+	u32 usage_id;
+	struct hid_sensor_hub_callbacks *usage_callback;
+	void *priv;
+};
+
+static int sensor_hub_check_for_sensor_page(struct hid_device *hdev)
+{
+	int i;
+	int ret = -EINVAL;
+
+	for (i = 0; i < hdev->maxcollection; i++) {
+		struct hid_collection *col = &hdev->collection[i];
+		if (col->type == HID_COLLECTION_PHYSICAL &&
+		   (col->usage & HID_USAGE_PAGE) == HID_UP_SENSOR) {
+			ret = 0;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static struct hid_report *sensor_hub_report(int id, struct hid_device *hdev,
+						int dir)
+{
+	struct hid_report *report;
+
+	list_for_each_entry(report, &hdev->report_enum[dir].report_list, list) {
+		if (report->id == id)
+			return report;
+	}
+	hid_warn(hdev, "No report with id 0x%x found\n", id);
+
+	return NULL;
+}
+
+static int sensor_hub_get_physical_device_count(
+				struct hid_report_enum *report_enum)
+{
+	struct hid_report *report;
+	struct hid_field *field;
+	int cnt = 0;
+
+	list_for_each_entry(report, &report_enum->report_list, list) {
+		field = report->field[0];
+		if (report->maxfield && field &&
+					field->physical)
+			cnt++;
+	}
+
+	return cnt;
+}
+
+static void sensor_hub_fill_attr_info(
+		struct hid_sensor_hub_attribute_info *info,
+		s32 index, s32 report_id, s32 units, s32 unit_expo, s32 size)
+{
+	info->index = index;
+	info->report_id = report_id;
+	info->units = units;
+	info->unit_expo = unit_expo;
+	info->size = size/8;
+}
+
+static struct hid_sensor_hub_callbacks *sensor_hub_get_callback(
+					struct hid_device *hdev,
+					u32 usage_id, void **priv)
+{
+	struct hid_sensor_hub_callbacks_list *callback;
+	struct sensor_hub_data *pdata = hid_get_drvdata(hdev);
+
+	spin_lock(&pdata->dyn_callback_lock);
+	list_for_each_entry(callback, &pdata->dyn_callback_list, list)
+		if (callback->usage_id == usage_id) {
+			*priv = callback->priv;
+			spin_unlock(&pdata->dyn_callback_lock);
+			return callback->usage_callback;
+		}
+	spin_unlock(&pdata->dyn_callback_lock);
+
+	return NULL;
+}
+
+int sensor_hub_register_callback(struct hid_sensor_hub_device *hsdev,
+			u32 usage_id,
+			struct hid_sensor_hub_callbacks *usage_callback)
+{
+	struct hid_sensor_hub_callbacks_list *callback;
+	struct sensor_hub_data *pdata = hid_get_drvdata(hsdev->hdev);
+
+	spin_lock(&pdata->dyn_callback_lock);
+	list_for_each_entry(callback, &pdata->dyn_callback_list, list)
+		if (callback->usage_id == usage_id) {
+			spin_unlock(&pdata->dyn_callback_lock);
+			return -EINVAL;
+		}
+	callback = kzalloc(sizeof(*callback), GFP_ATOMIC);
+	if (!callback) {
+		spin_unlock(&pdata->dyn_callback_lock);
+		return -ENOMEM;
+	}
+	callback->usage_callback = usage_callback;
+	callback->usage_id = usage_id;
+	callback->priv = NULL;
+	list_add_tail(&callback->list, &pdata->dyn_callback_list);
+	spin_unlock(&pdata->dyn_callback_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(sensor_hub_register_callback);
+
+int sensor_hub_remove_callback(struct hid_sensor_hub_device *hsdev,
+				u32 usage_id)
+{
+	struct hid_sensor_hub_callbacks_list *callback;
+	struct sensor_hub_data *pdata = hid_get_drvdata(hsdev->hdev);
+
+	spin_lock(&pdata->dyn_callback_lock);
+	list_for_each_entry(callback, &pdata->dyn_callback_list, list)
+		if (callback->usage_id == usage_id) {
+			list_del(&callback->list);
+			kfree(callback);
+			break;
+		}
+	spin_unlock(&pdata->dyn_callback_lock);
+
+	return 0;
+}
+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)
+{
+	struct hid_report *report;
+	struct sensor_hub_data *data =  hid_get_drvdata(hsdev->hdev);
+	int ret = 0;
+
+	mutex_lock(&data->mutex);
+	report = sensor_hub_report(report_id, hsdev->hdev, HID_FEATURE_REPORT);
+	if (!report || (field_index >=  report->maxfield)) {
+		ret = -EINVAL;
+		goto done_proc;
+	}
+	hid_set_field(report->field[field_index], 0, value);
+	usbhid_submit_report(hsdev->hdev, report, USB_DIR_OUT);
+	usbhid_wait_io(hsdev->hdev);
+
+done_proc:
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+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)
+{
+	struct hid_report *report;
+	struct sensor_hub_data *data =  hid_get_drvdata(hsdev->hdev);
+	int ret = 0;
+
+	mutex_lock(&data->mutex);
+	report = sensor_hub_report(report_id, hsdev->hdev, HID_FEATURE_REPORT);
+	if (!report || (field_index >=  report->maxfield)) {
+		ret = -EINVAL;
+		goto done_proc;
+	}
+	usbhid_submit_report(hsdev->hdev, report, USB_DIR_IN);
+	usbhid_wait_io(hsdev->hdev);
+	*value = report->field[field_index]->value[0];
+
+done_proc:
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+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)
+{
+	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;
+	report = sensor_hub_report(report_id, hsdev->hdev, HID_INPUT_REPORT);
+	if (!report) {
+		spin_unlock_irqrestore(&data->lock, flags);
+		goto err_free;
+	}
+	usbhid_submit_report(hsdev->hdev, report, USB_DIR_IN);
+	spin_unlock_irqrestore(&data->lock, flags);
+	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;
+	}
+	kfree(data->pending.raw_data);
+
+err_free:
+	data->pending.status = false;
+	mutex_unlock(&data->mutex);
+
+	return ret_val;
+}
+EXPORT_SYMBOL_GPL(sensor_hub_input_attr_get_raw_value);
+
+int sensor_hub_input_get_attribute_info(struct hid_sensor_hub_device *hsdev,
+				u8 type,
+				u32 usage_id,
+				u32 attr_usage_id,
+				struct hid_sensor_hub_attribute_info *info)
+{
+	int ret = -1;
+	int i, j;
+	int collection_index = -1;
+	struct hid_report *report;
+	struct hid_field *field;
+	struct hid_report_enum *report_enum;
+	struct hid_device *hdev = hsdev->hdev;
+
+	/* Initialize with defaults */
+	info->usage_id = usage_id;
+	info->attrib_id =  attr_usage_id;
+	info->report_id = -1;
+	info->index = -1;
+	info->units = -1;
+	info->unit_expo = -1;
+
+	for (i = 0; i < hdev->maxcollection; ++i) {
+		struct hid_collection *collection = &hdev->collection[i];
+		if (usage_id == collection->usage) {
+			collection_index = i;
+			break;
+		}
+	}
+	if (collection_index == -1)
+		goto err_ret;
+
+	report_enum = &hdev->report_enum[type];
+	list_for_each_entry(report, &report_enum->report_list, list) {
+		for (i = 0; i < report->maxfield; ++i) {
+			field = report->field[i];
+			if (field->physical == usage_id &&
+				field->logical == attr_usage_id) {
+				sensor_hub_fill_attr_info(info, i, report->id,
+					field->unit, field->unit_exponent,
+					field->report_size);
+				ret = 0;
+			} else {
+				for (j = 0; j < field->maxusage; ++j) {
+					if (field->usage[j].hid ==
+					attr_usage_id &&
+					field->usage[j].collection_index ==
+					collection_index)  {
+						sensor_hub_fill_attr_info(info,
+							i, report->id,
+							field->unit,
+							field->unit_exponent,
+							field->report_size);
+						ret = 0;
+						break;
+					}
+				}
+			}
+			if (ret == 0)
+				break;
+		}
+	}
+
+err_ret:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(sensor_hub_input_get_attribute_info);
+
+#ifdef CONFIG_PM
+static int sensor_hub_suspend(struct hid_device *hdev, pm_message_t message)
+{
+	struct sensor_hub_data *pdata =  hid_get_drvdata(hdev);
+	struct hid_sensor_hub_callbacks_list *callback;
+
+	hid_dbg(hdev, " sensor_hub_suspend\n");
+	spin_lock(&pdata->dyn_callback_lock);
+	list_for_each_entry(callback, &pdata->dyn_callback_list, list) {
+		if (callback->usage_callback->suspend)
+			callback->usage_callback->suspend(
+					pdata->hsdev, callback->priv);
+	}
+	spin_unlock(&pdata->dyn_callback_lock);
+
+	return 0;
+}
+
+static int sensor_hub_resume(struct hid_device *hdev)
+{
+	struct sensor_hub_data *pdata =  hid_get_drvdata(hdev);
+	struct hid_sensor_hub_callbacks_list *callback;
+
+	hid_dbg(hdev, " sensor_hub_resume\n");
+	spin_lock(&pdata->dyn_callback_lock);
+	list_for_each_entry(callback, &pdata->dyn_callback_list, list) {
+		if (callback->usage_callback->resume)
+			callback->usage_callback->resume(
+					pdata->hsdev, callback->priv);
+	}
+	spin_unlock(&pdata->dyn_callback_lock);
+
+	return 0;
+}
+
+static int sensor_hub_reset_resume(struct hid_device *hdev)
+{
+	return 0;
+}
+#endif
+/*
+ * Handle raw report as sent by device
+ */
+static int sensor_hub_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	int i;
+	u8 *ptr;
+	int sz;
+	struct sensor_hub_data *pdata = hid_get_drvdata(hdev);
+	unsigned long flags;
+	struct hid_sensor_hub_callbacks *callback = NULL;
+	struct hid_collection *collection = NULL;
+	void *priv = NULL;
+
+	hid_dbg(hdev, "sensor_hub_raw_event report id:0x%x size:%d type:%d\n",
+			 report->id, size, report->type);
+	hid_dbg(hdev, "maxfield:%d\n", report->maxfield);
+	if (report->type != HID_INPUT_REPORT)
+		return 1;
+
+	ptr = raw_data;
+	ptr++; /*Skip report id*/
+
+	if (!report)
+		goto err_report;
+
+	spin_lock_irqsave(&pdata->lock, flags);
+
+	for (i = 0; i < report->maxfield; ++i) {
+
+		hid_dbg(hdev, "%d collection_index:%x hid:%x sz:%x\n",
+				i, report->field[i]->usage->collection_index,
+				report->field[i]->usage->hid,
+				report->field[i]->report_size/8);
+
+		sz = report->field[i]->report_size/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 = kmalloc(sz, GFP_ATOMIC);
+			if (pdata->pending.raw_data) {
+				memcpy(pdata->pending.raw_data, ptr, sz);
+				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",
+					collection->usage);
+		callback = sensor_hub_get_callback(pdata->hsdev->hdev,
+						report->field[i]->physical,
+							&priv);
+		if (callback && callback->capture_sample) {
+			if (report->field[i]->logical)
+				callback->capture_sample(pdata->hsdev,
+					report->field[i]->logical, sz, ptr,
+					callback->pdev);
+			else
+				callback->capture_sample(pdata->hsdev,
+					report->field[i]->usage->hid, sz, ptr,
+					callback->pdev);
+		}
+		ptr += sz;
+	}
+	if (callback && collection && callback->send_event)
+		callback->send_event(pdata->hsdev, collection->usage,
+				callback->pdev);
+	spin_unlock_irqrestore(&pdata->lock, flags);
+
+err_report:
+	return 1;
+}
+
+static int sensor_hub_probe(struct hid_device *hdev,
+				const struct hid_device_id *id)
+{
+	int ret;
+	struct sensor_hub_data *sd;
+	int i;
+	char *name;
+	struct hid_report *report;
+	struct hid_report_enum *report_enum;
+	struct hid_field *field;
+	int dev_cnt;
+
+	sd = kzalloc(sizeof(struct sensor_hub_data), GFP_KERNEL);
+	if (!sd) {
+		hid_err(hdev, "cannot allocate Sensor data\n");
+		return -ENOMEM;
+	}
+	sd->hsdev = kzalloc(sizeof(struct hid_sensor_hub_device), GFP_KERNEL);
+	if (!sd->hsdev) {
+		hid_err(hdev, "cannot allocate hid_sensor_hub_device\n");
+		ret = -ENOMEM;
+		goto err_free_hub;
+	}
+	hid_set_drvdata(hdev, sd);
+	sd->hsdev->hdev = hdev;
+	sd->hsdev->vendor_id = hdev->vendor;
+	sd->hsdev->product_id = hdev->product;
+	spin_lock_init(&sd->lock);
+	spin_lock_init(&sd->dyn_callback_lock);
+	mutex_init(&sd->mutex);
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "parse failed\n");
+		goto err_free;
+	}
+	if (sensor_hub_check_for_sensor_page(hdev) < 0) {
+		hid_err(hdev, "sensor page not found\n");
+		goto err_free;
+	}
+	INIT_LIST_HEAD(&hdev->inputs);
+
+	ret = hid_hw_start(hdev, 0);
+	if (ret) {
+		hid_err(hdev, "hw start failed\n");
+		goto err_free;
+	}
+	ret = hid_hw_open(hdev);
+	if (ret) {
+		hid_err(hdev, "failed to open input interrupt pipe\n");
+		goto err_stop_hw;
+	}
+
+	INIT_LIST_HEAD(&sd->dyn_callback_list);
+	sd->hid_sensor_client_cnt = 0;
+	report_enum = &hdev->report_enum[HID_INPUT_REPORT];
+
+	dev_cnt = sensor_hub_get_physical_device_count(report_enum);
+	if (dev_cnt > HID_MAX_PHY_DEVICES) {
+		hid_err(hdev, "Invalid Physical device count\n");
+		ret = -EINVAL;
+		goto err_close;
+	}
+	sd->hid_sensor_hub_client_devs = kzalloc(dev_cnt *
+						sizeof(struct mfd_cell),
+						GFP_KERNEL);
+	if (sd->hid_sensor_hub_client_devs == NULL) {
+		hid_err(hdev, "Failed to allocate memory for mfd cells\n");
+			ret = -ENOMEM;
+			goto err_close;
+	}
+	list_for_each_entry(report, &report_enum->report_list, list) {
+		hid_dbg(hdev, "Report id:%x\n", report->id);
+		field = report->field[0];
+		if (report->maxfield && field &&
+					field->physical) {
+			name = kasprintf(GFP_KERNEL, "HID-SENSOR-%x",
+						field->physical);
+			if (name  == NULL) {
+				hid_err(hdev, "Failed MFD device name\n");
+					ret = -ENOMEM;
+					goto err_free_names;
+			}
+			sd->hid_sensor_hub_client_devs[
+				sd->hid_sensor_client_cnt].name = name;
+			sd->hid_sensor_hub_client_devs[
+				sd->hid_sensor_client_cnt].platform_data =
+						sd->hsdev;
+			sd->hid_sensor_hub_client_devs[
+				sd->hid_sensor_client_cnt].pdata_size =
+						sizeof(*sd->hsdev);
+			hid_dbg(hdev, "Adding %s:%p\n", name, sd);
+			sd->hid_sensor_client_cnt++;
+		}
+	}
+	ret = mfd_add_devices(&hdev->dev, 0, sd->hid_sensor_hub_client_devs,
+		sd->hid_sensor_client_cnt, NULL, 0, NULL);
+	if (ret < 0)
+		goto err_free_names;
+
+	return ret;
+
+err_free_names:
+	for (i = 0; i < sd->hid_sensor_client_cnt ; ++i)
+		kfree(sd->hid_sensor_hub_client_devs[i].name);
+	kfree(sd->hid_sensor_hub_client_devs);
+err_close:
+	hid_hw_close(hdev);
+err_stop_hw:
+	hid_hw_stop(hdev);
+err_free:
+	kfree(sd->hsdev);
+err_free_hub:
+	kfree(sd);
+
+	return ret;
+}
+
+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);
+	spin_unlock_irqrestore(&data->lock, flags);
+	mfd_remove_devices(&hdev->dev);
+	for (i = 0; i < data->hid_sensor_client_cnt ; ++i)
+		kfree(data->hid_sensor_hub_client_devs[i].name);
+	kfree(data->hid_sensor_hub_client_devs);
+	hid_set_drvdata(hdev, NULL);
+	mutex_destroy(&data->mutex);
+	kfree(data->hsdev);
+	kfree(data);
+}
+
+static const struct hid_device_id sensor_hub_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8086,
+			USB_DEVICE_ID_SENSOR_HUB_1020) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8087,
+			USB_DEVICE_ID_SENSOR_HUB_1020) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8086,
+			USB_DEVICE_ID_SENSOR_HUB_09FA) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_INTEL_8087,
+			USB_DEVICE_ID_SENSOR_HUB_09FA) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_STM,
+			USB_DEVICE_ID_SENSOR_HUB_7014) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, sensor_hub_devices);
+
+static const struct hid_usage_id sensor_hub_grabbed_usages[] = {
+	{ HID_ANY_ID, HID_ANY_ID, HID_ANY_ID },
+	{ HID_ANY_ID - 1, HID_ANY_ID - 1, HID_ANY_ID - 1 }
+};
+
+static struct hid_driver sensor_hub_driver = {
+	.name = "hid-sensor-hub",
+	.id_table = sensor_hub_devices,
+	.probe = sensor_hub_probe,
+	.remove = sensor_hub_remove,
+	.raw_event = sensor_hub_raw_event,
+#ifdef CONFIG_PM
+	.suspend = sensor_hub_suspend,
+	.resume =  sensor_hub_resume,
+	.reset_resume =  sensor_hub_reset_resume,
+#endif
+};
+
+static int __init sensor_hub_init(void)
+{
+	return hid_register_driver(&sensor_hub_driver);
+}
+
+static void __exit sensor_hub_exit(void)
+{
+	hid_unregister_driver(&sensor_hub_driver);
+}
+
+module_init(sensor_hub_init);
+module_exit(sensor_hub_exit);
+
+MODULE_DESCRIPTION("HID Sensor Hub driver");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 5cd25bd907f8..7f33ebf299c2 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -4,7 +4,6 @@
  *  Copyright (c) 1999 Andreas Gal
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  *  Copyright (c) 2006-2008 Jiri Kosina
  */
diff --git a/drivers/hid/hid-sunplus.c b/drivers/hid/hid-sunplus.c
index d484a0043dd4..45b4b066a262 100644
--- a/drivers/hid/hid-sunplus.c
+++ b/drivers/hid/hid-sunplus.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby
  */
 
diff --git a/drivers/hid/hid-uclogic.c b/drivers/hid/hid-uclogic.c
index 3aba02be1f26..2e56a1fd2375 100644
--- a/drivers/hid/hid-uclogic.c
+++ b/drivers/hid/hid-uclogic.c
@@ -466,6 +466,86 @@ static __u8 twhl850_rdesc_fixed2[] = {
 	0xC0                /*  End Collection                      */
 };
 
+/*
+ * See TWHA60 description, device and HID report descriptors at
+ * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Tablet_TWHA60
+ */
+
+/* Size of the original descriptors of TWHA60 tablet */
+#define TWHA60_RDESC_ORIG_SIZE0 254
+#define TWHA60_RDESC_ORIG_SIZE1 139
+
+/* Fixed TWHA60 report descriptor, interface 0 (stylus) */
+static __u8 twha60_rdesc_fixed0[] = {
+	0x05, 0x0D,         /*  Usage Page (Digitizer),             */
+	0x09, 0x02,         /*  Usage (Pen),                        */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x09,         /*      Report ID (9),                  */
+	0x09, 0x20,         /*      Usage (Stylus),                 */
+	0xA0,               /*      Collection (Physical),          */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x09, 0x42,         /*          Usage (Tip Switch),         */
+	0x09, 0x44,         /*          Usage (Barrel Switch),      */
+	0x09, 0x46,         /*          Usage (Tablet Pick),        */
+	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),           */
+	0x09, 0x32,         /*          Usage (In Range),           */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x14,               /*          Logical Minimum (0),        */
+	0xA4,               /*          Push,                       */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x55, 0xFD,         /*          Unit Exponent (-3),         */
+	0x65, 0x13,         /*          Unit (Inch),                */
+	0x34,               /*          Physical Minimum (0),       */
+	0x09, 0x30,         /*          Usage (X),                  */
+	0x46, 0x10, 0x27,   /*          Physical Maximum (10000),   */
+	0x27, 0x3F, 0x9C,
+		0x00, 0x00, /*          Logical Maximum (39999),    */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x09, 0x31,         /*          Usage (Y),                  */
+	0x46, 0x6A, 0x18,   /*          Physical Maximum (6250),    */
+	0x26, 0xA7, 0x61,   /*          Logical Maximum (24999),    */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xB4,               /*          Pop,                        */
+	0x09, 0x30,         /*          Usage (Tip Pressure),       */
+	0x26, 0xFF, 0x03,   /*          Logical Maximum (1023),     */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xC0,               /*      End Collection,                 */
+	0xC0                /*  End Collection                      */
+};
+
+/* Fixed TWHA60 report descriptor, interface 1 (frame buttons) */
+static __u8 twha60_rdesc_fixed1[] = {
+	0x05, 0x01, /*  Usage Page (Desktop),       */
+	0x09, 0x06, /*  Usage (Keyboard),           */
+	0xA1, 0x01, /*  Collection (Application),   */
+	0x85, 0x05, /*      Report ID (5),          */
+	0x05, 0x07, /*      Usage Page (Keyboard),  */
+	0x14,       /*      Logical Minimum (0),    */
+	0x25, 0x01, /*      Logical Maximum (1),    */
+	0x75, 0x01, /*      Report Size (1),        */
+	0x95, 0x08, /*      Report Count (8),       */
+	0x81, 0x01, /*      Input (Constant),       */
+	0x95, 0x0C, /*      Report Count (12),      */
+	0x19, 0x3A, /*      Usage Minimum (KB F1),  */
+	0x29, 0x45, /*      Usage Maximum (KB F12), */
+	0x81, 0x02, /*      Input (Variable),       */
+	0x95, 0x0C, /*      Report Count (12),      */
+	0x19, 0x68, /*      Usage Minimum (KB F13), */
+	0x29, 0x73, /*      Usage Maximum (KB F24), */
+	0x81, 0x02, /*      Input (Variable),       */
+	0x95, 0x08, /*      Report Count (8),       */
+	0x81, 0x01, /*      Input (Constant),       */
+	0xC0        /*  End Collection              */
+};
+
 static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 					unsigned int *rsize)
 {
@@ -525,6 +605,22 @@ static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			break;
 		}
 		break;
+	case USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60:
+		switch (iface_num) {
+		case 0:
+			if (*rsize == TWHA60_RDESC_ORIG_SIZE0) {
+				rdesc = twha60_rdesc_fixed0;
+				*rsize = sizeof(twha60_rdesc_fixed0);
+			}
+			break;
+		case 1:
+			if (*rsize == TWHA60_RDESC_ORIG_SIZE1) {
+				rdesc = twha60_rdesc_fixed1;
+				*rsize = sizeof(twha60_rdesc_fixed1);
+			}
+			break;
+		}
+		break;
 	}
 
 	return rdesc;
@@ -543,6 +639,8 @@ static const struct hid_device_id uclogic_devices[] = {
 				USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
 				USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
+				USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, uclogic_devices);
diff --git a/drivers/hid/hid-wacom.c b/drivers/hid/hid-wacom.c
index fe23a1eb586b..2f60da9ed066 100644
--- a/drivers/hid/hid-wacom.c
+++ b/drivers/hid/hid-wacom.c
@@ -5,7 +5,6 @@
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
  *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
  *  Copyright (c) 2006-2007 Jiri Kosina
- *  Copyright (c) 2007 Paul Walmsley
  *  Copyright (c) 2008 Jiri Slaby <jirislaby@gmail.com>
  *  Copyright (c) 2006 Andrew Zabolotny <zap@homelink.ru>
  *  Copyright (c) 2009 Bastien Nocera <hadess@hadess.net>
@@ -33,6 +32,8 @@
 #define PAD_DEVICE_ID	0x0F
 
 #define WAC_CMD_LED_CONTROL     0x20
+#define WAC_CMD_ICON_START_STOP     0x21
+#define WAC_CMD_ICON_TRANSFER       0x26
 
 struct wacom_data {
 	__u16 tool;
@@ -69,6 +70,91 @@ static enum power_supply_property wacom_ac_props[] = {
 	POWER_SUPPLY_PROP_SCOPE,
 };
 
+static void wacom_scramble(__u8 *image)
+{
+	__u16 mask;
+	__u16 s1;
+	__u16 s2;
+	__u16 r1 ;
+	__u16 r2 ;
+	__u16 r;
+	__u8 buf[256];
+	int i, w, x, y, z;
+
+	for (x = 0; x < 32; x++) {
+		for (y = 0; y < 8; y++)
+			buf[(8 * x) + (7 - y)] = image[(8 * x) + y];
+	}
+
+	/* Change 76543210 into GECA6420 as required by Intuos4 WL
+	 *        HGFEDCBA      HFDB7531
+	 */
+	for (x = 0; x < 4; x++) {
+		for (y = 0; y < 4; y++) {
+			for (z = 0; z < 8; z++) {
+				mask = 0x0001;
+				r1 = 0;
+				r2 = 0;
+				i = (x << 6) + (y << 4) + z;
+				s1 = buf[i];
+				s2 = buf[i+8];
+				for (w = 0; w < 8; w++) {
+					r1 |= (s1 & mask);
+					r2 |= (s2 & mask);
+					s1 <<= 1;
+					s2 <<= 1;
+					mask <<= 2;
+				}
+				r = r1 | (r2 << 1);
+				i = (x << 6) + (y << 4) + (z << 1);
+				image[i] = 0xFF & r;
+				image[i+1] = (0xFF00 & r) >> 8;
+			}
+		}
+	}
+}
+
+static void wacom_set_image(struct hid_device *hdev, const char *image,
+						__u8 icon_no)
+{
+	__u8 rep_data[68];
+	__u8 p[256];
+	int ret, i, j;
+
+	for (i = 0; i < 256; i++)
+		p[i] = image[i];
+
+	rep_data[0] = WAC_CMD_ICON_START_STOP;
+	rep_data[1] = 0;
+	ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
+				HID_FEATURE_REPORT);
+	if (ret < 0)
+		goto err;
+
+	rep_data[0] = WAC_CMD_ICON_TRANSFER;
+	rep_data[1] = icon_no & 0x07;
+
+	wacom_scramble(p);
+
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 64; j++)
+			rep_data[j + 3] = p[(i << 6) + j];
+
+		rep_data[2] = i;
+		ret = hdev->hid_output_raw_report(hdev, rep_data, 67,
+					HID_FEATURE_REPORT);
+	}
+
+	rep_data[0] = WAC_CMD_ICON_START_STOP;
+	rep_data[1] = 0;
+
+	ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
+				HID_FEATURE_REPORT);
+
+err:
+	return;
+}
+
 static void wacom_leds_set_brightness(struct led_classdev *led_dev,
 						enum led_brightness value)
 {
@@ -91,7 +177,10 @@ static void wacom_leds_set_brightness(struct led_classdev *led_dev,
 	if (buf) {
 		buf[0] = WAC_CMD_LED_CONTROL;
 		buf[1] = led;
-		buf[2] = value;
+		buf[2] = value >> 2;
+		buf[3] = value;
+		/* use fixed brightness for OLEDs */
+		buf[4] = 0x08;
 		hdev->hid_output_raw_report(hdev, buf, 9, HID_FEATURE_REPORT);
 		kfree(buf);
 	}
@@ -317,6 +406,34 @@ static ssize_t wacom_store_speed(struct device *dev,
 static DEVICE_ATTR(speed, S_IRUGO | S_IWUSR | S_IWGRP,
 		wacom_show_speed, wacom_store_speed);
 
+#define WACOM_STORE(OLED_ID)						\
+static ssize_t wacom_oled##OLED_ID##_store(struct device *dev,		\
+				struct device_attribute *attr,		\
+				const char *buf, size_t count)		\
+{									\
+	struct hid_device *hdev = container_of(dev, struct hid_device,	\
+				dev);					\
+									\
+	if (count != 256)						\
+		return -EINVAL;						\
+									\
+	wacom_set_image(hdev, buf, OLED_ID);				\
+									\
+	return count;							\
+}									\
+									\
+static DEVICE_ATTR(oled##OLED_ID##_img, S_IWUSR | S_IWGRP, NULL,	\
+				wacom_oled##OLED_ID##_store)
+
+WACOM_STORE(0);
+WACOM_STORE(1);
+WACOM_STORE(2);
+WACOM_STORE(3);
+WACOM_STORE(4);
+WACOM_STORE(5);
+WACOM_STORE(6);
+WACOM_STORE(7);
+
 static int wacom_gr_parse_report(struct hid_device *hdev,
 			struct wacom_data *wdata,
 			struct input_dev *input, unsigned char *data)
@@ -717,17 +834,33 @@ static int wacom_probe(struct hid_device *hdev,
 		hid_warn(hdev,
 			 "can't create sysfs speed attribute err: %d\n", ret);
 
+#define OLED_INIT(OLED_ID)						\
+	do {								\
+		ret = device_create_file(&hdev->dev,			\
+				&dev_attr_oled##OLED_ID##_img);		\
+		if (ret)						\
+			hid_warn(hdev,					\
+			 "can't create sysfs oled attribute, err: %d\n", ret);\
+	} while (0)
+
+OLED_INIT(0);
+OLED_INIT(1);
+OLED_INIT(2);
+OLED_INIT(3);
+OLED_INIT(4);
+OLED_INIT(5);
+OLED_INIT(6);
+OLED_INIT(7);
+
 	wdata->features = 0;
 	wacom_set_features(hdev, 1);
 
 	if (hdev->product == USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH) {
 		sprintf(hdev->name, "%s", "Wacom Intuos4 WL");
 		ret = wacom_initialize_leds(hdev);
-		if (ret) {
+		if (ret)
 			hid_warn(hdev,
 				 "can't create led attribute, err: %d\n", ret);
-			goto destroy_leds;
-		}
 	}
 
 	wdata->battery.properties = wacom_battery_props;
@@ -740,8 +873,8 @@ static int wacom_probe(struct hid_device *hdev,
 
 	ret = power_supply_register(&hdev->dev, &wdata->battery);
 	if (ret) {
-		hid_warn(hdev, "can't create sysfs battery attribute, err: %d\n",
-			 ret);
+		hid_err(hdev, "can't create sysfs battery attribute, err: %d\n",
+			ret);
 		goto err_battery;
 	}
 
@@ -756,8 +889,8 @@ static int wacom_probe(struct hid_device *hdev,
 
 	ret = power_supply_register(&hdev->dev, &wdata->ac);
 	if (ret) {
-		hid_warn(hdev,
-			 "can't create ac battery attribute, err: %d\n", ret);
+		hid_err(hdev,
+			"can't create ac battery attribute, err: %d\n", ret);
 		goto err_ac;
 	}
 
@@ -767,10 +900,17 @@ static int wacom_probe(struct hid_device *hdev,
 err_ac:
 	power_supply_unregister(&wdata->battery);
 err_battery:
+	wacom_destroy_leds(hdev);
+	device_remove_file(&hdev->dev, &dev_attr_oled0_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled1_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled2_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled3_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled4_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled5_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled6_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled7_img);
 	device_remove_file(&hdev->dev, &dev_attr_speed);
 	hid_hw_stop(hdev);
-destroy_leds:
-	wacom_destroy_leds(hdev);
 err_free:
 	kfree(wdata);
 	return ret;
@@ -781,6 +921,14 @@ static void wacom_remove(struct hid_device *hdev)
 	struct wacom_data *wdata = hid_get_drvdata(hdev);
 
 	wacom_destroy_leds(hdev);
+	device_remove_file(&hdev->dev, &dev_attr_oled0_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled1_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled2_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled3_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled4_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled5_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled6_img);
+	device_remove_file(&hdev->dev, &dev_attr_oled7_img);
 	device_remove_file(&hdev->dev, &dev_attr_speed);
 	hid_hw_stop(hdev);
 
diff --git a/drivers/hid/hid-waltop.c b/drivers/hid/hid-waltop.c
index 745e4e9a8cf2..bb536ab5941e 100644
--- a/drivers/hid/hid-waltop.c
+++ b/drivers/hid/hid-waltop.c
@@ -638,28 +638,6 @@ static __u8 sirius_battery_free_tablet_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-static int waltop_probe(struct hid_device *hdev,
-			const struct hid_device_id *id)
-{
-	int ret;
-
-	ret = hid_parse(hdev);
-	if (ret) {
-		hid_err(hdev, "parse failed\n");
-		goto err;
-	}
-
-	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
-	if (ret) {
-		hid_err(hdev, "hw start failed\n");
-		goto err;
-	}
-
-	return 0;
-err:
-	return ret;
-}
-
 static __u8 *waltop_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
@@ -776,11 +754,6 @@ static int waltop_raw_event(struct hid_device *hdev, struct hid_report *report,
 	return 0;
 }
 
-static void waltop_remove(struct hid_device *hdev)
-{
-	hid_hw_stop(hdev);
-}
-
 static const struct hid_device_id waltop_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP,
 				USB_DEVICE_ID_WALTOP_SLIM_TABLET_5_8_INCH) },
@@ -803,10 +776,8 @@ MODULE_DEVICE_TABLE(hid, waltop_devices);
 static struct hid_driver waltop_driver = {
 	.name = "waltop",
 	.id_table = waltop_devices,
-	.probe = waltop_probe,
 	.report_fixup = waltop_report_fixup,
 	.raw_event = waltop_raw_event,
-	.remove = waltop_remove,
 };
 
 static int __init waltop_init(void)
diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index 0a1805c9b0e5..bc85bf29062e 100644
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -28,12 +28,14 @@ struct wiimote_ext {
 	bool mp_plugged;
 	bool motionp;
 	__u8 ext_type;
+	__u16 calib[4][3];
 };
 
 enum wiiext_type {
 	WIIEXT_NONE,		/* placeholder */
 	WIIEXT_CLASSIC,		/* Nintendo classic controller */
 	WIIEXT_NUNCHUCK,	/* Nintendo nunchuck controller */
+	WIIEXT_BALANCE_BOARD,	/* Nintendo balance board controller */
 };
 
 enum wiiext_keys {
@@ -126,6 +128,7 @@ error:
 static __u8 ext_read(struct wiimote_ext *ext)
 {
 	ssize_t ret;
+	__u8 buf[24], i, j, offs = 0;
 	__u8 rmem[2], wmem;
 	__u8 type = WIIEXT_NONE;
 
@@ -151,6 +154,28 @@ static __u8 ext_read(struct wiimote_ext *ext)
 			type = WIIEXT_NUNCHUCK;
 		else if (rmem[0] == 0x01 && rmem[1] == 0x01)
 			type = WIIEXT_CLASSIC;
+		else if (rmem[0] == 0x04 && rmem[1] == 0x02)
+			type = WIIEXT_BALANCE_BOARD;
+	}
+
+	/* get balance board calibration data */
+	if (type == WIIEXT_BALANCE_BOARD) {
+		ret = wiimote_cmd_read(ext->wdata, 0xa40024, buf, 12);
+		ret += wiimote_cmd_read(ext->wdata, 0xa40024 + 12,
+					buf + 12, 12);
+
+		if (ret != 24) {
+			type = WIIEXT_NONE;
+		} else {
+			for (i = 0; i < 3; i++) {
+				for (j = 0; j < 4; j++) {
+					ext->calib[j][i] = buf[offs];
+					ext->calib[j][i] <<= 8;
+					ext->calib[j][i] |= buf[offs + 1];
+					offs += 2;
+				}
+			}
+		}
 	}
 
 	wiimote_cmd_release(ext->wdata);
@@ -509,6 +534,71 @@ static void handler_classic(struct wiimote_ext *ext, const __u8 *payload)
 	input_sync(ext->input);
 }
 
+static void handler_balance_board(struct wiimote_ext *ext, const __u8 *payload)
+{
+	__s32 val[4], tmp;
+	unsigned int i;
+
+	/*   Byte |  8  7  6  5  4  3  2  1  |
+	 *   -----+--------------------------+
+	 *    1   |    Top Right <15:8>      |
+	 *    2   |    Top Right  <7:0>      |
+	 *   -----+--------------------------+
+	 *    3   | Bottom Right <15:8>      |
+	 *    4   | Bottom Right  <7:0>      |
+	 *   -----+--------------------------+
+	 *    5   |     Top Left <15:8>      |
+	 *    6   |     Top Left  <7:0>      |
+	 *   -----+--------------------------+
+	 *    7   |  Bottom Left <15:8>      |
+	 *    8   |  Bottom Left  <7:0>      |
+	 *   -----+--------------------------+
+	 *
+	 * These values represent the weight-measurements of the Wii-balance
+	 * board with 16bit precision.
+	 *
+	 * The balance-board is never reported interleaved with motionp.
+	 */
+
+	val[0] = payload[0];
+	val[0] <<= 8;
+	val[0] |= payload[1];
+
+	val[1] = payload[2];
+	val[1] <<= 8;
+	val[1] |= payload[3];
+
+	val[2] = payload[4];
+	val[2] <<= 8;
+	val[2] |= payload[5];
+
+	val[3] = payload[6];
+	val[3] <<= 8;
+	val[3] |= payload[7];
+
+	/* apply calibration data */
+	for (i = 0; i < 4; i++) {
+		if (val[i] < ext->calib[i][1]) {
+			tmp = val[i] - ext->calib[i][0];
+			tmp *= 1700;
+			tmp /= ext->calib[i][1] - ext->calib[i][0];
+		} else {
+			tmp = val[i] - ext->calib[i][1];
+			tmp *= 1700;
+			tmp /= ext->calib[i][2] - ext->calib[i][1];
+			tmp += 1700;
+		}
+		val[i] = tmp;
+	}
+
+	input_report_abs(ext->input, ABS_HAT0X, val[0]);
+	input_report_abs(ext->input, ABS_HAT0Y, val[1]);
+	input_report_abs(ext->input, ABS_HAT1X, val[2]);
+	input_report_abs(ext->input, ABS_HAT1Y, val[3]);
+
+	input_sync(ext->input);
+}
+
 /* call this with state.lock spinlock held */
 void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload)
 {
@@ -523,6 +613,8 @@ void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload)
 		handler_nunchuck(ext, payload);
 	} else if (ext->ext_type == WIIEXT_CLASSIC) {
 		handler_classic(ext, payload);
+	} else if (ext->ext_type == WIIEXT_BALANCE_BOARD) {
+		handler_balance_board(ext, payload);
 	}
 }
 
@@ -551,6 +643,11 @@ static ssize_t wiiext_show(struct device *dev, struct device_attribute *attr,
 			return sprintf(buf, "motionp+classic\n");
 		else
 			return sprintf(buf, "classic\n");
+	} else if (type == WIIEXT_BALANCE_BOARD) {
+		if (motionp)
+			return sprintf(buf, "motionp+balanceboard\n");
+		else
+			return sprintf(buf, "balanceboard\n");
 	} else {
 		if (motionp)
 			return sprintf(buf, "motionp\n");
diff --git a/drivers/hid/hidraw.c b/drivers/hid/hidraw.c
index 3b6f7bf5a77e..17d15bb610d1 100644
--- a/drivers/hid/hidraw.c
+++ b/drivers/hid/hidraw.c
@@ -42,6 +42,7 @@ static struct cdev hidraw_cdev;
 static struct class *hidraw_class;
 static struct hidraw *hidraw_table[HIDRAW_MAX_DEVICES];
 static DEFINE_MUTEX(minors_lock);
+static void drop_ref(struct hidraw *hid, int exists_bit);
 
 static ssize_t hidraw_read(struct file *file, char __user *buffer, size_t count, loff_t *ppos)
 {
@@ -113,7 +114,7 @@ static ssize_t hidraw_send_report(struct file *file, const char __user *buffer,
 	__u8 *buf;
 	int ret = 0;
 
-	if (!hidraw_table[minor]) {
+	if (!hidraw_table[minor] || !hidraw_table[minor]->exist) {
 		ret = -ENODEV;
 		goto out;
 	}
@@ -261,7 +262,7 @@ static int hidraw_open(struct inode *inode, struct file *file)
 	}
 
 	mutex_lock(&minors_lock);
-	if (!hidraw_table[minor]) {
+	if (!hidraw_table[minor] || !hidraw_table[minor]->exist) {
 		err = -ENODEV;
 		goto out_unlock;
 	}
@@ -298,36 +299,12 @@ out:
 static int hidraw_release(struct inode * inode, struct file * file)
 {
 	unsigned int minor = iminor(inode);
-	struct hidraw *dev;
 	struct hidraw_list *list = file->private_data;
-	int ret;
-	int i;
-
-	mutex_lock(&minors_lock);
-	if (!hidraw_table[minor]) {
-		ret = -ENODEV;
-		goto unlock;
-	}
 
+	drop_ref(hidraw_table[minor], 0);
 	list_del(&list->node);
-	dev = hidraw_table[minor];
-	if (!--dev->open) {
-		if (list->hidraw->exist) {
-			hid_hw_power(dev->hid, PM_HINT_NORMAL);
-			hid_hw_close(dev->hid);
-		} else {
-			kfree(list->hidraw);
-		}
-	}
-
-	for (i = 0; i < HIDRAW_BUFFER_SIZE; ++i)
-		kfree(list->buffer[i].value);
 	kfree(list);
-	ret = 0;
-unlock:
-	mutex_unlock(&minors_lock);
-
-	return ret;
+	return 0;
 }
 
 static long hidraw_ioctl(struct file *file, unsigned int cmd,
@@ -529,21 +506,7 @@ EXPORT_SYMBOL_GPL(hidraw_connect);
 void hidraw_disconnect(struct hid_device *hid)
 {
 	struct hidraw *hidraw = hid->hidraw;
-
-	mutex_lock(&minors_lock);
-	hidraw->exist = 0;
-
-	device_destroy(hidraw_class, MKDEV(hidraw_major, hidraw->minor));
-
-	hidraw_table[hidraw->minor] = NULL;
-
-	if (hidraw->open) {
-		hid_hw_close(hid);
-		wake_up_interruptible(&hidraw->wait);
-	} else {
-		kfree(hidraw);
-	}
-	mutex_unlock(&minors_lock);
+	drop_ref(hidraw, 1);
 }
 EXPORT_SYMBOL_GPL(hidraw_disconnect);
 
@@ -559,21 +522,28 @@ int __init hidraw_init(void)
 
 	if (result < 0) {
 		pr_warn("can't get major number\n");
-		result = 0;
 		goto out;
 	}
 
 	hidraw_class = class_create(THIS_MODULE, "hidraw");
 	if (IS_ERR(hidraw_class)) {
 		result = PTR_ERR(hidraw_class);
-		unregister_chrdev(hidraw_major, "hidraw");
-		goto out;
+		goto error_cdev;
 	}
 
         cdev_init(&hidraw_cdev, &hidraw_ops);
-        cdev_add(&hidraw_cdev, dev_id, HIDRAW_MAX_DEVICES);
+	result = cdev_add(&hidraw_cdev, dev_id, HIDRAW_MAX_DEVICES);
+	if (result < 0)
+		goto error_class;
+
 out:
 	return result;
+
+error_class:
+	class_destroy(hidraw_class);
+error_cdev:
+	unregister_chrdev_region(dev_id, HIDRAW_MAX_DEVICES);
+	goto out;
 }
 
 void hidraw_exit(void)
@@ -585,3 +555,23 @@ void hidraw_exit(void)
 	unregister_chrdev_region(dev_id, HIDRAW_MAX_DEVICES);
 
 }
+
+static void drop_ref(struct hidraw *hidraw, int exists_bit)
+{
+	mutex_lock(&minors_lock);
+	if (exists_bit) {
+		hid_hw_close(hidraw->hid);
+		hidraw->exist = 0;
+		if (hidraw->open)
+			wake_up_interruptible(&hidraw->wait);
+	} else {
+		--hidraw->open;
+	}
+
+	if (!hidraw->open && !hidraw->exist) {
+		device_destroy(hidraw_class, MKDEV(hidraw_major, hidraw->minor));
+		hidraw_table[hidraw->minor] = NULL;
+		kfree(hidraw);
+	}
+	mutex_unlock(&minors_lock);
+}
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index dedd8e4e5c6d..8e0c4bf94ebc 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -1415,20 +1415,20 @@ static int hid_post_reset(struct usb_interface *intf)
 	 * configuration descriptors passed, we already know that
 	 * the size of the HID report descriptor has not changed.
 	 */
-	rdesc = kmalloc(hid->rsize, GFP_KERNEL);
+	rdesc = kmalloc(hid->dev_rsize, GFP_KERNEL);
 	if (!rdesc) {
 		dbg_hid("couldn't allocate rdesc memory (post_reset)\n");
 		return 1;
 	}
 	status = hid_get_class_descriptor(dev,
 				interface->desc.bInterfaceNumber,
-				HID_DT_REPORT, rdesc, hid->rsize);
+				HID_DT_REPORT, rdesc, hid->dev_rsize);
 	if (status < 0) {
 		dbg_hid("reading report descriptor failed (post_reset)\n");
 		kfree(rdesc);
 		return 1;
 	}
-	status = memcmp(rdesc, hid->rdesc, hid->rsize);
+	status = memcmp(rdesc, hid->dev_rdesc, hid->dev_rsize);
 	kfree(rdesc);
 	if (status != 0) {
 		dbg_hid("report descriptor changed\n");
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 991e85c7325c..11c7932dc7e6 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -70,12 +70,13 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_AXIS_295, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2700, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_FREESCALE, USB_DEVICE_ID_FREESCALE_MX28, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_MGE, USB_DEVICE_ID_MGE_UPS, HID_QUIRK_NOGET },
 	{ 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_PRODIGE, USB_DEVICE_ID_PRODIGE_CORDLESS, HID_QUIRK_NOGET },
-	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN, 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 },
 	{ USB_VENDOR_ID_SENNHEISER, USB_DEVICE_ID_SENNHEISER_BTD500USB, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SUN, USB_DEVICE_ID_RARITAN_KVM_DONGLE, HID_QUIRK_NOGET },
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index 86f8885aeb45..3648f8f0f368 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -26,6 +26,7 @@
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
 #include <linux/hyperv.h>
+#include <linux/version.h>
 #include <asm/hyperv.h>
 #include "hyperv_vmbus.h"
 
@@ -38,28 +39,6 @@ struct hv_context hv_context = {
 };
 
 /*
- * query_hypervisor_presence
- * - Query the cpuid for presence of windows hypervisor
- */
-static int query_hypervisor_presence(void)
-{
-	unsigned int eax;
-	unsigned int ebx;
-	unsigned int ecx;
-	unsigned int edx;
-	unsigned int op;
-
-	eax = 0;
-	ebx = 0;
-	ecx = 0;
-	edx = 0;
-	op = HVCPUID_VERSION_FEATURES;
-	cpuid(op, &eax, &ebx, &ecx, &edx);
-
-	return ecx & HV_PRESENT_BIT;
-}
-
-/*
  * query_hypervisor_info - Get version info of the windows hypervisor
  */
 static int query_hypervisor_info(void)
@@ -159,14 +138,13 @@ int hv_init(void)
 	memset(hv_context.synic_message_page, 0,
 	       sizeof(void *) * NR_CPUS);
 
-	if (!query_hypervisor_presence())
-		goto cleanup;
-
 	max_leaf = query_hypervisor_info();
 
-	/* Write our OS info */
-	wrmsrl(HV_X64_MSR_GUEST_OS_ID, HV_LINUX_GUEST_ID);
-	hv_context.guestid = HV_LINUX_GUEST_ID;
+	/*
+	 * Write our OS ID.
+	 */
+	hv_context.guestid = generate_guest_id(0, LINUX_VERSION_CODE, 0);
+	wrmsrl(HV_X64_MSR_GUEST_OS_ID, hv_context.guestid);
 
 	/* See if the hypercall page is already set */
 	rdmsrl(HV_X64_MSR_HYPERCALL, hypercall_msr.as_uint64);
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index 0012eed6d872..ed50e9e83c61 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -48,13 +48,24 @@ static struct {
 	void *kvp_context; /* for the channel callback */
 } kvp_transaction;
 
+/*
+ * Before we can accept KVP messages from the host, we need
+ * to handshake with the user level daemon. This state tracks
+ * if we are in the handshake phase.
+ */
+static bool in_hand_shake = true;
+
+/*
+ * This state maintains the version number registered by the daemon.
+ */
+static int dm_reg_value;
+
 static void kvp_send_key(struct work_struct *dummy);
 
-#define TIMEOUT_FIRED 1
 
-static void kvp_respond_to_host(char *key, char *value, int error);
+static void kvp_respond_to_host(struct hv_kvp_msg *msg, int error);
 static void kvp_work_func(struct work_struct *dummy);
-static void kvp_register(void);
+static void kvp_register(int);
 
 static DECLARE_DELAYED_WORK(kvp_work, kvp_work_func);
 static DECLARE_WORK(kvp_sendkey_work, kvp_send_key);
@@ -68,7 +79,7 @@ static u8 *recv_buffer;
  */
 
 static void
-kvp_register(void)
+kvp_register(int reg_value)
 {
 
 	struct cn_msg *msg;
@@ -83,7 +94,7 @@ kvp_register(void)
 		msg->id.idx =  CN_KVP_IDX;
 		msg->id.val = CN_KVP_VAL;
 
-		kvp_msg->kvp_hdr.operation = KVP_OP_REGISTER;
+		kvp_msg->kvp_hdr.operation = reg_value;
 		strcpy(version, HV_DRV_VERSION);
 		msg->len = sizeof(struct hv_kvp_msg);
 		cn_netlink_send(msg, 0, GFP_ATOMIC);
@@ -97,9 +108,43 @@ kvp_work_func(struct work_struct *dummy)
 	 * If the timer fires, the user-mode component has not responded;
 	 * process the pending transaction.
 	 */
-	kvp_respond_to_host("Unknown key", "Guest timed out", TIMEOUT_FIRED);
+	kvp_respond_to_host(NULL, HV_E_FAIL);
+}
+
+static int kvp_handle_handshake(struct hv_kvp_msg *msg)
+{
+	int ret = 1;
+
+	switch (msg->kvp_hdr.operation) {
+	case KVP_OP_REGISTER:
+		dm_reg_value = KVP_OP_REGISTER;
+		pr_info("KVP: IP injection functionality not available\n");
+		pr_info("KVP: Upgrade the KVP daemon\n");
+		break;
+	case KVP_OP_REGISTER1:
+		dm_reg_value = KVP_OP_REGISTER1;
+		break;
+	default:
+		pr_info("KVP: incompatible daemon\n");
+		pr_info("KVP: KVP version: %d, Daemon version: %d\n",
+			KVP_OP_REGISTER1, msg->kvp_hdr.operation);
+		ret = 0;
+	}
+
+	if (ret) {
+		/*
+		 * We have a compatible daemon; complete the handshake.
+		 */
+		pr_info("KVP: user-mode registering done.\n");
+		kvp_register(dm_reg_value);
+		kvp_transaction.active = false;
+		if (kvp_transaction.kvp_context)
+			hv_kvp_onchannelcallback(kvp_transaction.kvp_context);
+	}
+	return ret;
 }
 
+
 /*
  * Callback when data is received from user mode.
  */
@@ -109,29 +154,165 @@ kvp_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 {
 	struct hv_kvp_msg *message;
 	struct hv_kvp_msg_enumerate *data;
+	int	error = 0;
 
 	message = (struct hv_kvp_msg *)msg->data;
-	switch (message->kvp_hdr.operation) {
+
+	/*
+	 * If we are negotiating the version information
+	 * with the daemon; handle that first.
+	 */
+
+	if (in_hand_shake) {
+		if (kvp_handle_handshake(message))
+			in_hand_shake = false;
+		return;
+	}
+
+	/*
+	 * Based on the version of the daemon, we propagate errors from the
+	 * daemon differently.
+	 */
+
+	data = &message->body.kvp_enum_data;
+
+	switch (dm_reg_value) {
 	case KVP_OP_REGISTER:
-		pr_info("KVP: user-mode registering done.\n");
-		kvp_register();
-		kvp_transaction.active = false;
-		hv_kvp_onchannelcallback(kvp_transaction.kvp_context);
+		/*
+		 * Null string is used to pass back error condition.
+		 */
+		if (data->data.key[0] == 0)
+			error = HV_S_CONT;
 		break;
 
-	default:
-		data = &message->body.kvp_enum_data;
+	case KVP_OP_REGISTER1:
 		/*
-		 * Complete the transaction by forwarding the key value
-		 * to the host. But first, cancel the timeout.
+		 * We use the message header information from
+		 * the user level daemon to transmit errors.
 		 */
-		if (cancel_delayed_work_sync(&kvp_work))
-			kvp_respond_to_host(data->data.key,
-					 data->data.value,
-					!strlen(data->data.key));
+		error = message->error;
+		break;
+	}
+
+	/*
+	 * Complete the transaction by forwarding the key value
+	 * to the host. But first, cancel the timeout.
+	 */
+	if (cancel_delayed_work_sync(&kvp_work))
+		kvp_respond_to_host(message, error);
+}
+
+
+static int process_ob_ipinfo(void *in_msg, void *out_msg, int op)
+{
+	struct hv_kvp_msg *in = in_msg;
+	struct hv_kvp_ip_msg *out = out_msg;
+	int len;
+
+	switch (op) {
+	case KVP_OP_GET_IP_INFO:
+		/*
+		 * Transform all parameters into utf16 encoding.
+		 */
+		len = utf8s_to_utf16s((char *)in->body.kvp_ip_val.ip_addr,
+				strlen((char *)in->body.kvp_ip_val.ip_addr),
+				UTF16_HOST_ENDIAN,
+				(wchar_t *)out->kvp_ip_val.ip_addr,
+				MAX_IP_ADDR_SIZE);
+		if (len < 0)
+			return len;
+
+		len = utf8s_to_utf16s((char *)in->body.kvp_ip_val.sub_net,
+				strlen((char *)in->body.kvp_ip_val.sub_net),
+				UTF16_HOST_ENDIAN,
+				(wchar_t *)out->kvp_ip_val.sub_net,
+				MAX_IP_ADDR_SIZE);
+		if (len < 0)
+			return len;
+
+		len = utf8s_to_utf16s((char *)in->body.kvp_ip_val.gate_way,
+				strlen((char *)in->body.kvp_ip_val.gate_way),
+				UTF16_HOST_ENDIAN,
+				(wchar_t *)out->kvp_ip_val.gate_way,
+				MAX_GATEWAY_SIZE);
+		if (len < 0)
+			return len;
+
+		len = utf8s_to_utf16s((char *)in->body.kvp_ip_val.dns_addr,
+				strlen((char *)in->body.kvp_ip_val.dns_addr),
+				UTF16_HOST_ENDIAN,
+				(wchar_t *)out->kvp_ip_val.dns_addr,
+				MAX_IP_ADDR_SIZE);
+		if (len < 0)
+			return len;
+
+		len = utf8s_to_utf16s((char *)in->body.kvp_ip_val.adapter_id,
+				strlen((char *)in->body.kvp_ip_val.adapter_id),
+				UTF16_HOST_ENDIAN,
+				(wchar_t *)out->kvp_ip_val.adapter_id,
+				MAX_IP_ADDR_SIZE);
+		if (len < 0)
+			return len;
+
+		out->kvp_ip_val.dhcp_enabled =
+			in->body.kvp_ip_val.dhcp_enabled;
+		out->kvp_ip_val.addr_family =
+			in->body.kvp_ip_val.addr_family;
+	}
+
+	return 0;
+}
+
+static void process_ib_ipinfo(void *in_msg, void *out_msg, int op)
+{
+	struct hv_kvp_ip_msg *in = in_msg;
+	struct hv_kvp_msg *out = out_msg;
+
+	switch (op) {
+	case KVP_OP_SET_IP_INFO:
+		/*
+		 * Transform all parameters into utf8 encoding.
+		 */
+		utf16s_to_utf8s((wchar_t *)in->kvp_ip_val.ip_addr,
+				MAX_IP_ADDR_SIZE,
+				UTF16_LITTLE_ENDIAN,
+				(__u8 *)out->body.kvp_ip_val.ip_addr,
+				MAX_IP_ADDR_SIZE);
+
+		utf16s_to_utf8s((wchar_t *)in->kvp_ip_val.sub_net,
+				MAX_IP_ADDR_SIZE,
+				UTF16_LITTLE_ENDIAN,
+				(__u8 *)out->body.kvp_ip_val.sub_net,
+				MAX_IP_ADDR_SIZE);
+
+		utf16s_to_utf8s((wchar_t *)in->kvp_ip_val.gate_way,
+				MAX_GATEWAY_SIZE,
+				UTF16_LITTLE_ENDIAN,
+				(__u8 *)out->body.kvp_ip_val.gate_way,
+				MAX_GATEWAY_SIZE);
+
+		utf16s_to_utf8s((wchar_t *)in->kvp_ip_val.dns_addr,
+				MAX_IP_ADDR_SIZE,
+				UTF16_LITTLE_ENDIAN,
+				(__u8 *)out->body.kvp_ip_val.dns_addr,
+				MAX_IP_ADDR_SIZE);
+
+		out->body.kvp_ip_val.dhcp_enabled = in->kvp_ip_val.dhcp_enabled;
+
+	default:
+		utf16s_to_utf8s((wchar_t *)in->kvp_ip_val.adapter_id,
+				MAX_ADAPTER_ID_SIZE,
+				UTF16_LITTLE_ENDIAN,
+				(__u8 *)out->body.kvp_ip_val.adapter_id,
+				MAX_ADAPTER_ID_SIZE);
+
+		out->body.kvp_ip_val.addr_family = in->kvp_ip_val.addr_family;
 	}
 }
 
+
+
+
 static void
 kvp_send_key(struct work_struct *dummy)
 {
@@ -167,6 +348,12 @@ kvp_send_key(struct work_struct *dummy)
 	 */
 
 	switch (message->kvp_hdr.operation) {
+	case KVP_OP_SET_IP_INFO:
+		process_ib_ipinfo(in_msg, message, KVP_OP_SET_IP_INFO);
+		break;
+	case KVP_OP_GET_IP_INFO:
+		process_ib_ipinfo(in_msg, message, KVP_OP_GET_IP_INFO);
+		break;
 	case KVP_OP_SET:
 		switch (in_msg->body.kvp_set.data.value_type) {
 		case REG_SZ:
@@ -243,17 +430,19 @@ kvp_send_key(struct work_struct *dummy)
  */
 
 static void
-kvp_respond_to_host(char *key, char *value, int error)
+kvp_respond_to_host(struct hv_kvp_msg *msg_to_host, int error)
 {
 	struct hv_kvp_msg  *kvp_msg;
 	struct hv_kvp_exchg_msg_value  *kvp_data;
 	char	*key_name;
+	char	*value;
 	struct icmsg_hdr *icmsghdrp;
 	int	keylen = 0;
 	int	valuelen = 0;
 	u32	buf_len;
 	struct vmbus_channel *channel;
 	u64	req_id;
+	int ret;
 
 	/*
 	 * If a transaction is not active; log and return.
@@ -287,6 +476,7 @@ kvp_respond_to_host(char *key, char *value, int error)
 		 */
 		return;
 
+	icmsghdrp->status = error;
 
 	/*
 	 * If the error parameter is set, terminate the host's enumeration
@@ -294,20 +484,27 @@ kvp_respond_to_host(char *key, char *value, int error)
 	 */
 	if (error) {
 		/*
-		 * Something failed or the we have timedout;
-		 * terminate the current  host-side iteration.
+		 * Something failed or we have timedout;
+		 * terminate the current host-side iteration.
 		 */
-		icmsghdrp->status = HV_S_CONT;
 		goto response_done;
 	}
 
-	icmsghdrp->status = HV_S_OK;
-
 	kvp_msg = (struct hv_kvp_msg *)
 			&recv_buffer[sizeof(struct vmbuspipe_hdr) +
 			sizeof(struct icmsg_hdr)];
 
 	switch (kvp_transaction.kvp_msg->kvp_hdr.operation) {
+	case KVP_OP_GET_IP_INFO:
+		ret = process_ob_ipinfo(msg_to_host,
+				 (struct hv_kvp_ip_msg *)kvp_msg,
+				 KVP_OP_GET_IP_INFO);
+		if (ret < 0)
+			icmsghdrp->status = HV_E_FAIL;
+
+		goto response_done;
+	case KVP_OP_SET_IP_INFO:
+		goto response_done;
 	case KVP_OP_GET:
 		kvp_data = &kvp_msg->body.kvp_get.data;
 		goto copy_value;
@@ -321,7 +518,7 @@ kvp_respond_to_host(char *key, char *value, int error)
 	}
 
 	kvp_data = &kvp_msg->body.kvp_enum_data.data;
-	key_name = key;
+	key_name = msg_to_host->body.kvp_enum_data.data.key;
 
 	/*
 	 * The windows host expects the key/value pair to be encoded
@@ -335,6 +532,7 @@ kvp_respond_to_host(char *key, char *value, int error)
 	kvp_data->key_size = 2*(keylen + 1); /* utf16 encoding */
 
 copy_value:
+	value = msg_to_host->body.kvp_enum_data.data.value;
 	valuelen = utf8s_to_utf16s(value, strlen(value), UTF16_HOST_ENDIAN,
 				(wchar_t *) kvp_data->value,
 				(HV_KVP_EXCHANGE_MAX_VALUE_SIZE / 2) - 2);
@@ -387,7 +585,8 @@ void hv_kvp_onchannelcallback(void *context)
 		return;
 	}
 
-	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE, &recvlen, &requestid);
+	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
+			 &requestid);
 
 	if (recvlen > 0) {
 		icmsghdrp = (struct icmsg_hdr *)&recv_buffer[
diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index d3ac6a40118b..a0667de7a04c 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -263,7 +263,7 @@ static int util_probe(struct hv_device *dev,
 		(struct hv_util_service *)dev_id->driver_data;
 	int ret;
 
-	srv->recv_buffer = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	srv->recv_buffer = kmalloc(PAGE_SIZE * 2, GFP_KERNEL);
 	if (!srv->recv_buffer)
 		return -ENOMEM;
 	if (srv->util_init) {
@@ -274,7 +274,7 @@ static int util_probe(struct hv_device *dev,
 		}
 	}
 
-	ret = vmbus_open(dev->channel, 2 * PAGE_SIZE, 2 * PAGE_SIZE, NULL, 0,
+	ret = vmbus_open(dev->channel, 4 * PAGE_SIZE, 4 * PAGE_SIZE, NULL, 0,
 			srv->util_cb, dev->channel);
 	if (ret)
 		goto error;
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 0614ff3a7d7e..d8d1fadb398a 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -410,10 +410,49 @@ enum {
 
 #define HV_PRESENT_BIT			0x80000000
 
-#define HV_LINUX_GUEST_ID_LO		0x00000000
-#define HV_LINUX_GUEST_ID_HI		2976579765
-#define HV_LINUX_GUEST_ID		(((u64)HV_LINUX_GUEST_ID_HI << 32) | \
-					   HV_LINUX_GUEST_ID_LO)
+/*
+ * The guest OS needs to register the guest ID with the hypervisor.
+ * The guest ID is a 64 bit entity and the structure of this ID is
+ * specified in the Hyper-V specification:
+ *
+ * http://msdn.microsoft.com/en-us/library/windows/hardware/ff542653%28v=vs.85%29.aspx
+ *
+ * While the current guideline does not specify how Linux guest ID(s)
+ * need to be generated, our plan is to publish the guidelines for
+ * Linux and other guest operating systems that currently are hosted
+ * on Hyper-V. The implementation here conforms to this yet
+ * unpublished guidelines.
+ *
+ *
+ * Bit(s)
+ * 63 - Indicates if the OS is Open Source or not; 1 is Open Source
+ * 62:56 - Os Type; Linux is 0x100
+ * 55:48 - Distro specific identification
+ * 47:16 - Linux kernel version number
+ * 15:0  - Distro specific identification
+ *
+ *
+ */
+
+#define HV_LINUX_VENDOR_ID		0x8100
+
+/*
+ * Generate the guest ID based on the guideline described above.
+ */
+
+static inline  __u64 generate_guest_id(__u8 d_info1, __u32 kernel_version,
+					__u16 d_info2)
+{
+	__u64 guest_id = 0;
+
+	guest_id = (((__u64)HV_LINUX_VENDOR_ID) << 48);
+	guest_id |= (((__u64)(d_info1)) << 48);
+	guest_id |= (((__u64)(kernel_version)) << 16);
+	guest_id |= ((__u64)(d_info2));
+
+	return guest_id;
+}
+
 
 #define HV_CPU_POWER_MANAGEMENT		(1 << 0)
 #define HV_RECOMMENDATIONS_MAX		4
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index 4748086eaaf2..8e1a9ec53003 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -34,6 +34,7 @@
 #include <linux/completion.h>
 #include <linux/hyperv.h>
 #include <asm/hyperv.h>
+#include <asm/hypervisor.h>
 #include "hyperv_vmbus.h"
 
 
@@ -146,43 +147,9 @@ static ssize_t vmbus_show_device_attr(struct device *dev,
 	get_channel_info(hv_dev, device_info);
 
 	if (!strcmp(dev_attr->attr.name, "class_id")) {
-		ret = sprintf(buf, "{%02x%02x%02x%02x-%02x%02x-%02x%02x-"
-			       "%02x%02x%02x%02x%02x%02x%02x%02x}\n",
-			       device_info->chn_type.b[3],
-			       device_info->chn_type.b[2],
-			       device_info->chn_type.b[1],
-			       device_info->chn_type.b[0],
-			       device_info->chn_type.b[5],
-			       device_info->chn_type.b[4],
-			       device_info->chn_type.b[7],
-			       device_info->chn_type.b[6],
-			       device_info->chn_type.b[8],
-			       device_info->chn_type.b[9],
-			       device_info->chn_type.b[10],
-			       device_info->chn_type.b[11],
-			       device_info->chn_type.b[12],
-			       device_info->chn_type.b[13],
-			       device_info->chn_type.b[14],
-			       device_info->chn_type.b[15]);
+		ret = sprintf(buf, "{%pUl}\n", device_info->chn_type.b);
 	} else if (!strcmp(dev_attr->attr.name, "device_id")) {
-		ret = sprintf(buf, "{%02x%02x%02x%02x-%02x%02x-%02x%02x-"
-			       "%02x%02x%02x%02x%02x%02x%02x%02x}\n",
-			       device_info->chn_instance.b[3],
-			       device_info->chn_instance.b[2],
-			       device_info->chn_instance.b[1],
-			       device_info->chn_instance.b[0],
-			       device_info->chn_instance.b[5],
-			       device_info->chn_instance.b[4],
-			       device_info->chn_instance.b[7],
-			       device_info->chn_instance.b[6],
-			       device_info->chn_instance.b[8],
-			       device_info->chn_instance.b[9],
-			       device_info->chn_instance.b[10],
-			       device_info->chn_instance.b[11],
-			       device_info->chn_instance.b[12],
-			       device_info->chn_instance.b[13],
-			       device_info->chn_instance.b[14],
-			       device_info->chn_instance.b[15]);
+		ret = sprintf(buf, "{%pUl}\n", device_info->chn_instance.b);
 	} else if (!strcmp(dev_attr->attr.name, "modalias")) {
 		print_alias_name(hv_dev, alias_name);
 		ret = sprintf(buf, "vmbus:%s\n", alias_name);
@@ -757,6 +724,9 @@ static int __init hv_acpi_init(void)
 {
 	int ret, t;
 
+	if (x86_hyper != &x86_hyper_ms_hyperv)
+		return -ENODEV;
+
 	init_completion(&probe_event);
 
 	/*
diff --git a/drivers/hwmon/ad7314.c b/drivers/hwmon/ad7314.c
index cfec802cf9ca..f915eb1c29f7 100644
--- a/drivers/hwmon/ad7314.c
+++ b/drivers/hwmon/ad7314.c
@@ -87,10 +87,18 @@ static ssize_t ad7314_show_temperature(struct device *dev,
 	}
 }
 
+static ssize_t ad7314_show_name(struct device *dev,
+				struct device_attribute *devattr, char *buf)
+{
+	return sprintf(buf, "%s\n", to_spi_device(dev)->modalias);
+}
+
+static DEVICE_ATTR(name, S_IRUGO, ad7314_show_name, NULL);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO,
 			  ad7314_show_temperature, NULL, 0);
 
 static struct attribute *ad7314_attributes[] = {
+	&dev_attr_name.attr,
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	NULL,
 };
diff --git a/drivers/hwmon/ads7871.c b/drivers/hwmon/ads7871.c
index e65c6e45d36b..7bf4ce3d405e 100644
--- a/drivers/hwmon/ads7871.c
+++ b/drivers/hwmon/ads7871.c
@@ -139,6 +139,12 @@ static ssize_t show_voltage(struct device *dev,
 	}
 }
 
+static ssize_t ads7871_show_name(struct device *dev,
+				 struct device_attribute *devattr, char *buf)
+{
+	return sprintf(buf, "%s\n", to_spi_device(dev)->modalias);
+}
+
 static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, show_voltage, NULL, 0);
 static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, show_voltage, NULL, 1);
 static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, show_voltage, NULL, 2);
@@ -148,6 +154,8 @@ static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, show_voltage, NULL, 5);
 static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, show_voltage, NULL, 6);
 static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, show_voltage, NULL, 7);
 
+static DEVICE_ATTR(name, S_IRUGO, ads7871_show_name, NULL);
+
 static struct attribute *ads7871_attributes[] = {
 	&sensor_dev_attr_in0_input.dev_attr.attr,
 	&sensor_dev_attr_in1_input.dev_attr.attr,
@@ -157,6 +165,7 @@ static struct attribute *ads7871_attributes[] = {
 	&sensor_dev_attr_in5_input.dev_attr.attr,
 	&sensor_dev_attr_in6_input.dev_attr.attr,
 	&sensor_dev_attr_in7_input.dev_attr.attr,
+	&dev_attr_name.attr,
 	NULL
 };
 
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 282708860517..8f3f6f2c45fd 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -53,10 +53,10 @@
 
 #define APPLESMC_MAX_DATA_LENGTH 32
 
-/* wait up to 32 ms for a status change. */
+/* wait up to 128 ms for a status change. */
 #define APPLESMC_MIN_WAIT	0x0010
 #define APPLESMC_RETRY_WAIT	0x0100
-#define APPLESMC_MAX_WAIT	0x8000
+#define APPLESMC_MAX_WAIT	0x20000
 
 #define APPLESMC_READ_CMD	0x10
 #define APPLESMC_WRITE_CMD	0x11
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 0fa356fe82cc..984a3f13923b 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -815,17 +815,20 @@ static int __init coretemp_init(void)
 	if (err)
 		goto exit;
 
+	get_online_cpus();
 	for_each_online_cpu(i)
 		get_core_online(i);
 
 #ifndef CONFIG_HOTPLUG_CPU
 	if (list_empty(&pdev_list)) {
+		put_online_cpus();
 		err = -ENODEV;
 		goto exit_driver_unreg;
 	}
 #endif
 
 	register_hotcpu_notifier(&coretemp_cpu_notifier);
+	put_online_cpus();
 	return 0;
 
 #ifndef CONFIG_HOTPLUG_CPU
@@ -840,6 +843,7 @@ static void __exit coretemp_exit(void)
 {
 	struct pdev_entry *p, *n;
 
+	get_online_cpus();
 	unregister_hotcpu_notifier(&coretemp_cpu_notifier);
 	mutex_lock(&pdev_list_mutex);
 	list_for_each_entry_safe(p, n, &pdev_list, list) {
@@ -848,6 +852,7 @@ static void __exit coretemp_exit(void)
 		kfree(p);
 	}
 	mutex_unlock(&pdev_list_mutex);
+	put_online_cpus();
 	platform_driver_unregister(&coretemp_driver);
 }
 
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index 2764b78a784b..af69073b3fe8 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -129,12 +129,12 @@ static bool __devinit fam15h_power_is_internal_node0(struct pci_dev *f4)
  * counter saturations resulting in bogus power readings.
  * We correct this value ourselves to cope with older BIOSes.
  */
-static DEFINE_PCI_DEVICE_TABLE(affected_device) = {
+static const struct pci_device_id affected_device[] = {
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_NB_F4) },
 	{ 0 }
 };
 
-static void __devinit tweak_runavg_range(struct pci_dev *pdev)
+static void tweak_runavg_range(struct pci_dev *pdev)
 {
 	u32 val;
 
@@ -158,6 +158,16 @@ static void __devinit tweak_runavg_range(struct pci_dev *pdev)
 		REG_TDP_RUNNING_AVERAGE, val);
 }
 
+#ifdef CONFIG_PM
+static int fam15h_power_resume(struct pci_dev *pdev)
+{
+	tweak_runavg_range(pdev);
+	return 0;
+}
+#else
+#define fam15h_power_resume NULL
+#endif
+
 static void __devinit fam15h_power_init_data(struct pci_dev *f4,
 					     struct fam15h_power_data *data)
 {
@@ -256,6 +266,7 @@ static struct pci_driver fam15h_power_driver = {
 	.id_table = fam15h_power_id_table,
 	.probe = fam15h_power_probe,
 	.remove = __devexit_p(fam15h_power_remove),
+	.resume = fam15h_power_resume,
 };
 
 module_pci_driver(fam15h_power_driver);
diff --git a/drivers/hwmon/via-cputemp.c b/drivers/hwmon/via-cputemp.c
index ee4ebc198a94..2e56c6ce9fb6 100644
--- a/drivers/hwmon/via-cputemp.c
+++ b/drivers/hwmon/via-cputemp.c
@@ -328,6 +328,7 @@ static int __init via_cputemp_init(void)
 	if (err)
 		goto exit;
 
+	get_online_cpus();
 	for_each_online_cpu(i) {
 		struct cpuinfo_x86 *c = &cpu_data(i);
 
@@ -347,12 +348,14 @@ static int __init via_cputemp_init(void)
 
 #ifndef CONFIG_HOTPLUG_CPU
 	if (list_empty(&pdev_list)) {
+		put_online_cpus();
 		err = -ENODEV;
 		goto exit_driver_unreg;
 	}
 #endif
 
 	register_hotcpu_notifier(&via_cputemp_cpu_notifier);
+	put_online_cpus();
 	return 0;
 
 #ifndef CONFIG_HOTPLUG_CPU
@@ -367,6 +370,7 @@ static void __exit via_cputemp_exit(void)
 {
 	struct pdev_entry *p, *n;
 
+	get_online_cpus();
 	unregister_hotcpu_notifier(&via_cputemp_cpu_notifier);
 	mutex_lock(&pdev_list_mutex);
 	list_for_each_entry_safe(p, n, &pdev_list, list) {
@@ -375,6 +379,7 @@ static void __exit via_cputemp_exit(void)
 		kfree(p);
 	}
 	mutex_unlock(&pdev_list_mutex);
+	put_online_cpus();
 	platform_driver_unregister(&via_cputemp_driver);
 }
 
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index 1201a15784c3..db713c0dfba4 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -552,7 +552,7 @@ EXPORT_SYMBOL_GPL(hwspin_lock_request_specific);
  */
 int hwspin_lock_free(struct hwspinlock *hwlock)
 {
-	struct device *dev = hwlock->bank->dev;
+	struct device *dev;
 	struct hwspinlock *tmp;
 	int ret;
 
@@ -561,6 +561,7 @@ int hwspin_lock_free(struct hwspinlock *hwlock)
 		return -EINVAL;
 	}
 
+	dev = hwlock->bank->dev;
 	mutex_lock(&hwspinlock_tree_lock);
 
 	/* make sure the hwspinlock is used */
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index d4984c8be973..6e3f143fc71d 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -1,5 +1,5 @@
 #
-# Industrial I/O subsytem configuration
+# Industrial I/O subsystem configuration
 #
 
 menuconfig IIO
@@ -54,10 +54,15 @@ config IIO_CONSUMERS_PER_TRIGGER
 	This value controls the maximum number of consumers that a
 	given trigger may handle. Default is 2.
 
+source "drivers/iio/accel/Kconfig"
 source "drivers/iio/adc/Kconfig"
 source "drivers/iio/amplifiers/Kconfig"
 source "drivers/iio/light/Kconfig"
 source "drivers/iio/frequency/Kconfig"
 source "drivers/iio/dac/Kconfig"
+source "drivers/iio/common/Kconfig"
+source "drivers/iio/gyro/Kconfig"
+source "drivers/iio/light/Kconfig"
+source "drivers/iio/magnetometer/Kconfig"
 
 endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index 34309abb7979..f7fa3c0867b4 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -10,8 +10,13 @@ industrialio-$(CONFIG_IIO_TRIGGER) += industrialio-trigger.o
 obj-$(CONFIG_IIO_TRIGGERED_BUFFER) += industrialio-triggered-buffer.o
 obj-$(CONFIG_IIO_KFIFO_BUF) += kfifo_buf.o
 
+obj-y += accel/
 obj-y += adc/
 obj-y += amplifiers/
 obj-y += light/
 obj-y += frequency/
 obj-y += dac/
+obj-y += common/
+obj-y += gyro/
+obj-y += light/
+obj-y += magnetometer/
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
new file mode 100644
index 000000000000..b2510c4d9a5a
--- /dev/null
+++ b/drivers/iio/accel/Kconfig
@@ -0,0 +1,16 @@
+#
+# Accelerometer drivers
+#
+menu "Accelerometers"
+
+config HID_SENSOR_ACCEL_3D
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	tristate "HID Acelerometers 3D"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  accelerometers 3D.
+
+endmenu
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
new file mode 100644
index 000000000000..5bc6855a973e
--- /dev/null
+++ b/drivers/iio/accel/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for industrial I/O accelerometer drivers
+#
+
+obj-$(CONFIG_HID_SENSOR_ACCEL_3D) += hid-sensor-accel-3d.o
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
new file mode 100644
index 000000000000..314a4057879e
--- /dev/null
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -0,0 +1,418 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "../common/hid-sensors/hid-sensor-attributes.h"
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+/*Format: HID-SENSOR-usage_id_in_hex*/
+/*Usage ID from spec for Accelerometer-3D: 0x200073*/
+#define DRIVER_NAME "HID-SENSOR-200073"
+
+enum accel_3d_channel {
+	CHANNEL_SCAN_INDEX_X,
+	CHANNEL_SCAN_INDEX_Y,
+	CHANNEL_SCAN_INDEX_Z,
+	ACCEL_3D_CHANNEL_MAX,
+};
+
+struct accel_3d_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_iio_common common_attributes;
+	struct hid_sensor_hub_attribute_info accel[ACCEL_3D_CHANNEL_MAX];
+	u32 accel_val[ACCEL_3D_CHANNEL_MAX];
+};
+
+static const u32 accel_3d_addresses[ACCEL_3D_CHANNEL_MAX] = {
+	HID_USAGE_SENSOR_ACCEL_X_AXIS,
+	HID_USAGE_SENSOR_ACCEL_Y_AXIS,
+	HID_USAGE_SENSOR_ACCEL_Z_AXIS
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec accel_3d_channels[] = {
+	{
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_X,
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Y,
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Z,
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void accel_3d_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+						int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	channels[channel].scan_type.storagebits = sizeof(u32) * 8;
+}
+
+/* Channel read_raw handler */
+static int accel_3d_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct accel_3d_state *accel_state = iio_priv(indio_dev);
+	int report_id = -1;
+	u32 address;
+	int ret;
+	int ret_type;
+
+	*val = 0;
+	*val2 = 0;
+	switch (mask) {
+	case 0:
+		report_id = accel_state->accel[chan->scan_index].report_id;
+		address = accel_3d_addresses[chan->scan_index];
+		if (report_id >= 0)
+			*val = sensor_hub_input_attr_get_raw_value(
+				accel_state->common_attributes.hsdev,
+				HID_USAGE_SENSOR_ACCEL_3D, address,
+				report_id);
+		else {
+			*val = 0;
+			return -EINVAL;
+		}
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = accel_state->accel[CHANNEL_SCAN_INDEX_X].units;
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = hid_sensor_convert_exponent(
+			accel_state->accel[CHANNEL_SCAN_INDEX_X].unit_expo);
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_read_samp_freq_value(
+			&accel_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_read_raw_hyst_value(
+			&accel_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int accel_3d_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct accel_3d_state *accel_state = iio_priv(indio_dev);
+	int ret = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&accel_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&accel_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int accel_3d_write_raw_get_fmt(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       long mask)
+{
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static const struct iio_info accel_3d_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &accel_3d_read_raw,
+	.write_raw = &accel_3d_write_raw,
+	.write_raw_get_fmt = &accel_3d_write_raw_get_fmt,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	int datum_sz;
+
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n");
+	if (!buffer) {
+		dev_err(&indio_dev->dev, "Buffer == NULL\n");
+		return;
+	}
+	datum_sz = buffer->access->get_bytes_per_datum(buffer);
+	if (len > datum_sz) {
+		dev_err(&indio_dev->dev, "Datum size mismatch %d:%d\n", len,
+				datum_sz);
+		return;
+	}
+	iio_push_to_buffer(buffer, (u8 *)data);
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int accel_3d_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct accel_3d_state *accel_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "accel_3d_proc_event [%d]\n",
+				accel_state->common_attributes.data_ready);
+	if (accel_state->common_attributes.data_ready)
+		hid_sensor_push_data(indio_dev,
+				(u8 *)accel_state->accel_val,
+				sizeof(accel_state->accel_val));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int accel_3d_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct accel_3d_state *accel_state = iio_priv(indio_dev);
+	int offset;
+	int ret = -EINVAL;
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_ACCEL_X_AXIS:
+	case HID_USAGE_SENSOR_ACCEL_Y_AXIS:
+	case HID_USAGE_SENSOR_ACCEL_Z_AXIS:
+		offset = usage_id - HID_USAGE_SENSOR_ACCEL_X_AXIS;
+		accel_state->accel_val[CHANNEL_SCAN_INDEX_X + offset] =
+						*(u32 *)raw_data;
+		ret = 0;
+	break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/* Parse report which is specific to an usage id*/
+static int accel_3d_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct accel_3d_state *st)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i <= CHANNEL_SCAN_INDEX_Z; ++i) {
+		ret = sensor_hub_input_get_attribute_info(hsdev,
+				HID_INPUT_REPORT,
+				usage_id,
+				HID_USAGE_SENSOR_ACCEL_X_AXIS + i,
+				&st->accel[CHANNEL_SCAN_INDEX_X + i]);
+		if (ret < 0)
+			break;
+		accel_3d_adjust_channel_bit_mask(channels,
+				CHANNEL_SCAN_INDEX_X + i,
+				st->accel[CHANNEL_SCAN_INDEX_X + i].size);
+	}
+	dev_dbg(&pdev->dev, "accel_3d %x:%x, %x:%x, %x:%x\n",
+			st->accel[0].index,
+			st->accel[0].report_id,
+			st->accel[1].index, st->accel[1].report_id,
+			st->accel[2].index, st->accel[2].report_id);
+
+	return ret;
+}
+
+/* Function to initialize the processing for usage id */
+static int __devinit hid_accel_3d_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	static const char *name = "accel_3d";
+	struct iio_dev *indio_dev;
+	struct accel_3d_state *accel_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = iio_device_alloc(sizeof(struct accel_3d_state));
+	if (indio_dev == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	platform_set_drvdata(pdev, indio_dev);
+
+	accel_state = iio_priv(indio_dev);
+	accel_state->common_attributes.hsdev = hsdev;
+	accel_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+					HID_USAGE_SENSOR_ACCEL_3D,
+					&accel_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		goto error_free_dev;
+	}
+
+	channels = kmemdup(accel_3d_channels,
+					sizeof(accel_3d_channels),
+					GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		goto error_free_dev;
+	}
+
+	ret = accel_3d_parse_report(pdev, hsdev, channels,
+					HID_USAGE_SENSOR_ACCEL_3D, accel_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		goto error_free_dev_mem;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels = ARRAY_SIZE(accel_3d_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &accel_3d_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		goto error_free_dev_mem;
+	}
+	accel_state->common_attributes.data_ready = false;
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+					&accel_state->common_attributes);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	accel_state->callbacks.send_event = accel_3d_proc_event;
+	accel_state->callbacks.capture_sample = accel_3d_capture_sample;
+	accel_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_ACCEL_3D,
+					&accel_state->callbacks);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return ret;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(indio_dev);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_free_dev_mem:
+	kfree(indio_dev->channels);
+error_free_dev:
+	iio_device_free(indio_dev);
+error_ret:
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int __devinit hid_accel_3d_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_ACCEL_3D);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	kfree(indio_dev->channels);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver hid_accel_3d_platform_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= hid_accel_3d_probe,
+	.remove		= hid_accel_3d_remove,
+};
+module_platform_driver(hid_accel_3d_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Accel 3D");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 8a78b4f3ef58..492758120338 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -3,6 +3,11 @@
 #
 menu "Analog to digital converters"
 
+config AD_SIGMA_DELTA
+	tristate
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+
 config AD7266
 	tristate "Analog Devices AD7265/AD7266 ADC driver"
 	depends on SPI_MASTER
@@ -13,6 +18,33 @@ config AD7266
 	  Say yes here to build support for Analog Devices AD7265 and AD7266
 	  ADCs.
 
+config AD7791
+	tristate "Analog Devices AD7791 ADC driver"
+	depends on SPI
+	select AD_SIGMA_DELTA
+	help
+	  Say yes here to build support for Analog Devices AD7787, AD7788, AD7789,
+	  AD7790 and AD7791 SPI analog to digital converters (ADC). If unsure, say
+	  N (but it is safe to say "Y").
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ad7791.
+
+config AD7476
+	tristate "Analog Devices AD7476 and similar 1-channel ADCs driver"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Analog Devices AD7273, AD7274, AD7276,
+	  AD7277, AD7278, AD7475, AD7476, AD7477, AD7478, AD7466, AD7467, AD7468,
+	  AD7495, AD7910, AD7920, AD7920 SPI analog to digital converters (ADC).
+
+	  If unsure, say N (but it's safe to say "Y").
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad7476.
+
 config AT91_ADC
 	tristate "Atmel AT91 ADC"
 	depends on ARCH_AT91
@@ -22,4 +54,10 @@ config AT91_ADC
 	help
 	  Say yes here to build support for Atmel AT91 ADC.
 
+config LP8788_ADC
+	bool "LP8788 ADC driver"
+	depends on MFD_LP8788
+	help
+	  Say yes here to build support for TI LP8788 ADC.
+
 endmenu
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 52eec254c38c..900995d5e179 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -2,5 +2,9 @@
 # Makefile for IIO ADC drivers
 #
 
+obj-$(CONFIG_AD_SIGMA_DELTA) += ad_sigma_delta.o
 obj-$(CONFIG_AD7266) += ad7266.o
+obj-$(CONFIG_AD7476) += ad7476.o
+obj-$(CONFIG_AD7791) += ad7791.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
+obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
diff --git a/drivers/iio/adc/ad7266.c b/drivers/iio/adc/ad7266.c
index 5c3f1ba5a06d..b11f214779a2 100644
--- a/drivers/iio/adc/ad7266.c
+++ b/drivers/iio/adc/ad7266.c
@@ -99,7 +99,7 @@ static irqreturn_t ad7266_trigger_handler(int irq, void *p)
 	if (ret == 0) {
 		if (indio_dev->scan_timestamp)
 			((s64 *)st->data)[1] = pf->timestamp;
-		iio_push_to_buffer(buffer, (u8 *)st->data, pf->timestamp);
+		iio_push_to_buffer(buffer, (u8 *)st->data);
 	}
 
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/staging/iio/adc/ad7476_core.c b/drivers/iio/adc/ad7476.c
index 4d30a798ba0d..7f2f45a0a48d 100644
--- a/drivers/staging/iio/adc/ad7476_core.c
+++ b/drivers/iio/adc/ad7476.c
@@ -18,8 +18,76 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
-#include "ad7476.h"
+#define RES_MASK(bits)	((1 << (bits)) - 1)
+
+struct ad7476_state;
+
+struct ad7476_chip_info {
+	unsigned int			int_vref_uv;
+	struct iio_chan_spec		channel[2];
+	void (*reset)(struct ad7476_state *);
+};
+
+struct ad7476_state {
+	struct spi_device		*spi;
+	const struct ad7476_chip_info	*chip_info;
+	struct regulator		*reg;
+	struct spi_transfer		xfer;
+	struct spi_message		msg;
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 * Make the buffer large enough for one 16 bit sample and one 64 bit
+	 * aligned 64 bit timestamp.
+	 */
+	unsigned char data[ALIGN(2, sizeof(s64)) + sizeof(s64)]
+			____cacheline_aligned;
+};
+
+enum ad7476_supported_device_ids {
+	ID_AD7091R,
+	ID_AD7276,
+	ID_AD7277,
+	ID_AD7278,
+	ID_AD7466,
+	ID_AD7467,
+	ID_AD7468,
+	ID_AD7495,
+	ID_AD7940,
+};
+
+static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ad7476_state *st = iio_priv(indio_dev);
+	s64 time_ns;
+	int b_sent;
+
+	b_sent = spi_sync(st->spi, &st->msg);
+	if (b_sent < 0)
+		goto done;
+
+	time_ns = iio_get_time_ns();
+
+	if (indio_dev->scan_timestamp)
+		((s64 *)st->data)[1] = time_ns;
+
+	iio_push_to_buffer(indio_dev->buffer, st->data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static void ad7091_reset(struct ad7476_state *st)
+{
+	/* Any transfers with 8 scl cycles will reset the device */
+	spi_read(st->spi, st->data, 1);
+}
 
 static int ad7476_scan_direct(struct ad7476_state *st)
 {
@@ -29,7 +97,7 @@ static int ad7476_scan_direct(struct ad7476_state *st)
 	if (ret)
 		return ret;
 
-	return (st->data[0] << 8) | st->data[1];
+	return be16_to_cpup((__be16 *)st->data);
 }
 
 static int ad7476_read_raw(struct iio_dev *indio_dev,
@@ -40,7 +108,7 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 {
 	int ret;
 	struct ad7476_state *st = iio_priv(indio_dev);
-	unsigned int scale_uv;
+	int scale_uv;
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
@@ -57,62 +125,80 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 			RES_MASK(st->chip_info->channel[0].scan_type.realbits);
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
-		scale_uv = (st->int_vref_mv * 1000)
-			>> st->chip_info->channel[0].scan_type.realbits;
-		*val =  scale_uv/1000;
-		*val2 = (scale_uv%1000)*1000;
+		if (!st->chip_info->int_vref_uv) {
+			scale_uv = regulator_get_voltage(st->reg);
+			if (scale_uv < 0)
+				return scale_uv;
+		} else {
+			scale_uv = st->chip_info->int_vref_uv;
+		}
+		scale_uv >>= chan->scan_type.realbits;
+		*val =  scale_uv / 1000;
+		*val2 = (scale_uv % 1000) * 1000;
 		return IIO_VAL_INT_PLUS_MICRO;
 	}
 	return -EINVAL;
 }
 
-#define AD7476_CHAN(bits)					\
+#define _AD7476_CHAN(bits, _shift, _info_mask)			\
 	{							\
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+	.info_mask = _info_mask |				\
 	IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
 	.scan_type = {						\
 		.sign = 'u',					\
-		.realbits = bits,				\
+		.realbits = (bits),				\
 		.storagebits = 16,				\
-		.shift = 12 - bits,				\
+		.shift = (_shift),				\
+		.endianness = IIO_BE,				\
 	},							\
 }
 
+#define AD7476_CHAN(bits) _AD7476_CHAN((bits), 13 - (bits), \
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT)
+#define AD7940_CHAN(bits) _AD7476_CHAN((bits), 15 - (bits), \
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT)
+#define AD7091R_CHAN(bits) _AD7476_CHAN((bits), 16 - (bits), 0)
+
 static const struct ad7476_chip_info ad7476_chip_info_tbl[] = {
-	[ID_AD7466] = {
-		.channel[0] = AD7476_CHAN(12),
+	[ID_AD7091R] = {
+		.channel[0] = AD7091R_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
+		.reset = ad7091_reset,
 	},
-	[ID_AD7467] = {
-		.channel[0] = AD7476_CHAN(10),
+	[ID_AD7276] = {
+		.channel[0] = AD7940_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
-	[ID_AD7468] = {
-		.channel[0] = AD7476_CHAN(8),
+	[ID_AD7277] = {
+		.channel[0] = AD7940_CHAN(10),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
-	[ID_AD7475] = {
-		.channel[0] = AD7476_CHAN(12),
+	[ID_AD7278] = {
+		.channel[0] = AD7940_CHAN(8),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
-	[ID_AD7476] = {
+	[ID_AD7466] = {
 		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
-	[ID_AD7477] = {
+	[ID_AD7467] = {
 		.channel[0] = AD7476_CHAN(10),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
-	[ID_AD7478] = {
+	[ID_AD7468] = {
 		.channel[0] = AD7476_CHAN(8),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7495] = {
 		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
-		.int_vref_mv = 2500,
+		.int_vref_uv = 2500000,
+	},
+	[ID_AD7940] = {
+		.channel[0] = AD7940_CHAN(14),
+		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 };
 
@@ -123,10 +209,9 @@ static const struct iio_info ad7476_info = {
 
 static int __devinit ad7476_probe(struct spi_device *spi)
 {
-	struct ad7476_platform_data *pdata = spi->dev.platform_data;
 	struct ad7476_state *st;
 	struct iio_dev *indio_dev;
-	int ret, voltage_uv = 0;
+	int ret;
 
 	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
@@ -134,25 +219,18 @@ static int __devinit ad7476_probe(struct spi_device *spi)
 		goto error_ret;
 	}
 	st = iio_priv(indio_dev);
-	st->reg = regulator_get(&spi->dev, "vcc");
-	if (!IS_ERR(st->reg)) {
-		ret = regulator_enable(st->reg);
-		if (ret)
-			goto error_put_reg;
-
-		voltage_uv = regulator_get_voltage(st->reg);
-	}
 	st->chip_info =
 		&ad7476_chip_info_tbl[spi_get_device_id(spi)->driver_data];
 
-	if (st->chip_info->int_vref_mv)
-		st->int_vref_mv = st->chip_info->int_vref_mv;
-	else if (pdata && pdata->vref_mv)
-		st->int_vref_mv = pdata->vref_mv;
-	else if (voltage_uv)
-		st->int_vref_mv = voltage_uv / 1000;
-	else
-		dev_warn(&spi->dev, "reference voltage unspecified\n");
+	st->reg = regulator_get(&spi->dev, "vcc");
+	if (IS_ERR(st->reg)) {
+		ret = PTR_ERR(st->reg);
+		goto error_free_dev;
+	}
+
+	ret = regulator_enable(st->reg);
+	if (ret)
+		goto error_put_reg;
 
 	spi_set_drvdata(spi, indio_dev);
 
@@ -173,57 +251,67 @@ static int __devinit ad7476_probe(struct spi_device *spi)
 	spi_message_init(&st->msg);
 	spi_message_add_tail(&st->xfer, &st->msg);
 
-	ret = ad7476_register_ring_funcs_and_init(indio_dev);
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+			&ad7476_trigger_handler, NULL);
 	if (ret)
 		goto error_disable_reg;
 
+	if (st->chip_info->reset)
+		st->chip_info->reset(st);
+
 	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto error_ring_unregister;
 	return 0;
 
 error_ring_unregister:
-	ad7476_ring_cleanup(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
 error_disable_reg:
-	if (!IS_ERR(st->reg))
-		regulator_disable(st->reg);
+	regulator_disable(st->reg);
 error_put_reg:
-	if (!IS_ERR(st->reg))
-		regulator_put(st->reg);
+	regulator_put(st->reg);
+error_free_dev:
 	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
 }
 
-static int ad7476_remove(struct spi_device *spi)
+static int __devexit ad7476_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ad7476_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	ad7476_ring_cleanup(indio_dev);
-	if (!IS_ERR(st->reg)) {
-		regulator_disable(st->reg);
-		regulator_put(st->reg);
-	}
+	iio_triggered_buffer_cleanup(indio_dev);
+	regulator_disable(st->reg);
+	regulator_put(st->reg);
 	iio_device_free(indio_dev);
 
 	return 0;
 }
 
 static const struct spi_device_id ad7476_id[] = {
+	{"ad7091r", ID_AD7091R},
+	{"ad7273", ID_AD7277},
+	{"ad7274", ID_AD7276},
+	{"ad7276", ID_AD7276},
+	{"ad7277", ID_AD7277},
+	{"ad7278", ID_AD7278},
 	{"ad7466", ID_AD7466},
 	{"ad7467", ID_AD7467},
 	{"ad7468", ID_AD7468},
-	{"ad7475", ID_AD7475},
-	{"ad7476", ID_AD7476},
-	{"ad7476a", ID_AD7476},
-	{"ad7477", ID_AD7477},
-	{"ad7477a", ID_AD7477},
-	{"ad7478", ID_AD7478},
-	{"ad7478a", ID_AD7478},
+	{"ad7475", ID_AD7466},
+	{"ad7476", ID_AD7466},
+	{"ad7476a", ID_AD7466},
+	{"ad7477", ID_AD7467},
+	{"ad7477a", ID_AD7467},
+	{"ad7478", ID_AD7468},
+	{"ad7478a", ID_AD7468},
 	{"ad7495", ID_AD7495},
+	{"ad7910", ID_AD7467},
+	{"ad7920", ID_AD7466},
+	{"ad7940", ID_AD7940},
 	{}
 };
 MODULE_DEVICE_TABLE(spi, ad7476_id);
@@ -240,5 +328,5 @@ static struct spi_driver ad7476_driver = {
 module_spi_driver(ad7476_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
-MODULE_DESCRIPTION("Analog Devices AD7475/6/7/8(A) AD7466/7/8 ADC");
+MODULE_DESCRIPTION("Analog Devices AD7476 and similar 1-channel ADCs");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ad7791.c b/drivers/iio/adc/ad7791.c
new file mode 100644
index 000000000000..e93740843b2b
--- /dev/null
+++ b/drivers/iio/adc/ad7791.c
@@ -0,0 +1,460 @@
+/*
+ * AD7787/AD7788/AD7789/AD7790/AD7791 SPI ADC driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/adc/ad_sigma_delta.h>
+
+#include <linux/platform_data/ad7791.h>
+
+#define AD7791_REG_COMM			0x0 /* For writes */
+#define AD7791_REG_STATUS		0x0 /* For reads */
+#define AD7791_REG_MODE			0x1
+#define AD7791_REG_FILTER		0x2
+#define AD7791_REG_DATA			0x3
+
+#define AD7791_MODE_CONTINUOUS		0x00
+#define AD7791_MODE_SINGLE		0x02
+#define AD7791_MODE_POWERDOWN		0x03
+
+#define AD7791_CH_AIN1P_AIN1N		0x00
+#define AD7791_CH_AIN2			0x01
+#define AD7791_CH_AIN1N_AIN1N		0x02
+#define AD7791_CH_AVDD_MONITOR		0x03
+
+#define AD7791_FILTER_CLK_DIV_1		(0x0 << 4)
+#define AD7791_FILTER_CLK_DIV_2		(0x1 << 4)
+#define AD7791_FILTER_CLK_DIV_4		(0x2 << 4)
+#define AD7791_FILTER_CLK_DIV_8		(0x3 << 4)
+#define AD7791_FILTER_CLK_MASK		(0x3 << 4)
+#define AD7791_FILTER_RATE_120		0x0
+#define AD7791_FILTER_RATE_100		0x1
+#define AD7791_FILTER_RATE_33_3		0x2
+#define AD7791_FILTER_RATE_20		0x3
+#define AD7791_FILTER_RATE_16_6		0x4
+#define AD7791_FILTER_RATE_16_7		0x5
+#define AD7791_FILTER_RATE_13_3		0x6
+#define AD7791_FILTER_RATE_9_5		0x7
+#define AD7791_FILTER_RATE_MASK		0x7
+
+#define AD7791_MODE_BUFFER		BIT(1)
+#define AD7791_MODE_UNIPOLAR		BIT(2)
+#define AD7791_MODE_BURNOUT		BIT(3)
+#define AD7791_MODE_SEL_MASK		(0x3 << 6)
+#define AD7791_MODE_SEL(x)		((x) << 6)
+
+#define DECLARE_AD7787_CHANNELS(name, bits, storagebits) \
+const struct iio_chan_spec name[] = { \
+	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
+		(bits), (storagebits), 0), \
+	AD_SD_CHANNEL(1, 1, AD7791_CH_AIN2, (bits), (storagebits), 0), \
+	AD_SD_SHORTED_CHANNEL(2, 0, AD7791_CH_AIN1N_AIN1N, \
+		(bits), (storagebits), 0), \
+	AD_SD_SUPPLY_CHANNEL(3, 2, AD7791_CH_AVDD_MONITOR,  \
+		(bits), (storagebits), 0), \
+	IIO_CHAN_SOFT_TIMESTAMP(4), \
+}
+
+#define DECLARE_AD7791_CHANNELS(name, bits, storagebits) \
+const struct iio_chan_spec name[] = { \
+	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
+		(bits), (storagebits), 0), \
+	AD_SD_SHORTED_CHANNEL(1, 0, AD7791_CH_AIN1N_AIN1N, \
+		(bits), (storagebits), 0), \
+	AD_SD_SUPPLY_CHANNEL(2, 1, AD7791_CH_AVDD_MONITOR, \
+		(bits), (storagebits), 0), \
+	IIO_CHAN_SOFT_TIMESTAMP(3), \
+}
+
+static DECLARE_AD7787_CHANNELS(ad7787_channels, 24, 32);
+static DECLARE_AD7791_CHANNELS(ad7790_channels, 16, 16);
+static DECLARE_AD7791_CHANNELS(ad7791_channels, 24, 32);
+
+enum {
+	AD7787,
+	AD7788,
+	AD7789,
+	AD7790,
+	AD7791,
+};
+
+enum ad7791_chip_info_flags {
+	AD7791_FLAG_HAS_FILTER		= (1 << 0),
+	AD7791_FLAG_HAS_BUFFER		= (1 << 1),
+	AD7791_FLAG_HAS_UNIPOLAR	= (1 << 2),
+	AD7791_FLAG_HAS_BURNOUT		= (1 << 3),
+};
+
+struct ad7791_chip_info {
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+	enum ad7791_chip_info_flags flags;
+};
+
+static const struct ad7791_chip_info ad7791_chip_infos[] = {
+	[AD7787] = {
+		.channels = ad7787_channels,
+		.num_channels = ARRAY_SIZE(ad7787_channels),
+		.flags = AD7791_FLAG_HAS_FILTER | AD7791_FLAG_HAS_BUFFER |
+			AD7791_FLAG_HAS_UNIPOLAR | AD7791_FLAG_HAS_BURNOUT,
+	},
+	[AD7788] = {
+		.channels = ad7790_channels,
+		.num_channels = ARRAY_SIZE(ad7790_channels),
+		.flags = AD7791_FLAG_HAS_UNIPOLAR,
+	},
+	[AD7789] = {
+		.channels = ad7791_channels,
+		.num_channels = ARRAY_SIZE(ad7791_channels),
+		.flags = AD7791_FLAG_HAS_UNIPOLAR,
+	},
+	[AD7790] = {
+		.channels = ad7790_channels,
+		.num_channels = ARRAY_SIZE(ad7790_channels),
+		.flags = AD7791_FLAG_HAS_FILTER | AD7791_FLAG_HAS_BUFFER |
+			AD7791_FLAG_HAS_BURNOUT,
+	},
+	[AD7791] = {
+		.channels = ad7791_channels,
+		.num_channels = ARRAY_SIZE(ad7791_channels),
+		.flags = AD7791_FLAG_HAS_FILTER | AD7791_FLAG_HAS_BUFFER |
+			AD7791_FLAG_HAS_UNIPOLAR | AD7791_FLAG_HAS_BURNOUT,
+	},
+};
+
+struct ad7791_state {
+	struct ad_sigma_delta sd;
+	uint8_t mode;
+	uint8_t filter;
+
+	struct regulator *reg;
+	const struct ad7791_chip_info *info;
+};
+
+static struct ad7791_state *ad_sigma_delta_to_ad7791(struct ad_sigma_delta *sd)
+{
+	return container_of(sd, struct ad7791_state, sd);
+}
+
+static int ad7791_set_channel(struct ad_sigma_delta *sd, unsigned int channel)
+{
+	ad_sd_set_comm(sd, channel);
+
+	return 0;
+}
+
+static int ad7791_set_mode(struct ad_sigma_delta *sd,
+	enum ad_sigma_delta_mode mode)
+{
+	struct ad7791_state *st = ad_sigma_delta_to_ad7791(sd);
+
+	switch (mode) {
+	case AD_SD_MODE_CONTINUOUS:
+		mode = AD7791_MODE_CONTINUOUS;
+		break;
+	case AD_SD_MODE_SINGLE:
+		mode = AD7791_MODE_SINGLE;
+		break;
+	case AD_SD_MODE_IDLE:
+	case AD_SD_MODE_POWERDOWN:
+		mode = AD7791_MODE_POWERDOWN;
+		break;
+	}
+
+	st->mode &= ~AD7791_MODE_SEL_MASK;
+	st->mode |= AD7791_MODE_SEL(mode);
+
+	return ad_sd_write_reg(sd, AD7791_REG_MODE, sizeof(st->mode), st->mode);
+}
+
+static const struct ad_sigma_delta_info ad7791_sigma_delta_info = {
+	.set_channel = ad7791_set_channel,
+	.set_mode = ad7791_set_mode,
+	.has_registers = true,
+	.addr_shift = 4,
+	.read_mask = BIT(3),
+};
+
+static int ad7791_read_raw(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, int *val, int *val2, long info)
+{
+	struct ad7791_state *st = iio_priv(indio_dev);
+	bool unipolar = !!(st->mode & AD7791_MODE_UNIPOLAR);
+	unsigned long long scale_pv;
+
+	switch (info) {
+	case IIO_CHAN_INFO_RAW:
+		return ad_sigma_delta_single_conversion(indio_dev, chan, val);
+	case IIO_CHAN_INFO_OFFSET:
+		/**
+		 * Unipolar: 0 to VREF
+		 * Bipolar -VREF to VREF
+		 **/
+		if (unipolar)
+			*val = 0;
+		else
+			*val = -(1 << (chan->scan_type.realbits - 1));
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/* The monitor channel uses an internal reference. */
+		if (chan->address == AD7791_CH_AVDD_MONITOR) {
+			scale_pv = 5850000000000ULL;
+		} else {
+			int voltage_uv;
+
+			voltage_uv = regulator_get_voltage(st->reg);
+			if (voltage_uv < 0)
+				return voltage_uv;
+			scale_pv = (unsigned long long)voltage_uv * 1000000;
+		}
+		if (unipolar)
+			scale_pv >>= chan->scan_type.realbits;
+		else
+			scale_pv >>= chan->scan_type.realbits - 1;
+		*val2 = do_div(scale_pv, 1000000000);
+		*val = scale_pv;
+
+		return IIO_VAL_INT_PLUS_NANO;
+	}
+
+	return -EINVAL;
+}
+
+static const char * const ad7791_sample_freq_avail[] = {
+	[AD7791_FILTER_RATE_120] = "120",
+	[AD7791_FILTER_RATE_100] = "100",
+	[AD7791_FILTER_RATE_33_3] = "33.3",
+	[AD7791_FILTER_RATE_20] = "20",
+	[AD7791_FILTER_RATE_16_6] = "16.6",
+	[AD7791_FILTER_RATE_16_7] = "16.7",
+	[AD7791_FILTER_RATE_13_3] = "13.3",
+	[AD7791_FILTER_RATE_9_5] = "9.5",
+};
+
+static ssize_t ad7791_read_frequency(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7791_state *st = iio_priv(indio_dev);
+	unsigned int rate = st->filter & AD7791_FILTER_RATE_MASK;
+
+	return sprintf(buf, "%s\n", ad7791_sample_freq_avail[rate]);
+}
+
+static ssize_t ad7791_write_frequency(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ad7791_state *st = iio_priv(indio_dev);
+	int i, ret;
+
+	mutex_lock(&indio_dev->mlock);
+	if (iio_buffer_enabled(indio_dev)) {
+		mutex_unlock(&indio_dev->mlock);
+		return -EBUSY;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	ret = -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(ad7791_sample_freq_avail); i++) {
+		if (sysfs_streq(ad7791_sample_freq_avail[i], buf)) {
+
+			mutex_lock(&indio_dev->mlock);
+			st->filter &= ~AD7791_FILTER_RATE_MASK;
+			st->filter |= i;
+			ad_sd_write_reg(&st->sd, AD7791_REG_FILTER,
+					 sizeof(st->filter), st->filter);
+			mutex_unlock(&indio_dev->mlock);
+			ret = 0;
+			break;
+		}
+	}
+
+	return ret ? ret : len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+		ad7791_read_frequency,
+		ad7791_write_frequency);
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("120 100 33.3 20 16.7 16.6 13.3 9.5");
+
+static struct attribute *ad7791_attributes[] = {
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group ad7791_attribute_group = {
+	.attrs = ad7791_attributes,
+};
+
+static const struct iio_info ad7791_info = {
+	.read_raw = &ad7791_read_raw,
+	.attrs = &ad7791_attribute_group,
+	.validate_trigger = ad_sd_validate_trigger,
+	.driver_module = THIS_MODULE,
+};
+
+static const struct iio_info ad7791_no_filter_info = {
+	.read_raw = &ad7791_read_raw,
+	.validate_trigger = ad_sd_validate_trigger,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit ad7791_setup(struct ad7791_state *st,
+	struct ad7791_platform_data *pdata)
+{
+	/* Set to poweron-reset default values */
+	st->mode = AD7791_MODE_BUFFER;
+	st->filter = AD7791_FILTER_RATE_16_6;
+
+	if (!pdata)
+		return 0;
+
+	if ((st->info->flags & AD7791_FLAG_HAS_BUFFER) && !pdata->buffered)
+		st->mode &= ~AD7791_MODE_BUFFER;
+
+	if ((st->info->flags & AD7791_FLAG_HAS_BURNOUT) &&
+		pdata->burnout_current)
+		st->mode |= AD7791_MODE_BURNOUT;
+
+	if ((st->info->flags & AD7791_FLAG_HAS_UNIPOLAR) && pdata->unipolar)
+		st->mode |= AD7791_MODE_UNIPOLAR;
+
+	return ad_sd_write_reg(&st->sd, AD7791_REG_MODE, sizeof(st->mode),
+		st->mode);
+}
+
+static int __devinit ad7791_probe(struct spi_device *spi)
+{
+	struct ad7791_platform_data *pdata = spi->dev.platform_data;
+	struct iio_dev *indio_dev;
+	struct ad7791_state *st;
+	int ret;
+
+	if (!spi->irq) {
+		dev_err(&spi->dev, "Missing IRQ.\n");
+		return -ENXIO;
+	}
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = regulator_get(&spi->dev, "refin");
+	if (IS_ERR(st->reg)) {
+		ret = PTR_ERR(st->reg);
+		goto err_iio_free;
+	}
+
+	ret = regulator_enable(st->reg);
+	if (ret)
+		goto error_put_reg;
+
+	st->info = &ad7791_chip_infos[spi_get_device_id(spi)->driver_data];
+	ad_sd_init(&st->sd, indio_dev, spi, &ad7791_sigma_delta_info);
+
+	spi_set_drvdata(spi, indio_dev);
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = st->info->channels;
+	indio_dev->num_channels = st->info->num_channels;
+	if (st->info->flags & AD7791_FLAG_HAS_FILTER)
+		indio_dev->info = &ad7791_info;
+	else
+		indio_dev->info = &ad7791_no_filter_info;
+
+	ret = ad_sd_setup_buffer_and_trigger(indio_dev);
+	if (ret)
+		goto error_disable_reg;
+
+	ret = ad7791_setup(st, pdata);
+	if (ret)
+		goto error_remove_trigger;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_remove_trigger;
+
+	return 0;
+
+error_remove_trigger:
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
+error_disable_reg:
+	regulator_disable(st->reg);
+error_put_reg:
+	regulator_put(st->reg);
+err_iio_free:
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad7791_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad7791_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
+
+	regulator_disable(st->reg);
+	regulator_put(st->reg);
+
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad7791_spi_ids[] = {
+	{ "ad7787", AD7787 },
+	{ "ad7788", AD7788 },
+	{ "ad7789", AD7789 },
+	{ "ad7790", AD7790 },
+	{ "ad7791", AD7791 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad7791_spi_ids);
+
+static struct spi_driver ad7791_driver = {
+	.driver = {
+		.name	= "ad7791",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ad7791_probe,
+	.remove		= __devexit_p(ad7791_remove),
+	.id_table	= ad7791_spi_ids,
+};
+module_spi_driver(ad7791_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Device AD7787/AD7788/AD7789/AD7790/AD7791 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ad_sigma_delta.c b/drivers/iio/adc/ad_sigma_delta.c
new file mode 100644
index 000000000000..67baa1363d7a
--- /dev/null
+++ b/drivers/iio/adc/ad_sigma_delta.c
@@ -0,0 +1,558 @@
+/*
+ * Support code for Analog Devices Sigma-Delta ADCs
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/adc/ad_sigma_delta.h>
+
+#include <asm/unaligned.h>
+
+
+#define AD_SD_COMM_CHAN_MASK	0x3
+
+#define AD_SD_REG_COMM		0x00
+#define AD_SD_REG_DATA		0x03
+
+/**
+ * ad_sd_set_comm() - Set communications register
+ *
+ * @sigma_delta: The sigma delta device
+ * @comm: New value for the communications register
+ */
+void ad_sd_set_comm(struct ad_sigma_delta *sigma_delta, uint8_t comm)
+{
+	/* Some variants use the lower two bits of the communications register
+	 * to select the channel */
+	sigma_delta->comm = comm & AD_SD_COMM_CHAN_MASK;
+}
+EXPORT_SYMBOL_GPL(ad_sd_set_comm);
+
+/**
+ * ad_sd_write_reg() - Write a register
+ *
+ * @sigma_delta: The sigma delta device
+ * @reg: Address of the register
+ * @size: Size of the register (0-3)
+ * @val: Value to write to the register
+ *
+ * Returns 0 on success, an error code otherwise.
+ **/
+int ad_sd_write_reg(struct ad_sigma_delta *sigma_delta, unsigned int reg,
+	unsigned int size, unsigned int val)
+{
+	uint8_t *data = sigma_delta->data;
+	struct spi_transfer t = {
+		.tx_buf		= data,
+		.len		= size + 1,
+		.cs_change	= sigma_delta->bus_locked,
+	};
+	struct spi_message m;
+	int ret;
+
+	data[0] = (reg << sigma_delta->info->addr_shift) | sigma_delta->comm;
+
+	switch (size) {
+	case 3:
+		data[1] = val >> 16;
+		data[2] = val >> 8;
+		data[3] = val;
+		break;
+	case 2:
+		put_unaligned_be16(val, &data[1]);
+		break;
+	case 1:
+		data[1] = val;
+		break;
+	case 0:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+
+	if (sigma_delta->bus_locked)
+		ret = spi_sync_locked(sigma_delta->spi, &m);
+	else
+		ret = spi_sync(sigma_delta->spi, &m);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ad_sd_write_reg);
+
+static int ad_sd_read_reg_raw(struct ad_sigma_delta *sigma_delta,
+	unsigned int reg, unsigned int size, uint8_t *val)
+{
+	uint8_t *data = sigma_delta->data;
+	int ret;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = data,
+			.len = 1,
+		}, {
+			.rx_buf = val,
+			.len = size,
+			.cs_change = sigma_delta->bus_locked,
+		},
+	};
+	struct spi_message m;
+
+	spi_message_init(&m);
+
+	if (sigma_delta->info->has_registers) {
+		data[0] = reg << sigma_delta->info->addr_shift;
+		data[0] |= sigma_delta->info->read_mask;
+		spi_message_add_tail(&t[0], &m);
+	}
+	spi_message_add_tail(&t[1], &m);
+
+	if (sigma_delta->bus_locked)
+		ret = spi_sync_locked(sigma_delta->spi, &m);
+	else
+		ret = spi_sync(sigma_delta->spi, &m);
+
+	return ret;
+}
+
+/**
+ * ad_sd_read_reg() - Read a register
+ *
+ * @sigma_delta: The sigma delta device
+ * @reg: Address of the register
+ * @size: Size of the register (1-4)
+ * @val: Read value
+ *
+ * Returns 0 on success, an error code otherwise.
+ **/
+int ad_sd_read_reg(struct ad_sigma_delta *sigma_delta,
+	unsigned int reg, unsigned int size, unsigned int *val)
+{
+	int ret;
+
+	ret = ad_sd_read_reg_raw(sigma_delta, reg, size, sigma_delta->data);
+	if (ret < 0)
+		goto out;
+
+	switch (size) {
+	case 4:
+		*val = get_unaligned_be32(sigma_delta->data);
+		break;
+	case 3:
+		*val = (sigma_delta->data[0] << 16) |
+			(sigma_delta->data[1] << 8) |
+			sigma_delta->data[2];
+		break;
+	case 2:
+		*val = get_unaligned_be16(sigma_delta->data);
+		break;
+	case 1:
+		*val = sigma_delta->data[0];
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+out:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ad_sd_read_reg);
+
+static int ad_sd_calibrate(struct ad_sigma_delta *sigma_delta,
+	unsigned int mode, unsigned int channel)
+{
+	int ret;
+
+	ret = ad_sigma_delta_set_channel(sigma_delta, channel);
+	if (ret)
+		return ret;
+
+	spi_bus_lock(sigma_delta->spi->master);
+	sigma_delta->bus_locked = true;
+	INIT_COMPLETION(sigma_delta->completion);
+
+	ret = ad_sigma_delta_set_mode(sigma_delta, mode);
+	if (ret < 0)
+		goto out;
+
+	sigma_delta->irq_dis = false;
+	enable_irq(sigma_delta->spi->irq);
+	ret = wait_for_completion_timeout(&sigma_delta->completion, 2*HZ);
+	if (ret == 0) {
+		sigma_delta->irq_dis = true;
+		disable_irq_nosync(sigma_delta->spi->irq);
+		ret = -EIO;
+	} else {
+		ret = 0;
+	}
+out:
+	sigma_delta->bus_locked = false;
+	spi_bus_unlock(sigma_delta->spi->master);
+	ad_sigma_delta_set_mode(sigma_delta, AD_SD_MODE_IDLE);
+
+	return ret;
+}
+
+/**
+ * ad_sd_calibrate_all() - Performs channel calibration
+ * @sigma_delta: The sigma delta device
+ * @cb: Array of channels and calibration type to perform
+ * @n: Number of items in cb
+ *
+ * Returns 0 on success, an error code otherwise.
+ **/
+int ad_sd_calibrate_all(struct ad_sigma_delta *sigma_delta,
+	const struct ad_sd_calib_data *cb, unsigned int n)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < n; i++) {
+		ret = ad_sd_calibrate(sigma_delta, cb[i].mode, cb[i].channel);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ad_sd_calibrate_all);
+
+/**
+ * ad_sigma_delta_single_conversion() - Performs a single data conversion
+ * @indio_dev: The IIO device
+ * @chan: The conversion is done for this channel
+ * @val: Pointer to the location where to store the read value
+ *
+ * Returns: 0 on success, an error value otherwise.
+ */
+int ad_sigma_delta_single_conversion(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, int *val)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+	unsigned int sample, raw_sample;
+	int ret = 0;
+
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
+	mutex_lock(&indio_dev->mlock);
+	ad_sigma_delta_set_channel(sigma_delta, chan->address);
+
+	spi_bus_lock(sigma_delta->spi->master);
+	sigma_delta->bus_locked = true;
+	INIT_COMPLETION(sigma_delta->completion);
+
+	ad_sigma_delta_set_mode(sigma_delta, AD_SD_MODE_SINGLE);
+
+	sigma_delta->irq_dis = false;
+	enable_irq(sigma_delta->spi->irq);
+	ret = wait_for_completion_interruptible_timeout(
+			&sigma_delta->completion, HZ);
+
+	sigma_delta->bus_locked = false;
+	spi_bus_unlock(sigma_delta->spi->master);
+
+	if (ret == 0)
+		ret = -EIO;
+	if (ret < 0)
+		goto out;
+
+	ret = ad_sd_read_reg(sigma_delta, AD_SD_REG_DATA,
+		DIV_ROUND_UP(chan->scan_type.realbits + chan->scan_type.shift, 8),
+		&raw_sample);
+
+out:
+	if (!sigma_delta->irq_dis) {
+		disable_irq_nosync(sigma_delta->spi->irq);
+		sigma_delta->irq_dis = true;
+	}
+
+	ad_sigma_delta_set_mode(sigma_delta, AD_SD_MODE_IDLE);
+	mutex_unlock(&indio_dev->mlock);
+
+	if (ret)
+		return ret;
+
+	sample = raw_sample >> chan->scan_type.shift;
+	sample &= (1 << chan->scan_type.realbits) - 1;
+	*val = sample;
+
+	ret = ad_sigma_delta_postprocess_sample(sigma_delta, raw_sample);
+	if (ret)
+		return ret;
+
+	return IIO_VAL_INT;
+}
+EXPORT_SYMBOL_GPL(ad_sigma_delta_single_conversion);
+
+static int ad_sd_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+	unsigned int channel;
+	int ret;
+
+	ret = iio_triggered_buffer_postenable(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	channel = find_first_bit(indio_dev->active_scan_mask,
+				 indio_dev->masklength);
+	ret = ad_sigma_delta_set_channel(sigma_delta,
+		indio_dev->channels[channel].address);
+	if (ret)
+		goto err_predisable;
+
+	spi_bus_lock(sigma_delta->spi->master);
+	sigma_delta->bus_locked = true;
+	ret = ad_sigma_delta_set_mode(sigma_delta, AD_SD_MODE_CONTINUOUS);
+	if (ret)
+		goto err_unlock;
+
+	sigma_delta->irq_dis = false;
+	enable_irq(sigma_delta->spi->irq);
+
+	return 0;
+
+err_unlock:
+	spi_bus_unlock(sigma_delta->spi->master);
+err_predisable:
+
+	return ret;
+}
+
+static int ad_sd_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+
+	INIT_COMPLETION(sigma_delta->completion);
+	wait_for_completion_timeout(&sigma_delta->completion, HZ);
+
+	if (!sigma_delta->irq_dis) {
+		disable_irq_nosync(sigma_delta->spi->irq);
+		sigma_delta->irq_dis = true;
+	}
+
+	ad_sigma_delta_set_mode(sigma_delta, AD_SD_MODE_IDLE);
+
+	sigma_delta->bus_locked = false;
+	return spi_bus_unlock(sigma_delta->spi->master);
+}
+
+static irqreturn_t ad_sd_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+	unsigned int reg_size;
+	uint8_t data[16];
+	int ret;
+
+	memset(data, 0x00, 16);
+
+	/* Guaranteed to be aligned with 8 byte boundary */
+	if (indio_dev->scan_timestamp)
+		((s64 *)data)[1] = pf->timestamp;
+
+	reg_size = indio_dev->channels[0].scan_type.realbits +
+			indio_dev->channels[0].scan_type.shift;
+	reg_size = DIV_ROUND_UP(reg_size, 8);
+
+	switch (reg_size) {
+	case 4:
+	case 2:
+	case 1:
+		ret = ad_sd_read_reg_raw(sigma_delta, AD_SD_REG_DATA,
+			reg_size, &data[0]);
+		break;
+	case 3:
+		/* We store 24 bit samples in a 32 bit word. Keep the upper
+		 * byte set to zero. */
+		ret = ad_sd_read_reg_raw(sigma_delta, AD_SD_REG_DATA,
+			reg_size, &data[1]);
+		break;
+	}
+
+	iio_push_to_buffer(indio_dev->buffer, (uint8_t *)data);
+
+	iio_trigger_notify_done(indio_dev->trig);
+	sigma_delta->irq_dis = false;
+	enable_irq(sigma_delta->spi->irq);
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_buffer_setup_ops ad_sd_buffer_setup_ops = {
+	.preenable = &iio_sw_buffer_preenable,
+	.postenable = &ad_sd_buffer_postenable,
+	.predisable = &iio_triggered_buffer_predisable,
+	.postdisable = &ad_sd_buffer_postdisable,
+	.validate_scan_mask = &iio_validate_scan_mask_onehot,
+};
+
+static irqreturn_t ad_sd_data_rdy_trig_poll(int irq, void *private)
+{
+	struct ad_sigma_delta *sigma_delta = private;
+
+	complete(&sigma_delta->completion);
+	disable_irq_nosync(irq);
+	sigma_delta->irq_dis = true;
+	iio_trigger_poll(sigma_delta->trig, iio_get_time_ns());
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ad_sd_validate_trigger() - validate_trigger callback for ad_sigma_delta devices
+ * @indio_dev: The IIO device
+ * @trig: The new trigger
+ *
+ * Returns: 0 if the 'trig' matches the trigger registered by the ad_sigma_delta
+ * device, -EINVAL otherwise.
+ */
+int ad_sd_validate_trigger(struct iio_dev *indio_dev, struct iio_trigger *trig)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+
+	if (sigma_delta->trig != trig)
+		return -EINVAL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ad_sd_validate_trigger);
+
+static const struct iio_trigger_ops ad_sd_trigger_ops = {
+	.owner = THIS_MODULE,
+};
+
+static int ad_sd_probe_trigger(struct iio_dev *indio_dev)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+	int ret;
+
+	sigma_delta->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
+						indio_dev->id);
+	if (sigma_delta->trig == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	sigma_delta->trig->ops = &ad_sd_trigger_ops;
+	init_completion(&sigma_delta->completion);
+
+	ret = request_irq(sigma_delta->spi->irq,
+			  ad_sd_data_rdy_trig_poll,
+			  IRQF_TRIGGER_LOW,
+			  indio_dev->name,
+			  sigma_delta);
+	if (ret)
+		goto error_free_trig;
+
+	if (!sigma_delta->irq_dis) {
+		sigma_delta->irq_dis = true;
+		disable_irq_nosync(sigma_delta->spi->irq);
+	}
+	sigma_delta->trig->dev.parent = &sigma_delta->spi->dev;
+	sigma_delta->trig->private_data = sigma_delta;
+
+	ret = iio_trigger_register(sigma_delta->trig);
+	if (ret)
+		goto error_free_irq;
+
+	/* select default trigger */
+	indio_dev->trig = sigma_delta->trig;
+
+	return 0;
+
+error_free_irq:
+	free_irq(sigma_delta->spi->irq, sigma_delta);
+error_free_trig:
+	iio_trigger_free(sigma_delta->trig);
+error_ret:
+	return ret;
+}
+
+static void ad_sd_remove_trigger(struct iio_dev *indio_dev)
+{
+	struct ad_sigma_delta *sigma_delta = iio_device_get_drvdata(indio_dev);
+
+	iio_trigger_unregister(sigma_delta->trig);
+	free_irq(sigma_delta->spi->irq, sigma_delta);
+	iio_trigger_free(sigma_delta->trig);
+}
+
+/**
+ * ad_sd_setup_buffer_and_trigger() -
+ * @indio_dev: The IIO device
+ */
+int ad_sd_setup_buffer_and_trigger(struct iio_dev *indio_dev)
+{
+	int ret;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+			&ad_sd_trigger_handler, &ad_sd_buffer_setup_ops);
+	if (ret)
+		return ret;
+
+	ret = ad_sd_probe_trigger(indio_dev);
+	if (ret) {
+		iio_triggered_buffer_cleanup(indio_dev);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ad_sd_setup_buffer_and_trigger);
+
+/**
+ * ad_sd_cleanup_buffer_and_trigger() -
+ * @indio_dev: The IIO device
+ */
+void ad_sd_cleanup_buffer_and_trigger(struct iio_dev *indio_dev)
+{
+	ad_sd_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+}
+EXPORT_SYMBOL_GPL(ad_sd_cleanup_buffer_and_trigger);
+
+/**
+ * ad_sd_init() - Initializes a ad_sigma_delta struct
+ * @sigma_delta: The ad_sigma_delta device
+ * @indio_dev: The IIO device which the Sigma Delta device is used for
+ * @spi: The SPI device for the ad_sigma_delta device
+ * @info: Device specific callbacks and options
+ *
+ * This function needs to be called before any other operations are performed on
+ * the ad_sigma_delta struct.
+ */
+int ad_sd_init(struct ad_sigma_delta *sigma_delta, struct iio_dev *indio_dev,
+	struct spi_device *spi, const struct ad_sigma_delta_info *info)
+{
+	sigma_delta->spi = spi;
+	sigma_delta->info = info;
+	iio_device_set_drvdata(indio_dev, sigma_delta);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ad_sd_init);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices Sigma-Delta ADCs");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 3bd5540238a7..3ed94bf80596 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -82,7 +82,7 @@ static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
 		*timestamp = pf->timestamp;
 	}
 
-	buffer->access->store_to(buffer, (u8 *)st->buffer, pf->timestamp);
+	buffer->access->store_to(buffer, (u8 *)st->buffer);
 
 	iio_trigger_notify_done(idev->trig);
 	st->irq_enabled = true;
@@ -545,13 +545,6 @@ static int __devinit at91_adc_probe(struct platform_device *pdev)
 		goto error_free_device;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "No resource defined\n");
-		ret = -ENXIO;
-		goto error_ret;
-	}
-
 	platform_set_drvdata(pdev, idev);
 
 	idev->dev.parent = &pdev->dev;
@@ -566,18 +559,12 @@ static int __devinit at91_adc_probe(struct platform_device *pdev)
 		goto error_free_device;
 	}
 
-	if (!request_mem_region(res->start, resource_size(res),
-				"AT91 adc registers")) {
-		dev_err(&pdev->dev, "Resources are unavailable.\n");
-		ret = -EBUSY;
-		goto error_free_device;
-	}
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	st->reg_base = ioremap(res->start, resource_size(res));
+	st->reg_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!st->reg_base) {
-		dev_err(&pdev->dev, "Failed to map registers.\n");
 		ret = -ENOMEM;
-		goto error_release_mem;
+		goto error_free_device;
 	}
 
 	/*
@@ -592,45 +579,35 @@ static int __devinit at91_adc_probe(struct platform_device *pdev)
 			  idev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to allocate IRQ.\n");
-		goto error_unmap_reg;
+		goto error_free_device;
 	}
 
-	st->clk = clk_get(&pdev->dev, "adc_clk");
+	st->clk = devm_clk_get(&pdev->dev, "adc_clk");
 	if (IS_ERR(st->clk)) {
 		dev_err(&pdev->dev, "Failed to get the clock.\n");
 		ret = PTR_ERR(st->clk);
 		goto error_free_irq;
 	}
 
-	ret = clk_prepare(st->clk);
+	ret = clk_prepare_enable(st->clk);
 	if (ret) {
-		dev_err(&pdev->dev, "Could not prepare the clock.\n");
-		goto error_free_clk;
-	}
-
-	ret = clk_enable(st->clk);
-	if (ret) {
-		dev_err(&pdev->dev, "Could not enable the clock.\n");
-		goto error_unprepare_clk;
+		dev_err(&pdev->dev,
+			"Could not prepare or enable the clock.\n");
+		goto error_free_irq;
 	}
 
-	st->adc_clk = clk_get(&pdev->dev, "adc_op_clk");
+	st->adc_clk = devm_clk_get(&pdev->dev, "adc_op_clk");
 	if (IS_ERR(st->adc_clk)) {
 		dev_err(&pdev->dev, "Failed to get the ADC clock.\n");
 		ret = PTR_ERR(st->adc_clk);
 		goto error_disable_clk;
 	}
 
-	ret = clk_prepare(st->adc_clk);
+	ret = clk_prepare_enable(st->adc_clk);
 	if (ret) {
-		dev_err(&pdev->dev, "Could not prepare the ADC clock.\n");
-		goto error_free_adc_clk;
-	}
-
-	ret = clk_enable(st->adc_clk);
-	if (ret) {
-		dev_err(&pdev->dev, "Could not enable the ADC clock.\n");
-		goto error_unprepare_adc_clk;
+		dev_err(&pdev->dev,
+			"Could not prepare or enable the ADC clock.\n");
+		goto error_disable_clk;
 	}
 
 	/*
@@ -694,23 +671,11 @@ error_remove_triggers:
 error_unregister_buffer:
 	at91_adc_buffer_remove(idev);
 error_disable_adc_clk:
-	clk_disable(st->adc_clk);
-error_unprepare_adc_clk:
-	clk_unprepare(st->adc_clk);
-error_free_adc_clk:
-	clk_put(st->adc_clk);
+	clk_disable_unprepare(st->adc_clk);
 error_disable_clk:
-	clk_disable(st->clk);
-error_unprepare_clk:
-	clk_unprepare(st->clk);
-error_free_clk:
-	clk_put(st->clk);
+	clk_disable_unprepare(st->clk);
 error_free_irq:
 	free_irq(st->irq, idev);
-error_unmap_reg:
-	iounmap(st->reg_base);
-error_release_mem:
-	release_mem_region(res->start, resource_size(res));
 error_free_device:
 	iio_device_free(idev);
 error_ret:
@@ -720,20 +685,14 @@ error_ret:
 static int __devexit at91_adc_remove(struct platform_device *pdev)
 {
 	struct iio_dev *idev = platform_get_drvdata(pdev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct at91_adc_state *st = iio_priv(idev);
 
 	iio_device_unregister(idev);
 	at91_adc_trigger_remove(idev);
 	at91_adc_buffer_remove(idev);
 	clk_disable_unprepare(st->adc_clk);
-	clk_put(st->adc_clk);
-	clk_disable(st->clk);
-	clk_unprepare(st->clk);
-	clk_put(st->clk);
+	clk_disable_unprepare(st->clk);
 	free_irq(st->irq, idev);
-	iounmap(st->reg_base);
-	release_mem_region(res->start, resource_size(res));
 	iio_device_free(idev);
 
 	return 0;
diff --git a/drivers/iio/adc/lp8788_adc.c b/drivers/iio/adc/lp8788_adc.c
new file mode 100644
index 000000000000..a93aaf0bb841
--- /dev/null
+++ b/drivers/iio/adc/lp8788_adc.c
@@ -0,0 +1,264 @@
+/*
+ * TI LP8788 MFD - ADC driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/machine.h>
+#include <linux/mfd/lp8788.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+/* register address */
+#define LP8788_ADC_CONF			0x60
+#define LP8788_ADC_RAW			0x61
+#define LP8788_ADC_DONE			0x63
+
+#define ADC_CONV_START			1
+
+struct lp8788_adc {
+	struct lp8788 *lp;
+	struct iio_map *map;
+	struct mutex lock;
+};
+
+static const int lp8788_scale[LPADC_MAX] = {
+	[LPADC_VBATT_5P5] = 1343101,
+	[LPADC_VIN_CHG]   = 3052503,
+	[LPADC_IBATT]     = 610500,
+	[LPADC_IC_TEMP]   = 61050,
+	[LPADC_VBATT_6P0] = 1465201,
+	[LPADC_VBATT_5P0] = 1221001,
+	[LPADC_ADC1]      = 610500,
+	[LPADC_ADC2]      = 610500,
+	[LPADC_VDD]       = 1025641,
+	[LPADC_VCOIN]     = 757020,
+	[LPADC_ADC3]      = 610500,
+	[LPADC_ADC4]      = 610500,
+};
+
+static int lp8788_get_adc_result(struct lp8788_adc *adc, enum lp8788_adc_id id,
+				int *val)
+{
+	unsigned int msb;
+	unsigned int lsb;
+	unsigned int result;
+	u8 data;
+	u8 rawdata[2];
+	int size = ARRAY_SIZE(rawdata);
+	int retry = 5;
+	int ret;
+
+	data = (id << 1) | ADC_CONV_START;
+	ret = lp8788_write_byte(adc->lp, LP8788_ADC_CONF, data);
+	if (ret)
+		goto err_io;
+
+	/* retry until adc conversion is done */
+	data = 0;
+	while (retry--) {
+		usleep_range(100, 200);
+
+		ret = lp8788_read_byte(adc->lp, LP8788_ADC_DONE, &data);
+		if (ret)
+			goto err_io;
+
+		/* conversion done */
+		if (data)
+			break;
+	}
+
+	ret = lp8788_read_multi_bytes(adc->lp, LP8788_ADC_RAW, rawdata, size);
+	if (ret)
+		goto err_io;
+
+	msb = (rawdata[0] << 4) & 0x00000ff0;
+	lsb = (rawdata[1] >> 4) & 0x0000000f;
+	result = msb | lsb;
+	*val = result;
+
+	return 0;
+
+err_io:
+	return ret;
+}
+
+static int lp8788_adc_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val, int *val2, long mask)
+{
+	struct lp8788_adc *adc = iio_priv(indio_dev);
+	enum lp8788_adc_id id = chan->channel;
+	int ret;
+
+	mutex_lock(&adc->lock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = lp8788_get_adc_result(adc, id, val) ? -EIO : IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = lp8788_scale[id] / 1000000;
+		*val2 = lp8788_scale[id] % 1000000;
+		ret = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&adc->lock);
+
+	return ret;
+}
+
+static const struct iio_info lp8788_adc_info = {
+	.read_raw = &lp8788_adc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define LP8788_CHAN(_id, _type) {				\
+		.type = _type,					\
+		.indexed = 1,					\
+		.channel = LPADC_##_id,				\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
+		.datasheet_name = #_id,				\
+}
+
+static const struct iio_chan_spec lp8788_adc_channels[] = {
+	[LPADC_VBATT_5P5] = LP8788_CHAN(VBATT_5P5, IIO_VOLTAGE),
+	[LPADC_VIN_CHG]   = LP8788_CHAN(VIN_CHG, IIO_VOLTAGE),
+	[LPADC_IBATT]     = LP8788_CHAN(IBATT, IIO_CURRENT),
+	[LPADC_IC_TEMP]   = LP8788_CHAN(IC_TEMP, IIO_TEMP),
+	[LPADC_VBATT_6P0] = LP8788_CHAN(VBATT_6P0, IIO_VOLTAGE),
+	[LPADC_VBATT_5P0] = LP8788_CHAN(VBATT_5P0, IIO_VOLTAGE),
+	[LPADC_ADC1]      = LP8788_CHAN(ADC1, IIO_VOLTAGE),
+	[LPADC_ADC2]      = LP8788_CHAN(ADC2, IIO_VOLTAGE),
+	[LPADC_VDD]       = LP8788_CHAN(VDD, IIO_VOLTAGE),
+	[LPADC_VCOIN]     = LP8788_CHAN(VCOIN, IIO_VOLTAGE),
+	[LPADC_ADC3]      = LP8788_CHAN(ADC3, IIO_VOLTAGE),
+	[LPADC_ADC4]      = LP8788_CHAN(ADC4, IIO_VOLTAGE),
+};
+
+/* default maps used by iio consumer (lp8788-charger driver) */
+static struct iio_map lp8788_default_iio_maps[] = {
+	{
+		.consumer_dev_name = "lp8788-charger",
+		.consumer_channel = "lp8788_vbatt_5p0",
+		.adc_channel_label = "VBATT_5P0",
+	},
+	{
+		.consumer_dev_name = "lp8788-charger",
+		.consumer_channel = "lp8788_adc1",
+		.adc_channel_label = "ADC1",
+	},
+	{ }
+};
+
+static int lp8788_iio_map_register(struct iio_dev *indio_dev,
+				struct lp8788_platform_data *pdata,
+				struct lp8788_adc *adc)
+{
+	struct iio_map *map;
+	int ret;
+
+	map = (!pdata || !pdata->adc_pdata) ?
+		lp8788_default_iio_maps : pdata->adc_pdata;
+
+	ret = iio_map_array_register(indio_dev, map);
+	if (ret) {
+		dev_err(adc->lp->dev, "iio map err: %d\n", ret);
+		return ret;
+	}
+
+	adc->map = map;
+	return 0;
+}
+
+static inline void lp8788_iio_map_unregister(struct iio_dev *indio_dev,
+				struct lp8788_adc *adc)
+{
+	iio_map_array_unregister(indio_dev, adc->map);
+}
+
+static int __devinit lp8788_adc_probe(struct platform_device *pdev)
+{
+	struct lp8788 *lp = dev_get_drvdata(pdev->dev.parent);
+	struct iio_dev *indio_dev;
+	struct lp8788_adc *adc;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc = iio_priv(indio_dev);
+	adc->lp = lp;
+	platform_set_drvdata(pdev, indio_dev);
+
+	ret = lp8788_iio_map_register(indio_dev, lp->pdata, adc);
+	if (ret)
+		goto err_iio_map;
+
+	mutex_init(&adc->lock);
+
+	indio_dev->dev.parent = lp->dev;
+	indio_dev->name = pdev->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &lp8788_adc_info;
+	indio_dev->channels = lp8788_adc_channels;
+	indio_dev->num_channels = ARRAY_SIZE(lp8788_adc_channels);
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(lp->dev, "iio dev register err: %d\n", ret);
+		goto err_iio_device;
+	}
+
+	return 0;
+
+err_iio_device:
+	lp8788_iio_map_unregister(indio_dev, adc);
+err_iio_map:
+	iio_device_free(indio_dev);
+	return ret;
+}
+
+static int __devexit lp8788_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct lp8788_adc *adc = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	lp8788_iio_map_unregister(indio_dev, adc);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver lp8788_adc_driver = {
+	.probe = lp8788_adc_probe,
+	.remove = __devexit_p(lp8788_adc_remove),
+	.driver = {
+		.name = LP8788_DEV_ADC,
+		.owner = THIS_MODULE,
+	},
+};
+module_platform_driver(lp8788_adc_driver);
+
+MODULE_DESCRIPTION("Texas Instruments LP8788 ADC Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lp8788-adc");
diff --git a/drivers/iio/common/Kconfig b/drivers/iio/common/Kconfig
new file mode 100644
index 000000000000..ed45ee54500c
--- /dev/null
+++ b/drivers/iio/common/Kconfig
@@ -0,0 +1,5 @@
+#
+# IIO common modules
+#
+
+source "drivers/iio/common/hid-sensors/Kconfig"
diff --git a/drivers/iio/common/Makefile b/drivers/iio/common/Makefile
new file mode 100644
index 000000000000..81584009b21b
--- /dev/null
+++ b/drivers/iio/common/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the IIO common modules.
+# Common modules contains modules, which can be shared among multiple
+# IIO modules. For example if the trigger processing is common for
+# multiple IIO modules then this can be moved to a common module
+# instead of duplicating in each module.
+#
+
+obj-y += hid-sensors/
diff --git a/drivers/iio/common/hid-sensors/Kconfig b/drivers/iio/common/hid-sensors/Kconfig
new file mode 100644
index 000000000000..8e63d81d652a
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/Kconfig
@@ -0,0 +1,26 @@
+#
+# Hid Sensor common modules
+#
+menu "Hid Sensor IIO Common"
+
+config HID_SENSOR_IIO_COMMON
+	tristate "Common modules for all HID Sensor IIO drivers"
+	depends on HID_SENSOR_HUB
+	select IIO_TRIGGER if IIO_BUFFER
+	help
+	  Say yes here to build support for HID sensor to use
+	  HID sensor common processing for attributes and IIO triggers.
+	  There are many attributes which can be shared among multiple
+	  HID sensor drivers, this module contains processing for those
+	  attributes.
+
+config HID_SENSOR_ENUM_BASE_QUIRKS
+	tristate "ENUM base quirks for HID Sensor IIO drivers"
+	depends on HID_SENSOR_IIO_COMMON
+	help
+	  Say yes here to build support for sensor hub FW using
+	  enumeration, which is using 1 as base instead of 0.
+	  Since logical minimum is still set 0 instead of 1,
+	  there is no easy way to differentiate.
+
+endmenu
diff --git a/drivers/iio/common/hid-sensors/Makefile b/drivers/iio/common/hid-sensors/Makefile
new file mode 100644
index 000000000000..1f463e00c242
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the Hid sensor common modules.
+#
+
+obj-$(CONFIG_HID_SENSOR_IIO_COMMON) += hid-sensor-iio-common.o
+hid-sensor-iio-common-y := hid-sensor-attributes.o hid-sensor-trigger.o
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
new file mode 100644
index 000000000000..75374955caba
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -0,0 +1,250 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include "hid-sensor-attributes.h"
+
+static int pow_10(unsigned power)
+{
+	int i;
+	int ret = 1;
+	for (i = 0; i < power; ++i)
+		ret = ret * 10;
+
+	return ret;
+}
+
+static void simple_div(int dividend, int divisor, int *whole,
+				int *micro_frac)
+{
+	int rem;
+	int exp = 0;
+
+	*micro_frac = 0;
+	if (divisor == 0) {
+		*whole = 0;
+		return;
+	}
+	*whole = dividend/divisor;
+	rem = dividend % divisor;
+	if (rem) {
+		while (rem <= divisor) {
+			rem *= 10;
+			exp++;
+		}
+		*micro_frac = (rem / divisor) * pow_10(6-exp);
+	}
+}
+
+static void split_micro_fraction(unsigned int no, int exp, int *val1, int *val2)
+{
+	*val1 = no/pow_10(exp);
+	*val2 = no%pow_10(exp) * pow_10(6-exp);
+}
+
+/*
+VTF format uses exponent and variable size format.
+For example if the size is 2 bytes
+0x0067 with VTF16E14 format -> +1.03
+To convert just change to 0x67 to decimal and use two decimal as E14 stands
+for 10^-2.
+Negative numbers are 2's complement
+*/
+static void convert_from_vtf_format(u32 value, int size, int exp,
+					int *val1, int *val2)
+{
+	int sign = 1;
+
+	if (value & BIT(size*8 - 1)) {
+		value =  ((1LL << (size * 8)) - value);
+		sign = -1;
+	}
+	exp = hid_sensor_convert_exponent(exp);
+	if (exp >= 0) {
+		*val1 = sign * value * pow_10(exp);
+		*val2 = 0;
+	} else {
+		split_micro_fraction(value, -exp, val1, val2);
+		if (*val1)
+			*val1 = sign * (*val1);
+		else
+			*val2 = sign * (*val2);
+	}
+}
+
+static u32 convert_to_vtf_format(int size, int exp, int val1, int val2)
+{
+	u32 value;
+	int sign = 1;
+
+	if (val1 < 0 || val2 < 0)
+		sign = -1;
+	exp = hid_sensor_convert_exponent(exp);
+	if (exp < 0) {
+		value = abs(val1) * pow_10(-exp);
+		value += abs(val2) / pow_10(6+exp);
+	} else
+		value = abs(val1) / pow_10(exp);
+	if (sign < 0)
+		value =  ((1LL << (size * 8)) - value);
+
+	return value;
+}
+
+int hid_sensor_read_samp_freq_value(struct hid_sensor_iio_common *st,
+				int *val1, int *val2)
+{
+	s32 value;
+	int ret;
+
+	ret = sensor_hub_get_feature(st->hsdev,
+		st->poll.report_id,
+		st->poll.index, &value);
+	if (ret < 0 || value < 0) {
+		*val1 = *val2 = 0;
+		return -EINVAL;
+	} else {
+		if (st->poll.units == HID_USAGE_SENSOR_UNITS_MILLISECOND)
+			simple_div(1000, value, val1, val2);
+		else if (st->poll.units == HID_USAGE_SENSOR_UNITS_SECOND)
+			simple_div(1, value, val1, val2);
+		else {
+			*val1 = *val2 = 0;
+			return -EINVAL;
+		}
+	}
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+EXPORT_SYMBOL(hid_sensor_read_samp_freq_value);
+
+int hid_sensor_write_samp_freq_value(struct hid_sensor_iio_common *st,
+				int val1, int val2)
+{
+	s32 value;
+	int ret;
+
+	if (val1 < 0 || val2 < 0)
+		ret = -EINVAL;
+
+	value = val1 * pow_10(6) + val2;
+	if (value) {
+		if (st->poll.units == HID_USAGE_SENSOR_UNITS_MILLISECOND)
+			value = pow_10(9)/value;
+		else if (st->poll.units == HID_USAGE_SENSOR_UNITS_SECOND)
+			value = pow_10(6)/value;
+		else
+			value = 0;
+	}
+	ret = sensor_hub_set_feature(st->hsdev,
+		st->poll.report_id,
+		st->poll.index, value);
+	if (ret < 0 || value < 0)
+		ret = -EINVAL;
+
+	return ret;
+}
+EXPORT_SYMBOL(hid_sensor_write_samp_freq_value);
+
+int hid_sensor_read_raw_hyst_value(struct hid_sensor_iio_common *st,
+				int *val1, int *val2)
+{
+	s32 value;
+	int ret;
+
+	ret = sensor_hub_get_feature(st->hsdev,
+		st->sensitivity.report_id,
+		st->sensitivity.index, &value);
+	if (ret < 0 || value < 0) {
+		*val1 = *val2 = 0;
+		return -EINVAL;
+	} else {
+		convert_from_vtf_format(value, st->sensitivity.size,
+					st->sensitivity.unit_expo,
+					val1, val2);
+	}
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+EXPORT_SYMBOL(hid_sensor_read_raw_hyst_value);
+
+int hid_sensor_write_raw_hyst_value(struct hid_sensor_iio_common *st,
+					int val1, int val2)
+{
+	s32 value;
+	int ret;
+
+	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);
+	if (ret < 0 || value < 0)
+		ret = -EINVAL;
+
+	return ret;
+}
+EXPORT_SYMBOL(hid_sensor_write_raw_hyst_value);
+
+int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+					u32 usage_id,
+					struct hid_sensor_iio_common *st)
+{
+
+	sensor_hub_input_get_attribute_info(hsdev,
+					HID_FEATURE_REPORT, usage_id,
+					HID_USAGE_SENSOR_PROP_REPORT_INTERVAL,
+					&st->poll);
+
+	sensor_hub_input_get_attribute_info(hsdev,
+					HID_FEATURE_REPORT, usage_id,
+					HID_USAGE_SENSOR_PROP_REPORT_STATE,
+					&st->report_state);
+
+	sensor_hub_input_get_attribute_info(hsdev,
+					HID_FEATURE_REPORT, usage_id,
+					HID_USAGE_SENSOR_PROY_POWER_STATE,
+					&st->power_state);
+
+	sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_PROP_SENSITIVITY_ABS,
+			 &st->sensitivity);
+
+	hid_dbg(hsdev->hdev, "common attributes: %x:%x, %x:%x, %x:%x %x:%x\n",
+			st->poll.index, st->poll.report_id,
+			st->report_state.index, st->report_state.report_id,
+			st->power_state.index, st->power_state.report_id,
+			st->sensitivity.index, st->sensitivity.report_id);
+
+	return 0;
+}
+EXPORT_SYMBOL(hid_sensor_parse_common_attributes);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_DESCRIPTION("HID Sensor common attribute processing");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.h b/drivers/iio/common/hid-sensors/hid-sensor-attributes.h
new file mode 100644
index 000000000000..a4676a0c3de5
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.h
@@ -0,0 +1,57 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#ifndef _HID_SENSORS_ATTRIBUTES_H
+#define _HID_SENSORS_ATTRIBUTES_H
+
+/* Common hid sensor iio structure */
+struct hid_sensor_iio_common {
+	struct hid_sensor_hub_device *hsdev;
+	struct platform_device *pdev;
+	unsigned usage_id;
+	bool data_ready;
+	struct hid_sensor_hub_attribute_info poll;
+	struct hid_sensor_hub_attribute_info report_state;
+	struct hid_sensor_hub_attribute_info power_state;
+	struct hid_sensor_hub_attribute_info sensitivity;
+};
+
+/*Convert from hid unit expo to regular exponent*/
+static inline int hid_sensor_convert_exponent(int unit_expo)
+{
+	if (unit_expo < 0x08)
+		return unit_expo;
+	else if (unit_expo <= 0x0f)
+		return -(0x0f-unit_expo+1);
+	else
+		return 0;
+}
+
+int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+					u32 usage_id,
+					struct hid_sensor_iio_common *st);
+int hid_sensor_write_raw_hyst_value(struct hid_sensor_iio_common *st,
+					int val1, int val2);
+int hid_sensor_read_raw_hyst_value(struct hid_sensor_iio_common *st,
+					int *val1, int *val2);
+int hid_sensor_write_samp_freq_value(struct hid_sensor_iio_common *st,
+					int val1, int val2);
+int hid_sensor_read_samp_freq_value(struct hid_sensor_iio_common *st,
+					int *val1, int *val2);
+
+#endif
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
new file mode 100644
index 000000000000..d4b790d18efb
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -0,0 +1,103 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/sysfs.h>
+#include "hid-sensor-attributes.h"
+#include "hid-sensor-trigger.h"
+
+static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						bool state)
+{
+	struct hid_sensor_iio_common *st = trig->private_data;
+	int state_val;
+
+	state_val = state ? 1 : 0;
+#if (defined CONFIG_HID_SENSOR_ENUM_BASE_QUIRKS) || \
+	(defined CONFIG_HID_SENSOR_ENUM_BASE_QUIRKS_MODULE)
+	++state_val;
+#endif
+	st->data_ready = state;
+	sensor_hub_set_feature(st->hsdev, st->power_state.report_id,
+					st->power_state.index,
+					(s32)state_val);
+
+	sensor_hub_set_feature(st->hsdev, st->report_state.report_id,
+					st->report_state.index,
+					(s32)state_val);
+
+	return 0;
+}
+
+void hid_sensor_remove_trigger(struct iio_dev *indio_dev)
+{
+	iio_trigger_unregister(indio_dev->trig);
+	iio_trigger_free(indio_dev->trig);
+	indio_dev->trig = NULL;
+}
+EXPORT_SYMBOL(hid_sensor_remove_trigger);
+
+static const struct iio_trigger_ops hid_sensor_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = &hid_sensor_data_rdy_trigger_set_state,
+};
+
+int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
+				struct hid_sensor_iio_common *attrb)
+{
+	int ret;
+	struct iio_trigger *trig;
+
+	trig = iio_trigger_alloc("%s-dev%d", name, indio_dev->id);
+	if (trig == NULL) {
+		dev_err(&indio_dev->dev, "Trigger Allocate Failed\n");
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	trig->dev.parent = indio_dev->dev.parent;
+	trig->private_data = attrb;
+	trig->ops = &hid_sensor_trigger_ops;
+	ret = iio_trigger_register(trig);
+
+	if (ret) {
+		dev_err(&indio_dev->dev, "Trigger Register Failed\n");
+		goto error_free_trig;
+	}
+	indio_dev->trig = trig;
+
+	return ret;
+
+error_free_trig:
+	iio_trigger_free(trig);
+error_ret:
+	return ret;
+}
+EXPORT_SYMBOL(hid_sensor_setup_trigger);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_DESCRIPTION("HID Sensor trigger processing");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
new file mode 100644
index 000000000000..fd982971b1b8
--- /dev/null
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
@@ -0,0 +1,26 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#ifndef _HID_SENSOR_TRIGGER_H
+#define _HID_SENSOR_TRIGGER_H
+
+int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
+				struct hid_sensor_iio_common *attrb);
+void hid_sensor_remove_trigger(struct iio_dev *indio_dev);
+
+#endif
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index 1be15fa9d618..b1c0ee5294ca 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -57,11 +57,12 @@ config AD5624R_SPI
 
 config AD5446
 	tristate "Analog Devices AD5446 and similar single channel DACs driver"
-	depends on SPI
+	depends on (SPI_MASTER || I2C)
 	help
-	  Say yes here to build support for Analog Devices AD5444, AD5446, AD5450,
-	  AD5451, AD5452, AD5453, AD5512A, AD5541A, AD5542A, AD5543, AD5553, AD5601,
-	  AD5611, AD5620, AD5621, AD5640, AD5660, AD5662 DACs.
+	  Say yes here to build support for Analog Devices AD5300, AD5301, AD5310,
+	  AD5311, AD5320, AD5321, AD5444, AD5446, AD5450, AD5451, AD5452, AD5453,
+	  AD5512A, AD5541A, AD5542A, AD5543, AD5553, AD5601, AD5602, AD5611, AD5612,
+	  AD5620, AD5621, AD5622, AD5640, AD5660, AD5662 DACs.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5446.
@@ -76,6 +77,17 @@ config AD5504
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5504.
 
+config AD5755
+	tristate "Analog Devices AD5755/AD5755-1/AD5757/AD5735/AD5737 DAC driver"
+	depends on SPI_MASTER
+	help
+	  Say yes here to build support for Analog Devices AD5755, AD5755-1,
+	  AD5757, AD5735, AD5737 quad channel Digital to
+	  Analog Converter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad5755.
+
 config AD5764
 	tristate "Analog Devices AD5764/64R/44/44R DAC driver"
 	depends on SPI_MASTER
diff --git a/drivers/iio/dac/Makefile b/drivers/iio/dac/Makefile
index 9ea3ceeefc07..c0d333b23ba3 100644
--- a/drivers/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_AD5624R_SPI) += ad5624r_spi.o
 obj-$(CONFIG_AD5064) += ad5064.o
 obj-$(CONFIG_AD5504) += ad5504.o
 obj-$(CONFIG_AD5446) += ad5446.o
+obj-$(CONFIG_AD5755) += ad5755.o
 obj-$(CONFIG_AD5764) += ad5764.o
 obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
diff --git a/drivers/iio/dac/ad5446.c b/drivers/iio/dac/ad5446.c
index 2ca5059ef89e..3310cbbd41e7 100644
--- a/drivers/iio/dac/ad5446.c
+++ b/drivers/iio/dac/ad5446.c
@@ -14,6 +14,7 @@
 #include <linux/sysfs.h>
 #include <linux/list.h>
 #include <linux/spi/spi.h>
+#include <linux/i2c.h>
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
 #include <linux/module.h>
@@ -21,24 +22,40 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
-#include "ad5446.h"
+#define MODE_PWRDWN_1k		0x1
+#define MODE_PWRDWN_100k	0x2
+#define MODE_PWRDWN_TRISTATE	0x3
 
-static int ad5446_write(struct ad5446_state *st, unsigned val)
-{
-	__be16 data = cpu_to_be16(val);
-	return spi_write(st->spi, &data, sizeof(data));
-}
+/**
+ * struct ad5446_state - driver instance specific data
+ * @spi:		spi_device
+ * @chip_info:		chip model specific constants, available modes etc
+ * @reg:		supply regulator
+ * @vref_mv:		actual reference voltage used
+ */
 
-static int ad5660_write(struct ad5446_state *st, unsigned val)
-{
-	uint8_t data[3];
+struct ad5446_state {
+	struct device		*dev;
+	const struct ad5446_chip_info	*chip_info;
+	struct regulator		*reg;
+	unsigned short			vref_mv;
+	unsigned			cached_val;
+	unsigned			pwr_down_mode;
+	unsigned			pwr_down;
+};
 
-	data[0] = (val >> 16) & 0xFF;
-	data[1] = (val >> 8) & 0xFF;
-	data[2] = val & 0xFF;
+/**
+ * struct ad5446_chip_info - chip specific information
+ * @channel:		channel spec for the DAC
+ * @int_vref_mv:	AD5620/40/60: the internal reference voltage
+ * @write:		chip specific helper function to write to the register
+ */
 
-	return spi_write(st->spi, data, sizeof(data));
-}
+struct ad5446_chip_info {
+	struct iio_chan_spec	channel;
+	u16			int_vref_mv;
+	int			(*write)(struct ad5446_state *st, unsigned val);
+};
 
 static const char * const ad5446_powerdown_modes[] = {
 	"1kohm_to_gnd", "100kohm_to_gnd", "three_state"
@@ -110,7 +127,7 @@ static ssize_t ad5446_write_dac_powerdown(struct iio_dev *indio_dev,
 	return ret ? ret : len;
 }
 
-static const struct iio_chan_spec_ext_info ad5064_ext_info_powerdown[] = {
+static const struct iio_chan_spec_ext_info ad5446_ext_info_powerdown[] = {
 	{
 		.name = "powerdown",
 		.read = ad5446_read_dac_powerdown,
@@ -136,84 +153,7 @@ static const struct iio_chan_spec_ext_info ad5064_ext_info_powerdown[] = {
 	_AD5446_CHANNEL(bits, storage, shift, NULL)
 
 #define AD5446_CHANNEL_POWERDOWN(bits, storage, shift) \
-	_AD5446_CHANNEL(bits, storage, shift, ad5064_ext_info_powerdown)
-
-static const struct ad5446_chip_info ad5446_chip_info_tbl[] = {
-	[ID_AD5444] = {
-		.channel = AD5446_CHANNEL(12, 16, 2),
-		.write = ad5446_write,
-	},
-	[ID_AD5446] = {
-		.channel = AD5446_CHANNEL(14, 16, 0),
-		.write = ad5446_write,
-	},
-	[ID_AD5450] = {
-		.channel = AD5446_CHANNEL(8, 16, 6),
-		.write = ad5446_write,
-	},
-	[ID_AD5451] = {
-		.channel = AD5446_CHANNEL(10, 16, 4),
-		.write = ad5446_write,
-	},
-	[ID_AD5541A] = {
-		.channel = AD5446_CHANNEL(16, 16, 0),
-		.write = ad5446_write,
-	},
-	[ID_AD5512A] = {
-		.channel = AD5446_CHANNEL(12, 16, 4),
-		.write = ad5446_write,
-	},
-	[ID_AD5553] = {
-		.channel = AD5446_CHANNEL(14, 16, 0),
-		.write = ad5446_write,
-	},
-	[ID_AD5601] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(8, 16, 6),
-		.write = ad5446_write,
-	},
-	[ID_AD5611] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(10, 16, 4),
-		.write = ad5446_write,
-	},
-	[ID_AD5621] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
-		.write = ad5446_write,
-	},
-	[ID_AD5620_2500] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
-		.int_vref_mv = 2500,
-		.write = ad5446_write,
-	},
-	[ID_AD5620_1250] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
-		.int_vref_mv = 1250,
-		.write = ad5446_write,
-	},
-	[ID_AD5640_2500] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
-		.int_vref_mv = 2500,
-		.write = ad5446_write,
-	},
-	[ID_AD5640_1250] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
-		.int_vref_mv = 1250,
-		.write = ad5446_write,
-	},
-	[ID_AD5660_2500] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
-		.int_vref_mv = 2500,
-		.write = ad5660_write,
-	},
-	[ID_AD5660_1250] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
-		.int_vref_mv = 1250,
-		.write = ad5660_write,
-	},
-	[ID_AD5662] = {
-		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
-		.write = ad5660_write,
-	},
-};
+	_AD5446_CHANNEL(bits, storage, shift, ad5446_ext_info_powerdown)
 
 static int ad5446_read_raw(struct iio_dev *indio_dev,
 			   struct iio_chan_spec const *chan,
@@ -272,14 +212,15 @@ static const struct iio_info ad5446_info = {
 	.driver_module = THIS_MODULE,
 };
 
-static int __devinit ad5446_probe(struct spi_device *spi)
+static int __devinit ad5446_probe(struct device *dev, const char *name,
+	const struct ad5446_chip_info *chip_info)
 {
 	struct ad5446_state *st;
 	struct iio_dev *indio_dev;
 	struct regulator *reg;
 	int ret, voltage_uv = 0;
 
-	reg = regulator_get(&spi->dev, "vcc");
+	reg = regulator_get(dev, "vcc");
 	if (!IS_ERR(reg)) {
 		ret = regulator_enable(reg);
 		if (ret)
@@ -294,16 +235,15 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 		goto error_disable_reg;
 	}
 	st = iio_priv(indio_dev);
-	st->chip_info =
-		&ad5446_chip_info_tbl[spi_get_device_id(spi)->driver_data];
+	st->chip_info = chip_info;
 
-	spi_set_drvdata(spi, indio_dev);
+	dev_set_drvdata(dev, indio_dev);
 	st->reg = reg;
-	st->spi = spi;
+	st->dev = dev;
 
-	/* Establish that the iio_dev is a child of the spi device */
-	indio_dev->dev.parent = &spi->dev;
-	indio_dev->name = spi_get_device_id(spi)->name;
+	/* Establish that the iio_dev is a child of the device */
+	indio_dev->dev.parent = dev;
+	indio_dev->name = name;
 	indio_dev->info = &ad5446_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = &st->chip_info->channel;
@@ -316,7 +256,7 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 	else if (voltage_uv)
 		st->vref_mv = voltage_uv / 1000;
 	else
-		dev_warn(&spi->dev, "reference voltage unspecified\n");
+		dev_warn(dev, "reference voltage unspecified\n");
 
 	ret = iio_device_register(indio_dev);
 	if (ret)
@@ -336,9 +276,9 @@ error_put_reg:
 	return ret;
 }
 
-static int ad5446_remove(struct spi_device *spi)
+static int ad5446_remove(struct device *dev)
 {
-	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5446_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
@@ -351,7 +291,151 @@ static int ad5446_remove(struct spi_device *spi)
 	return 0;
 }
 
-static const struct spi_device_id ad5446_id[] = {
+#if IS_ENABLED(CONFIG_SPI_MASTER)
+
+static int ad5446_write(struct ad5446_state *st, unsigned val)
+{
+	struct spi_device *spi = to_spi_device(st->dev);
+	__be16 data = cpu_to_be16(val);
+
+	return spi_write(spi, &data, sizeof(data));
+}
+
+static int ad5660_write(struct ad5446_state *st, unsigned val)
+{
+	struct spi_device *spi = to_spi_device(st->dev);
+	uint8_t data[3];
+
+	data[0] = (val >> 16) & 0xFF;
+	data[1] = (val >> 8) & 0xFF;
+	data[2] = val & 0xFF;
+
+	return spi_write(spi, data, sizeof(data));
+}
+
+/**
+ * ad5446_supported_spi_device_ids:
+ * The AD5620/40/60 parts are available in different fixed internal reference
+ * voltage options. The actual part numbers may look differently
+ * (and a bit cryptic), however this style is used to make clear which
+ * parts are supported here.
+ */
+enum ad5446_supported_spi_device_ids {
+	ID_AD5300,
+	ID_AD5310,
+	ID_AD5320,
+	ID_AD5444,
+	ID_AD5446,
+	ID_AD5450,
+	ID_AD5451,
+	ID_AD5541A,
+	ID_AD5512A,
+	ID_AD5553,
+	ID_AD5601,
+	ID_AD5611,
+	ID_AD5621,
+	ID_AD5620_2500,
+	ID_AD5620_1250,
+	ID_AD5640_2500,
+	ID_AD5640_1250,
+	ID_AD5660_2500,
+	ID_AD5660_1250,
+	ID_AD5662,
+};
+
+static const struct ad5446_chip_info ad5446_spi_chip_info[] = {
+	[ID_AD5300] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(8, 16, 4),
+		.write = ad5446_write,
+	},
+	[ID_AD5310] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(10, 16, 2),
+		.write = ad5446_write,
+	},
+	[ID_AD5320] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 0),
+		.write = ad5446_write,
+	},
+	[ID_AD5444] = {
+		.channel = AD5446_CHANNEL(12, 16, 2),
+		.write = ad5446_write,
+	},
+	[ID_AD5446] = {
+		.channel = AD5446_CHANNEL(14, 16, 0),
+		.write = ad5446_write,
+	},
+	[ID_AD5450] = {
+		.channel = AD5446_CHANNEL(8, 16, 6),
+		.write = ad5446_write,
+	},
+	[ID_AD5451] = {
+		.channel = AD5446_CHANNEL(10, 16, 4),
+		.write = ad5446_write,
+	},
+	[ID_AD5541A] = {
+		.channel = AD5446_CHANNEL(16, 16, 0),
+		.write = ad5446_write,
+	},
+	[ID_AD5512A] = {
+		.channel = AD5446_CHANNEL(12, 16, 4),
+		.write = ad5446_write,
+	},
+	[ID_AD5553] = {
+		.channel = AD5446_CHANNEL(14, 16, 0),
+		.write = ad5446_write,
+	},
+	[ID_AD5601] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(8, 16, 6),
+		.write = ad5446_write,
+	},
+	[ID_AD5611] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(10, 16, 4),
+		.write = ad5446_write,
+	},
+	[ID_AD5621] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
+		.write = ad5446_write,
+	},
+	[ID_AD5620_2500] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
+		.int_vref_mv = 2500,
+		.write = ad5446_write,
+	},
+	[ID_AD5620_1250] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
+		.int_vref_mv = 1250,
+		.write = ad5446_write,
+	},
+	[ID_AD5640_2500] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
+		.int_vref_mv = 2500,
+		.write = ad5446_write,
+	},
+	[ID_AD5640_1250] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
+		.int_vref_mv = 1250,
+		.write = ad5446_write,
+	},
+	[ID_AD5660_2500] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
+		.int_vref_mv = 2500,
+		.write = ad5660_write,
+	},
+	[ID_AD5660_1250] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
+		.int_vref_mv = 1250,
+		.write = ad5660_write,
+	},
+	[ID_AD5662] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
+		.write = ad5660_write,
+	},
+};
+
+static const struct spi_device_id ad5446_spi_ids[] = {
+	{"ad5300", ID_AD5300},
+	{"ad5310", ID_AD5310},
+	{"ad5320", ID_AD5320},
 	{"ad5444", ID_AD5444},
 	{"ad5446", ID_AD5446},
 	{"ad5450", ID_AD5450},
@@ -375,18 +459,160 @@ static const struct spi_device_id ad5446_id[] = {
 	{"ad5662", ID_AD5662},
 	{}
 };
-MODULE_DEVICE_TABLE(spi, ad5446_id);
+MODULE_DEVICE_TABLE(spi, ad5446_spi_ids);
+
+static int __devinit ad5446_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
 
-static struct spi_driver ad5446_driver = {
+	return ad5446_probe(&spi->dev, id->name,
+		&ad5446_spi_chip_info[id->driver_data]);
+}
+
+static int __devexit ad5446_spi_remove(struct spi_device *spi)
+{
+	return ad5446_remove(&spi->dev);
+}
+
+static struct spi_driver ad5446_spi_driver = {
 	.driver = {
 		.name	= "ad5446",
 		.owner	= THIS_MODULE,
 	},
-	.probe		= ad5446_probe,
-	.remove		= __devexit_p(ad5446_remove),
-	.id_table	= ad5446_id,
+	.probe		= ad5446_spi_probe,
+	.remove		= __devexit_p(ad5446_spi_remove),
+	.id_table	= ad5446_spi_ids,
 };
-module_spi_driver(ad5446_driver);
+
+static int __init ad5446_spi_register_driver(void)
+{
+	return spi_register_driver(&ad5446_spi_driver);
+}
+
+static void ad5446_spi_unregister_driver(void)
+{
+	spi_unregister_driver(&ad5446_spi_driver);
+}
+
+#else
+
+static inline int ad5446_spi_register_driver(void) { return 0; }
+static inline void ad5446_spi_unregister_driver(void) { }
+
+#endif
+
+#if IS_ENABLED(CONFIG_I2C)
+
+static int ad5622_write(struct ad5446_state *st, unsigned val)
+{
+	struct i2c_client *client = to_i2c_client(st->dev);
+	__be16 data = cpu_to_be16(val);
+
+	return i2c_master_send(client, (char *)&data, sizeof(data));
+}
+
+/**
+ * ad5446_supported_i2c_device_ids:
+ * The AD5620/40/60 parts are available in different fixed internal reference
+ * voltage options. The actual part numbers may look differently
+ * (and a bit cryptic), however this style is used to make clear which
+ * parts are supported here.
+ */
+enum ad5446_supported_i2c_device_ids {
+	ID_AD5602,
+	ID_AD5612,
+	ID_AD5622,
+};
+
+static const struct ad5446_chip_info ad5446_i2c_chip_info[] = {
+	[ID_AD5602] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(8, 16, 4),
+		.write = ad5622_write,
+	},
+	[ID_AD5612] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(10, 16, 2),
+		.write = ad5622_write,
+	},
+	[ID_AD5622] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 0),
+		.write = ad5622_write,
+	},
+};
+
+static int __devinit ad5446_i2c_probe(struct i2c_client *i2c,
+	const struct i2c_device_id *id)
+{
+	return ad5446_probe(&i2c->dev, id->name,
+		&ad5446_i2c_chip_info[id->driver_data]);
+}
+
+static int __devexit ad5446_i2c_remove(struct i2c_client *i2c)
+{
+	return ad5446_remove(&i2c->dev);
+}
+
+static const struct i2c_device_id ad5446_i2c_ids[] = {
+	{"ad5301", ID_AD5602},
+	{"ad5311", ID_AD5612},
+	{"ad5321", ID_AD5622},
+	{"ad5602", ID_AD5602},
+	{"ad5612", ID_AD5612},
+	{"ad5622", ID_AD5622},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ad5446_i2c_ids);
+
+static struct i2c_driver ad5446_i2c_driver = {
+	.driver = {
+		   .name = "ad5446",
+		   .owner = THIS_MODULE,
+	},
+	.probe = ad5446_i2c_probe,
+	.remove = __devexit_p(ad5446_i2c_remove),
+	.id_table = ad5446_i2c_ids,
+};
+
+static int __init ad5446_i2c_register_driver(void)
+{
+	return i2c_add_driver(&ad5446_i2c_driver);
+}
+
+static void __exit ad5446_i2c_unregister_driver(void)
+{
+	i2c_del_driver(&ad5446_i2c_driver);
+}
+
+#else
+
+static inline int ad5446_i2c_register_driver(void) { return 0; }
+static inline void ad5446_i2c_unregister_driver(void) { }
+
+#endif
+
+static int __init ad5446_init(void)
+{
+	int ret;
+
+	ret = ad5446_spi_register_driver();
+	if (ret)
+		return ret;
+
+	ret = ad5446_i2c_register_driver();
+	if (ret) {
+		ad5446_spi_unregister_driver();
+		return ret;
+	}
+
+	return 0;
+}
+module_init(ad5446_init);
+
+static void __exit ad5446_exit(void)
+{
+	ad5446_i2c_unregister_driver();
+	ad5446_spi_unregister_driver();
+}
+module_exit(ad5446_exit);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD5444/AD5446 DAC");
diff --git a/drivers/iio/dac/ad5446.h b/drivers/iio/dac/ad5446.h
deleted file mode 100644
index 2934269a56d5..000000000000
--- a/drivers/iio/dac/ad5446.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * AD5446 SPI DAC driver
- *
- * Copyright 2010 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-#ifndef IIO_DAC_AD5446_H_
-#define IIO_DAC_AD5446_H_
-
-/* DAC Control Bits */
-
-#define AD5446_LOAD		(0x0 << 14) /* Load and update */
-#define AD5446_SDO_DIS		(0x1 << 14) /* Disable SDO */
-#define AD5446_NOP		(0x2 << 14) /* No operation */
-#define AD5446_CLK_RISING	(0x3 << 14) /* Clock data on rising edge */
-
-#define AD5620_LOAD		(0x0 << 14) /* Load and update Norm Operation*/
-#define AD5620_PWRDWN_1k	(0x1 << 14) /* Power-down: 1kOhm to GND */
-#define AD5620_PWRDWN_100k	(0x2 << 14) /* Power-down: 100kOhm to GND */
-#define AD5620_PWRDWN_TRISTATE	(0x3 << 14) /* Power-down: Three-state */
-
-#define AD5660_LOAD		(0x0 << 16) /* Load and update Norm Operation*/
-#define AD5660_PWRDWN_1k	(0x1 << 16) /* Power-down: 1kOhm to GND */
-#define AD5660_PWRDWN_100k	(0x2 << 16) /* Power-down: 100kOhm to GND */
-#define AD5660_PWRDWN_TRISTATE	(0x3 << 16) /* Power-down: Three-state */
-
-#define MODE_PWRDWN_1k		0x1
-#define MODE_PWRDWN_100k	0x2
-#define MODE_PWRDWN_TRISTATE	0x3
-
-/**
- * struct ad5446_state - driver instance specific data
- * @spi:		spi_device
- * @chip_info:		chip model specific constants, available modes etc
- * @reg:		supply regulator
- * @vref_mv:		actual reference voltage used
- */
-
-struct ad5446_state {
-	struct spi_device		*spi;
-	const struct ad5446_chip_info	*chip_info;
-	struct regulator		*reg;
-	unsigned short			vref_mv;
-	unsigned			cached_val;
-	unsigned			pwr_down_mode;
-	unsigned			pwr_down;
-};
-
-/**
- * struct ad5446_chip_info - chip specific information
- * @channel:		channel spec for the DAC
- * @int_vref_mv:	AD5620/40/60: the internal reference voltage
- * @write:		chip specific helper function to write to the register
- */
-
-struct ad5446_chip_info {
-	struct iio_chan_spec	channel;
-	u16			int_vref_mv;
-	int			(*write)(struct ad5446_state *st, unsigned val);
-};
-
-/**
- * ad5446_supported_device_ids:
- * The AD5620/40/60 parts are available in different fixed internal reference
- * voltage options. The actual part numbers may look differently
- * (and a bit cryptic), however this style is used to make clear which
- * parts are supported here.
- */
-
-enum ad5446_supported_device_ids {
-	ID_AD5444,
-	ID_AD5446,
-	ID_AD5450,
-	ID_AD5451,
-	ID_AD5541A,
-	ID_AD5512A,
-	ID_AD5553,
-	ID_AD5601,
-	ID_AD5611,
-	ID_AD5621,
-	ID_AD5620_2500,
-	ID_AD5620_1250,
-	ID_AD5640_2500,
-	ID_AD5640_1250,
-	ID_AD5660_2500,
-	ID_AD5660_1250,
-	ID_AD5662,
-};
-
-#endif /* IIO_DAC_AD5446_H_ */
diff --git a/drivers/iio/dac/ad5755.c b/drivers/iio/dac/ad5755.c
new file mode 100644
index 000000000000..5db3506034c5
--- /dev/null
+++ b/drivers/iio/dac/ad5755.c
@@ -0,0 +1,650 @@
+/*
+ * AD5755, AD5755-1, AD5757, AD5735, AD5737 Digital to analog converters driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/platform_data/ad5755.h>
+
+#define AD5755_NUM_CHANNELS 4
+
+#define AD5755_ADDR(x)			((x) << 16)
+
+#define AD5755_WRITE_REG_DATA(chan)	(chan)
+#define AD5755_WRITE_REG_GAIN(chan)	(0x08 | (chan))
+#define AD5755_WRITE_REG_OFFSET(chan)	(0x10 | (chan))
+#define AD5755_WRITE_REG_CTRL(chan)	(0x1c | (chan))
+
+#define AD5755_READ_REG_DATA(chan)	(chan)
+#define AD5755_READ_REG_CTRL(chan)	(0x4 | (chan))
+#define AD5755_READ_REG_GAIN(chan)	(0x8 | (chan))
+#define AD5755_READ_REG_OFFSET(chan)	(0xc | (chan))
+#define AD5755_READ_REG_CLEAR(chan)	(0x10 | (chan))
+#define AD5755_READ_REG_SLEW(chan)	(0x14 | (chan))
+#define AD5755_READ_REG_STATUS		0x18
+#define AD5755_READ_REG_MAIN		0x19
+#define AD5755_READ_REG_DC_DC		0x1a
+
+#define AD5755_CTRL_REG_SLEW	0x0
+#define AD5755_CTRL_REG_MAIN	0x1
+#define AD5755_CTRL_REG_DAC	0x2
+#define AD5755_CTRL_REG_DC_DC	0x3
+#define AD5755_CTRL_REG_SW	0x4
+
+#define AD5755_READ_FLAG 0x800000
+
+#define AD5755_NOOP 0x1CE000
+
+#define AD5755_DAC_INT_EN			BIT(8)
+#define AD5755_DAC_CLR_EN			BIT(7)
+#define AD5755_DAC_OUT_EN			BIT(6)
+#define AD5755_DAC_INT_CURRENT_SENSE_RESISTOR	BIT(5)
+#define AD5755_DAC_DC_DC_EN			BIT(4)
+#define AD5755_DAC_VOLTAGE_OVERRANGE_EN		BIT(3)
+
+#define AD5755_DC_DC_MAXV			0
+#define AD5755_DC_DC_FREQ_SHIFT			2
+#define AD5755_DC_DC_PHASE_SHIFT		4
+#define AD5755_EXT_DC_DC_COMP_RES		BIT(6)
+
+#define AD5755_SLEW_STEP_SIZE_SHIFT		0
+#define AD5755_SLEW_RATE_SHIFT			3
+#define AD5755_SLEW_ENABLE			BIT(12)
+
+/**
+ * struct ad5755_chip_info - chip specific information
+ * @channel_template:	channel specification
+ * @calib_shift:	shift for the calibration data registers
+ * @has_voltage_out:	whether the chip has voltage outputs
+ */
+struct ad5755_chip_info {
+	const struct iio_chan_spec channel_template;
+	unsigned int calib_shift;
+	bool has_voltage_out;
+};
+
+/**
+ * struct ad5755_state - driver instance specific data
+ * @spi:	spi device the driver is attached to
+ * @chip_info:	chip model specific constants, available modes etc
+ * @pwr_down:	bitmask which contains  hether a channel is powered down or not
+ * @ctrl:	software shadow of the channel ctrl registers
+ * @channels:	iio channel spec for the device
+ * @data:	spi transfer buffers
+ */
+struct ad5755_state {
+	struct spi_device		*spi;
+	const struct ad5755_chip_info	*chip_info;
+	unsigned int			pwr_down;
+	unsigned int			ctrl[AD5755_NUM_CHANNELS];
+	struct iio_chan_spec		channels[AD5755_NUM_CHANNELS];
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+
+	union {
+		u32 d32;
+		u8 d8[4];
+	} data[2] ____cacheline_aligned;
+};
+
+enum ad5755_type {
+	ID_AD5755,
+	ID_AD5757,
+	ID_AD5735,
+	ID_AD5737,
+};
+
+static int ad5755_write_unlocked(struct iio_dev *indio_dev,
+	unsigned int reg, unsigned int val)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+
+	st->data[0].d32 = cpu_to_be32((reg << 16) | val);
+
+	return spi_write(st->spi, &st->data[0].d8[1], 3);
+}
+
+static int ad5755_write_ctrl_unlocked(struct iio_dev *indio_dev,
+	unsigned int channel, unsigned int reg, unsigned int val)
+{
+	return ad5755_write_unlocked(indio_dev,
+		AD5755_WRITE_REG_CTRL(channel), (reg << 13) | val);
+}
+
+static int ad5755_write(struct iio_dev *indio_dev, unsigned int reg,
+	unsigned int val)
+{
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad5755_write_unlocked(indio_dev, reg, val);
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5755_write_ctrl(struct iio_dev *indio_dev, unsigned int channel,
+	unsigned int reg, unsigned int val)
+{
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad5755_write_ctrl_unlocked(indio_dev, channel, reg, val);
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5755_read(struct iio_dev *indio_dev, unsigned int addr)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	struct spi_message m;
+	int ret;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[1],
+			.len = 3,
+			.cs_change = 1,
+		}, {
+			.tx_buf = &st->data[1].d8[1],
+			.rx_buf = &st->data[1].d8[1],
+			.len = 3,
+		},
+	};
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	mutex_lock(&indio_dev->mlock);
+
+	st->data[0].d32 = cpu_to_be32(AD5755_READ_FLAG | (addr << 16));
+	st->data[1].d32 = cpu_to_be32(AD5755_NOOP);
+
+	ret = spi_sync(st->spi, &m);
+	if (ret >= 0)
+		ret = be32_to_cpu(st->data[1].d32) & 0xffff;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5755_update_dac_ctrl(struct iio_dev *indio_dev,
+	unsigned int channel, unsigned int set, unsigned int clr)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	int ret;
+
+	st->ctrl[channel] |= set;
+	st->ctrl[channel] &= ~clr;
+
+	ret = ad5755_write_ctrl_unlocked(indio_dev, channel,
+		AD5755_CTRL_REG_DAC, st->ctrl[channel]);
+
+	return ret;
+}
+
+static int ad5755_set_channel_pwr_down(struct iio_dev *indio_dev,
+	unsigned int channel, bool pwr_down)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	unsigned int mask = BIT(channel);
+
+	mutex_lock(&indio_dev->mlock);
+
+	if ((bool)(st->pwr_down & mask) == pwr_down)
+		goto out_unlock;
+
+	if (!pwr_down) {
+		st->pwr_down &= ~mask;
+		ad5755_update_dac_ctrl(indio_dev, channel,
+			AD5755_DAC_INT_EN | AD5755_DAC_DC_DC_EN, 0);
+		udelay(200);
+		ad5755_update_dac_ctrl(indio_dev, channel,
+			AD5755_DAC_OUT_EN, 0);
+	} else {
+		st->pwr_down |= mask;
+		ad5755_update_dac_ctrl(indio_dev, channel,
+			0, AD5755_DAC_INT_EN | AD5755_DAC_OUT_EN |
+				AD5755_DAC_DC_DC_EN);
+	}
+
+out_unlock:
+	mutex_unlock(&indio_dev->mlock);
+
+	return 0;
+}
+
+static const int ad5755_min_max_table[][2] = {
+	[AD5755_MODE_VOLTAGE_0V_5V] = { 0, 5000 },
+	[AD5755_MODE_VOLTAGE_0V_10V] = { 0, 10000 },
+	[AD5755_MODE_VOLTAGE_PLUSMINUS_5V] = { -5000, 5000 },
+	[AD5755_MODE_VOLTAGE_PLUSMINUS_10V] = { -10000, 10000 },
+	[AD5755_MODE_CURRENT_4mA_20mA] = { 4, 20 },
+	[AD5755_MODE_CURRENT_0mA_20mA] = { 0, 20 },
+	[AD5755_MODE_CURRENT_0mA_24mA] = { 0, 24 },
+};
+
+static void ad5755_get_min_max(struct ad5755_state *st,
+	struct iio_chan_spec const *chan, int *min, int *max)
+{
+	enum ad5755_mode mode = st->ctrl[chan->channel] & 7;
+	*min = ad5755_min_max_table[mode][0];
+	*max = ad5755_min_max_table[mode][1];
+}
+
+static inline int ad5755_get_offset(struct ad5755_state *st,
+	struct iio_chan_spec const *chan)
+{
+	int min, max;
+
+	ad5755_get_min_max(st, chan, &min, &max);
+	return (min * (1 << chan->scan_type.realbits)) / (max - min);
+}
+
+static inline int ad5755_get_scale(struct ad5755_state *st,
+	struct iio_chan_spec const *chan)
+{
+	int min, max;
+
+	ad5755_get_min_max(st, chan, &min, &max);
+	return ((max - min) * 1000000000ULL) >> chan->scan_type.realbits;
+}
+
+static int ad5755_chan_reg_info(struct ad5755_state *st,
+	struct iio_chan_spec const *chan, long info, bool write,
+	unsigned int *reg, unsigned int *shift, unsigned int *offset)
+{
+	switch (info) {
+	case IIO_CHAN_INFO_RAW:
+		if (write)
+			*reg = AD5755_WRITE_REG_DATA(chan->address);
+		else
+			*reg = AD5755_READ_REG_DATA(chan->address);
+		*shift = chan->scan_type.shift;
+		*offset = 0;
+		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (write)
+			*reg = AD5755_WRITE_REG_OFFSET(chan->address);
+		else
+			*reg = AD5755_READ_REG_OFFSET(chan->address);
+		*shift = st->chip_info->calib_shift;
+		*offset = 32768;
+		break;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (write)
+			*reg =  AD5755_WRITE_REG_GAIN(chan->address);
+		else
+			*reg =  AD5755_READ_REG_GAIN(chan->address);
+		*shift = st->chip_info->calib_shift;
+		*offset = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ad5755_read_raw(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, int *val, int *val2, long info)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	unsigned int reg, shift, offset;
+	int ret;
+
+	switch (info) {
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = ad5755_get_scale(st, chan);
+		return IIO_VAL_INT_PLUS_NANO;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = ad5755_get_offset(st, chan);
+		return IIO_VAL_INT;
+	default:
+		ret = ad5755_chan_reg_info(st, chan, info, false,
+						&reg, &shift, &offset);
+		if (ret)
+			return ret;
+
+		ret = ad5755_read(indio_dev, reg);
+		if (ret < 0)
+			return ret;
+
+		*val = (ret - offset) >> shift;
+
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static int ad5755_write_raw(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, int val, int val2, long info)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	unsigned int shift, reg, offset;
+	int ret;
+
+	ret = ad5755_chan_reg_info(st, chan, info, true,
+					&reg, &shift, &offset);
+	if (ret)
+		return ret;
+
+	val <<= shift;
+	val += offset;
+
+	if (val < 0 || val > 0xffff)
+		return -EINVAL;
+
+	return ad5755_write(indio_dev, reg, val);
+}
+
+static ssize_t ad5755_read_powerdown(struct iio_dev *indio_dev, uintptr_t priv,
+	const struct iio_chan_spec *chan, char *buf)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n",
+		       (bool)(st->pwr_down & (1 << chan->channel)));
+}
+
+static ssize_t ad5755_write_powerdown(struct iio_dev *indio_dev, uintptr_t priv,
+	struct iio_chan_spec const *chan, const char *buf, size_t len)
+{
+	bool pwr_down;
+	int ret;
+
+	ret = strtobool(buf, &pwr_down);
+	if (ret)
+		return ret;
+
+	ret = ad5755_set_channel_pwr_down(indio_dev, chan->channel, pwr_down);
+	return ret ? ret : len;
+}
+
+static const struct iio_info ad5755_info = {
+	.read_raw = ad5755_read_raw,
+	.write_raw = ad5755_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static const struct iio_chan_spec_ext_info ad5755_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ad5755_read_powerdown,
+		.write = ad5755_write_powerdown,
+	},
+	{ },
+};
+
+#define AD5755_CHANNEL(_bits) {					\
+	.indexed = 1,						\
+	.output = 1,						\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)),	\
+	.ext_info = ad5755_ext_info,				\
+}
+
+static const struct ad5755_chip_info ad5755_chip_info_tbl[] = {
+	[ID_AD5735] = {
+		.channel_template = AD5755_CHANNEL(14),
+		.has_voltage_out = true,
+		.calib_shift = 4,
+	},
+	[ID_AD5737] = {
+		.channel_template = AD5755_CHANNEL(14),
+		.has_voltage_out = false,
+		.calib_shift = 4,
+	},
+	[ID_AD5755] = {
+		.channel_template = AD5755_CHANNEL(16),
+		.has_voltage_out = true,
+		.calib_shift = 0,
+	},
+	[ID_AD5757] = {
+		.channel_template = AD5755_CHANNEL(16),
+		.has_voltage_out = false,
+		.calib_shift = 0,
+	},
+};
+
+static bool ad5755_is_valid_mode(struct ad5755_state *st, enum ad5755_mode mode)
+{
+	switch (mode) {
+	case AD5755_MODE_VOLTAGE_0V_5V:
+	case AD5755_MODE_VOLTAGE_0V_10V:
+	case AD5755_MODE_VOLTAGE_PLUSMINUS_5V:
+	case AD5755_MODE_VOLTAGE_PLUSMINUS_10V:
+		return st->chip_info->has_voltage_out;
+	case AD5755_MODE_CURRENT_4mA_20mA:
+	case AD5755_MODE_CURRENT_0mA_20mA:
+	case AD5755_MODE_CURRENT_0mA_24mA:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int __devinit ad5755_setup_pdata(struct iio_dev *indio_dev,
+	const struct ad5755_platform_data *pdata)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	unsigned int val;
+	unsigned int i;
+	int ret;
+
+	if (pdata->dc_dc_phase > AD5755_DC_DC_PHASE_90_DEGREE ||
+		pdata->dc_dc_freq > AD5755_DC_DC_FREQ_650kHZ ||
+		pdata->dc_dc_maxv > AD5755_DC_DC_MAXV_29V5)
+		return -EINVAL;
+
+	val = pdata->dc_dc_maxv << AD5755_DC_DC_MAXV;
+	val |= pdata->dc_dc_freq << AD5755_DC_DC_FREQ_SHIFT;
+	val |= pdata->dc_dc_phase << AD5755_DC_DC_PHASE_SHIFT;
+	if (pdata->ext_dc_dc_compenstation_resistor)
+		val |= AD5755_EXT_DC_DC_COMP_RES;
+
+	ret = ad5755_write_ctrl(indio_dev, 0, AD5755_CTRL_REG_DC_DC, val);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(pdata->dac); ++i) {
+		val = pdata->dac[i].slew.step_size <<
+			AD5755_SLEW_STEP_SIZE_SHIFT;
+		val |= pdata->dac[i].slew.rate <<
+			AD5755_SLEW_RATE_SHIFT;
+		if (pdata->dac[i].slew.enable)
+			val |= AD5755_SLEW_ENABLE;
+
+		ret = ad5755_write_ctrl(indio_dev, i,
+					AD5755_CTRL_REG_SLEW, val);
+		if (ret < 0)
+			return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(pdata->dac); ++i) {
+		if (!ad5755_is_valid_mode(st, pdata->dac[i].mode))
+			return -EINVAL;
+
+		val = 0;
+		if (!pdata->dac[i].ext_current_sense_resistor)
+			val |= AD5755_DAC_INT_CURRENT_SENSE_RESISTOR;
+		if (pdata->dac[i].enable_voltage_overrange)
+			val |= AD5755_DAC_VOLTAGE_OVERRANGE_EN;
+		val |= pdata->dac[i].mode;
+
+		ret = ad5755_update_dac_ctrl(indio_dev, i, val, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static bool __devinit ad5755_is_voltage_mode(enum ad5755_mode mode)
+{
+	switch (mode) {
+	case AD5755_MODE_VOLTAGE_0V_5V:
+	case AD5755_MODE_VOLTAGE_0V_10V:
+	case AD5755_MODE_VOLTAGE_PLUSMINUS_5V:
+	case AD5755_MODE_VOLTAGE_PLUSMINUS_10V:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int __devinit ad5755_init_channels(struct iio_dev *indio_dev,
+	const struct ad5755_platform_data *pdata)
+{
+	struct ad5755_state *st = iio_priv(indio_dev);
+	struct iio_chan_spec *channels = st->channels;
+	unsigned int i;
+
+	for (i = 0; i < AD5755_NUM_CHANNELS; ++i) {
+		channels[i] = st->chip_info->channel_template;
+		channels[i].channel = i;
+		channels[i].address = i;
+		if (pdata && ad5755_is_voltage_mode(pdata->dac[i].mode))
+			channels[i].type = IIO_VOLTAGE;
+		else
+			channels[i].type = IIO_CURRENT;
+	}
+
+	indio_dev->channels = channels;
+
+	return 0;
+}
+
+#define AD5755_DEFAULT_DAC_PDATA { \
+		.mode = AD5755_MODE_CURRENT_4mA_20mA, \
+		.ext_current_sense_resistor = true, \
+		.enable_voltage_overrange = false, \
+		.slew = { \
+			.enable = false, \
+			.rate = AD5755_SLEW_RATE_64k, \
+			.step_size = AD5755_SLEW_STEP_SIZE_1, \
+		}, \
+	}
+
+static const struct ad5755_platform_data ad5755_default_pdata = {
+	.ext_dc_dc_compenstation_resistor = false,
+	.dc_dc_phase = AD5755_DC_DC_PHASE_ALL_SAME_EDGE,
+	.dc_dc_freq = AD5755_DC_DC_FREQ_410kHZ,
+	.dc_dc_maxv = AD5755_DC_DC_MAXV_23V,
+	.dac = {
+		[0] = AD5755_DEFAULT_DAC_PDATA,
+		[1] = AD5755_DEFAULT_DAC_PDATA,
+		[2] = AD5755_DEFAULT_DAC_PDATA,
+		[3] = AD5755_DEFAULT_DAC_PDATA,
+	},
+};
+
+static int __devinit ad5755_probe(struct spi_device *spi)
+{
+	enum ad5755_type type = spi_get_device_id(spi)->driver_data;
+	const struct ad5755_platform_data *pdata = dev_get_platdata(&spi->dev);
+	struct iio_dev *indio_dev;
+	struct ad5755_state *st;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (indio_dev == NULL) {
+		dev_err(&spi->dev, "Failed to allocate iio device\n");
+		return  -ENOMEM;
+	}
+
+	st = iio_priv(indio_dev);
+	spi_set_drvdata(spi, indio_dev);
+
+	st->chip_info = &ad5755_chip_info_tbl[type];
+	st->spi = spi;
+	st->pwr_down = 0xf;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->info = &ad5755_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->num_channels = AD5755_NUM_CHANNELS;
+
+	if (!pdata)
+		pdata = &ad5755_default_pdata;
+
+	ret = ad5755_init_channels(indio_dev, pdata);
+	if (ret)
+		goto error_free;
+
+	ret = ad5755_setup_pdata(indio_dev, pdata);
+	if (ret)
+		goto error_free;
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to register iio device: %d\n", ret);
+		goto error_free;
+	}
+
+	return 0;
+
+error_free:
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad5755_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+
+	iio_device_unregister(indio_dev);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad5755_id[] = {
+	{ "ad5755", ID_AD5755 },
+	{ "ad5755-1", ID_AD5755 },
+	{ "ad5757", ID_AD5757 },
+	{ "ad5735", ID_AD5735 },
+	{ "ad5737", ID_AD5737 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad5755_id);
+
+static struct spi_driver ad5755_driver = {
+	.driver = {
+		.name = "ad5755",
+		.owner = THIS_MODULE,
+	},
+	.probe = ad5755_probe,
+	.remove = __devexit_p(ad5755_remove),
+	.id_table = ad5755_id,
+};
+module_spi_driver(ad5755_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD5755/55-1/57/35/37 DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/gyro/Kconfig b/drivers/iio/gyro/Kconfig
new file mode 100644
index 000000000000..21e27e2fc68c
--- /dev/null
+++ b/drivers/iio/gyro/Kconfig
@@ -0,0 +1,16 @@
+#
+# IIO Digital Gyroscope Sensor drivers configuration
+#
+menu "Digital gyroscope sensors"
+
+config HID_SENSOR_GYRO_3D
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	tristate "HID Gyroscope 3D"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  Gyroscope 3D.
+
+endmenu
diff --git a/drivers/iio/gyro/Makefile b/drivers/iio/gyro/Makefile
new file mode 100644
index 000000000000..8a895d9fcbce
--- /dev/null
+++ b/drivers/iio/gyro/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for industrial I/O gyroscope sensor drivers
+#
+
+obj-$(CONFIG_HID_SENSOR_GYRO_3D) += hid-sensor-gyro-3d.o
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
new file mode 100644
index 000000000000..4c56ada51c39
--- /dev/null
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -0,0 +1,418 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "../common/hid-sensors/hid-sensor-attributes.h"
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+/*Format: HID-SENSOR-usage_id_in_hex*/
+/*Usage ID from spec for Gyro-3D: 0x200076*/
+#define DRIVER_NAME "HID-SENSOR-200076"
+
+enum gyro_3d_channel {
+	CHANNEL_SCAN_INDEX_X,
+	CHANNEL_SCAN_INDEX_Y,
+	CHANNEL_SCAN_INDEX_Z,
+	GYRO_3D_CHANNEL_MAX,
+};
+
+struct gyro_3d_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_iio_common common_attributes;
+	struct hid_sensor_hub_attribute_info gyro[GYRO_3D_CHANNEL_MAX];
+	u32 gyro_val[GYRO_3D_CHANNEL_MAX];
+};
+
+static const u32 gyro_3d_addresses[GYRO_3D_CHANNEL_MAX] = {
+	HID_USAGE_SENSOR_ANGL_VELOCITY_X_AXIS,
+	HID_USAGE_SENSOR_ANGL_VELOCITY_Y_AXIS,
+	HID_USAGE_SENSOR_ANGL_VELOCITY_Z_AXIS
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec gyro_3d_channels[] = {
+	{
+		.type = IIO_ANGL_VEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_X,
+	}, {
+		.type = IIO_ANGL_VEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Y,
+	}, {
+		.type = IIO_ANGL_VEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Z,
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void gyro_3d_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+						int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	channels[channel].scan_type.storagebits = sizeof(u32) * 8;
+}
+
+/* Channel read_raw handler */
+static int gyro_3d_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
+	int report_id = -1;
+	u32 address;
+	int ret;
+	int ret_type;
+
+	*val = 0;
+	*val2 = 0;
+	switch (mask) {
+	case 0:
+		report_id = gyro_state->gyro[chan->scan_index].report_id;
+		address = gyro_3d_addresses[chan->scan_index];
+		if (report_id >= 0)
+			*val = sensor_hub_input_attr_get_raw_value(
+				gyro_state->common_attributes.hsdev,
+				HID_USAGE_SENSOR_GYRO_3D, address,
+				report_id);
+		else {
+			*val = 0;
+			return -EINVAL;
+		}
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = gyro_state->gyro[CHANNEL_SCAN_INDEX_X].units;
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = hid_sensor_convert_exponent(
+			gyro_state->gyro[CHANNEL_SCAN_INDEX_X].unit_expo);
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_read_samp_freq_value(
+			&gyro_state->common_attributes, val, val2);
+			ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_read_raw_hyst_value(
+			&gyro_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int gyro_3d_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
+	int ret = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&gyro_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&gyro_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int gyro_3d_write_raw_get_fmt(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       long mask)
+{
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static const struct iio_info gyro_3d_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &gyro_3d_read_raw,
+	.write_raw = &gyro_3d_write_raw,
+	.write_raw_get_fmt = &gyro_3d_write_raw_get_fmt,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	int datum_sz;
+
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n");
+	if (!buffer) {
+		dev_err(&indio_dev->dev, "Buffer == NULL\n");
+		return;
+	}
+	datum_sz = buffer->access->get_bytes_per_datum(buffer);
+	if (len > datum_sz) {
+		dev_err(&indio_dev->dev, "Datum size mismatch %d:%d\n", len,
+				datum_sz);
+		return;
+	}
+	iio_push_to_buffer(buffer, (u8 *)data);
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int gyro_3d_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "gyro_3d_proc_event [%d]\n",
+				gyro_state->common_attributes.data_ready);
+	if (gyro_state->common_attributes.data_ready)
+		hid_sensor_push_data(indio_dev,
+				(u8 *)gyro_state->gyro_val,
+				sizeof(gyro_state->gyro_val));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int gyro_3d_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
+	int offset;
+	int ret = -EINVAL;
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_ANGL_VELOCITY_X_AXIS:
+	case HID_USAGE_SENSOR_ANGL_VELOCITY_Y_AXIS:
+	case HID_USAGE_SENSOR_ANGL_VELOCITY_Z_AXIS:
+		offset = usage_id - HID_USAGE_SENSOR_ANGL_VELOCITY_X_AXIS;
+		gyro_state->gyro_val[CHANNEL_SCAN_INDEX_X + offset] =
+						*(u32 *)raw_data;
+		ret = 0;
+	break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/* Parse report which is specific to an usage id*/
+static int gyro_3d_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct gyro_3d_state *st)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i <= CHANNEL_SCAN_INDEX_Z; ++i) {
+		ret = sensor_hub_input_get_attribute_info(hsdev,
+				HID_INPUT_REPORT,
+				usage_id,
+				HID_USAGE_SENSOR_ANGL_VELOCITY_X_AXIS + i,
+				&st->gyro[CHANNEL_SCAN_INDEX_X + i]);
+		if (ret < 0)
+			break;
+		gyro_3d_adjust_channel_bit_mask(channels,
+				CHANNEL_SCAN_INDEX_X + i,
+				st->gyro[CHANNEL_SCAN_INDEX_X + i].size);
+	}
+	dev_dbg(&pdev->dev, "gyro_3d %x:%x, %x:%x, %x:%x\n",
+			st->gyro[0].index,
+			st->gyro[0].report_id,
+			st->gyro[1].index, st->gyro[1].report_id,
+			st->gyro[2].index, st->gyro[2].report_id);
+
+	return ret;
+}
+
+/* Function to initialize the processing for usage id */
+static int __devinit hid_gyro_3d_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	static const char *name = "gyro_3d";
+	struct iio_dev *indio_dev;
+	struct gyro_3d_state *gyro_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = iio_device_alloc(sizeof(struct gyro_3d_state));
+	if (indio_dev == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	platform_set_drvdata(pdev, indio_dev);
+
+	gyro_state = iio_priv(indio_dev);
+	gyro_state->common_attributes.hsdev = hsdev;
+	gyro_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+						HID_USAGE_SENSOR_GYRO_3D,
+						&gyro_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		goto error_free_dev;
+	}
+
+	channels = kmemdup(gyro_3d_channels,
+					sizeof(gyro_3d_channels),
+					GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		goto error_free_dev;
+	}
+
+	ret = gyro_3d_parse_report(pdev, hsdev, channels,
+					HID_USAGE_SENSOR_GYRO_3D, gyro_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		goto error_free_dev_mem;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels = ARRAY_SIZE(gyro_3d_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &gyro_3d_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		goto error_free_dev_mem;
+	}
+	gyro_state->common_attributes.data_ready = false;
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+					&gyro_state->common_attributes);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	gyro_state->callbacks.send_event = gyro_3d_proc_event;
+	gyro_state->callbacks.capture_sample = gyro_3d_capture_sample;
+	gyro_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_GYRO_3D,
+					&gyro_state->callbacks);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return ret;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(indio_dev);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_free_dev_mem:
+	kfree(indio_dev->channels);
+error_free_dev:
+	iio_device_free(indio_dev);
+error_ret:
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int __devinit hid_gyro_3d_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_GYRO_3D);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	kfree(indio_dev->channels);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver hid_gyro_3d_platform_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= hid_gyro_3d_probe,
+	.remove		= hid_gyro_3d_remove,
+};
+module_platform_driver(hid_gyro_3d_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Gyroscope 3D");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 4add9bb40eeb..d4ad37455a67 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -422,7 +422,7 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 			ret = indio_dev->setup_ops->preenable(indio_dev);
 			if (ret) {
 				printk(KERN_ERR
-				       "Buffer not started:"
+				       "Buffer not started: "
 				       "buffer preenable failed\n");
 				goto error_ret;
 			}
@@ -431,12 +431,12 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 			ret = buffer->access->request_update(buffer);
 			if (ret) {
 				printk(KERN_INFO
-				       "Buffer not started:"
+				       "Buffer not started: "
 				       "buffer parameter update failed\n");
 				goto error_ret;
 			}
 		}
-		/* Definitely possible for devices to support both of these.*/
+		/* Definitely possible for devices to support both of these. */
 		if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
 			if (!indio_dev->trig) {
 				printk(KERN_INFO
@@ -456,7 +456,7 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 			ret = indio_dev->setup_ops->postenable(indio_dev);
 			if (ret) {
 				printk(KERN_INFO
-				       "Buffer not started:"
+				       "Buffer not started: "
 				       "postenable failed\n");
 				indio_dev->currentmode = previous_mode;
 				if (indio_dev->setup_ops->postdisable)
@@ -657,7 +657,7 @@ EXPORT_SYMBOL_GPL(iio_scan_mask_query);
 /**
  * struct iio_demux_table() - table describing demux memcpy ops
  * @from:	index to copy from
- * @to:	index to copy to
+ * @to:		index to copy to
  * @length:	how many bytes to copy
  * @l:		list head used for management
  */
@@ -682,12 +682,11 @@ static unsigned char *iio_demux(struct iio_buffer *buffer,
 	return buffer->demux_bounce;
 }
 
-int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
-		       s64 timestamp)
+int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data)
 {
 	unsigned char *dataout = iio_demux(buffer, data);
 
-	return buffer->access->store_to(buffer, dataout, timestamp);
+	return buffer->access->store_to(buffer, dataout);
 }
 EXPORT_SYMBOL_GPL(iio_push_to_buffer);
 
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 2ec266ef41a3..6eb24dbc081e 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -29,7 +29,7 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/events.h>
 
-/* IDA to assign each registered device a unique id*/
+/* IDA to assign each registered device a unique id */
 static DEFINE_IDA(iio_ida);
 
 static dev_t iio_devt;
@@ -99,6 +99,7 @@ static const char * const iio_chan_info_postfix[] = {
 	[IIO_CHAN_INFO_FREQUENCY] = "frequency",
 	[IIO_CHAN_INFO_PHASE] = "phase",
 	[IIO_CHAN_INFO_HARDWAREGAIN] = "hardwaregain",
+	[IIO_CHAN_INFO_HYSTERESIS] = "hysteresis",
 };
 
 const struct iio_chan_spec
@@ -365,6 +366,7 @@ static ssize_t iio_read_channel_info(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	unsigned long long tmp;
 	int val, val2;
 	bool scale_db = false;
 	int ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
@@ -390,6 +392,11 @@ static ssize_t iio_read_channel_info(struct device *dev,
 			return sprintf(buf, "-%d.%09u\n", val, -val2);
 		else
 			return sprintf(buf, "%d.%09u\n", val, val2);
+	case IIO_VAL_FRACTIONAL:
+		tmp = div_s64((s64)val * 1000000000LL, val2);
+		val2 = do_div(tmp, 1000000000LL);
+		val = tmp;
+		return sprintf(buf, "%d.%09u\n", val, val2);
 	default:
 		return 0;
 	}
@@ -729,7 +736,7 @@ static int iio_device_register_sysfs(struct iio_dev *indio_dev)
 	attrcount = attrcount_orig;
 	/*
 	 * New channel registration method - relies on the fact a group does
-	 * not need to be initialized if it is name is NULL.
+	 * not need to be initialized if its name is NULL.
 	 */
 	if (indio_dev->channels)
 		for (i = 0; i < indio_dev->num_channels; i++) {
@@ -980,6 +987,6 @@ EXPORT_SYMBOL(iio_device_unregister);
 subsys_initcall(iio_init);
 module_exit(iio_exit);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Industrial I/O core");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index b5afc2ff34fd..f2b78d4fe457 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -111,6 +111,7 @@ struct iio_channel *iio_channel_get(const char *name, const char *channel_name)
 {
 	struct iio_map_internal *c_i = NULL, *c = NULL;
 	struct iio_channel *channel;
+	int err;
 
 	if (name == NULL && channel_name == NULL)
 		return ERR_PTR(-ENODEV);
@@ -130,18 +131,32 @@ struct iio_channel *iio_channel_get(const char *name, const char *channel_name)
 	if (c == NULL)
 		return ERR_PTR(-ENODEV);
 
-	channel = kmalloc(sizeof(*channel), GFP_KERNEL);
-	if (channel == NULL)
-		return ERR_PTR(-ENOMEM);
+	channel = kzalloc(sizeof(*channel), GFP_KERNEL);
+	if (channel == NULL) {
+		err = -ENOMEM;
+		goto error_no_mem;
+	}
 
 	channel->indio_dev = c->indio_dev;
 
-	if (c->map->adc_channel_label)
+	if (c->map->adc_channel_label) {
 		channel->channel =
 			iio_chan_spec_from_name(channel->indio_dev,
 						c->map->adc_channel_label);
 
+		if (channel->channel == NULL) {
+			err = -EINVAL;
+			goto error_no_chan;
+		}
+	}
+
 	return channel;
+
+error_no_chan:
+	kfree(channel);
+error_no_mem:
+	iio_device_put(c->indio_dev);
+	return ERR_PTR(err);
 }
 EXPORT_SYMBOL_GPL(iio_channel_get);
 
@@ -229,9 +244,21 @@ void iio_channel_release_all(struct iio_channel *channels)
 }
 EXPORT_SYMBOL_GPL(iio_channel_release_all);
 
+static int iio_channel_read(struct iio_channel *chan, int *val, int *val2,
+	enum iio_chan_info_enum info)
+{
+	int unused;
+
+	if (val2 == NULL)
+		val2 = &unused;
+
+	return chan->indio_dev->info->read_raw(chan->indio_dev, chan->channel,
+						val, val2, info);
+}
+
 int iio_read_channel_raw(struct iio_channel *chan, int *val)
 {
-	int val2, ret;
+	int ret;
 
 	mutex_lock(&chan->indio_dev->info_exist_lock);
 	if (chan->indio_dev->info == NULL) {
@@ -239,8 +266,7 @@ int iio_read_channel_raw(struct iio_channel *chan, int *val)
 		goto err_unlock;
 	}
 
-	ret = chan->indio_dev->info->read_raw(chan->indio_dev, chan->channel,
-					      val, &val2, 0);
+	ret = iio_channel_read(chan, val, NULL, IIO_CHAN_INFO_RAW);
 err_unlock:
 	mutex_unlock(&chan->indio_dev->info_exist_lock);
 
@@ -248,6 +274,100 @@ err_unlock:
 }
 EXPORT_SYMBOL_GPL(iio_read_channel_raw);
 
+static int iio_convert_raw_to_processed_unlocked(struct iio_channel *chan,
+	int raw, int *processed, unsigned int scale)
+{
+	int scale_type, scale_val, scale_val2, offset;
+	s64 raw64 = raw;
+	int ret;
+
+	ret = iio_channel_read(chan, &offset, NULL, IIO_CHAN_INFO_SCALE);
+	if (ret == 0)
+		raw64 += offset;
+
+	scale_type = iio_channel_read(chan, &scale_val, &scale_val2,
+					IIO_CHAN_INFO_SCALE);
+	if (scale_type < 0)
+		return scale_type;
+
+	switch (scale_type) {
+	case IIO_VAL_INT:
+		*processed = raw64 * scale_val;
+		break;
+	case IIO_VAL_INT_PLUS_MICRO:
+		if (scale_val2 < 0)
+			*processed = -raw64 * scale_val;
+		else
+			*processed = raw64 * scale_val;
+		*processed += div_s64(raw64 * (s64)scale_val2 * scale,
+				      1000000LL);
+		break;
+	case IIO_VAL_INT_PLUS_NANO:
+		if (scale_val2 < 0)
+			*processed = -raw64 * scale_val;
+		else
+			*processed = raw64 * scale_val;
+		*processed += div_s64(raw64 * (s64)scale_val2 * scale,
+				      1000000000LL);
+		break;
+	case IIO_VAL_FRACTIONAL:
+		*processed = div_s64(raw64 * (s64)scale_val * scale,
+				     scale_val2);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int iio_convert_raw_to_processed(struct iio_channel *chan, int raw,
+	int *processed, unsigned int scale)
+{
+	int ret;
+
+	mutex_lock(&chan->indio_dev->info_exist_lock);
+	if (chan->indio_dev->info == NULL) {
+		ret = -ENODEV;
+		goto err_unlock;
+	}
+
+	ret = iio_convert_raw_to_processed_unlocked(chan, raw, processed,
+							scale);
+err_unlock:
+	mutex_unlock(&chan->indio_dev->info_exist_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_convert_raw_to_processed);
+
+int iio_read_channel_processed(struct iio_channel *chan, int *val)
+{
+	int ret;
+
+	mutex_lock(&chan->indio_dev->info_exist_lock);
+	if (chan->indio_dev->info == NULL) {
+		ret = -ENODEV;
+		goto err_unlock;
+	}
+
+	if (iio_channel_has_info(chan->channel, IIO_CHAN_INFO_PROCESSED)) {
+		ret = iio_channel_read(chan, val, NULL,
+				       IIO_CHAN_INFO_PROCESSED);
+	} else {
+		ret = iio_channel_read(chan, val, NULL, IIO_CHAN_INFO_RAW);
+		if (ret < 0)
+			goto err_unlock;
+		ret = iio_convert_raw_to_processed_unlocked(chan, *val, val, 1);
+	}
+
+err_unlock:
+	mutex_unlock(&chan->indio_dev->info_exist_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_read_channel_processed);
+
 int iio_read_channel_scale(struct iio_channel *chan, int *val, int *val2)
 {
 	int ret;
@@ -258,10 +378,7 @@ int iio_read_channel_scale(struct iio_channel *chan, int *val, int *val2)
 		goto err_unlock;
 	}
 
-	ret = chan->indio_dev->info->read_raw(chan->indio_dev,
-					      chan->channel,
-					      val, val2,
-					      IIO_CHAN_INFO_SCALE);
+	ret = iio_channel_read(chan, val, val2, IIO_CHAN_INFO_SCALE);
 err_unlock:
 	mutex_unlock(&chan->indio_dev->info_exist_lock);
 
diff --git a/drivers/iio/kfifo_buf.c b/drivers/iio/kfifo_buf.c
index 6bf9d05f4841..5bc5c860e9ca 100644
--- a/drivers/iio/kfifo_buf.c
+++ b/drivers/iio/kfifo_buf.c
@@ -6,6 +6,7 @@
 #include <linux/kfifo.h>
 #include <linux/mutex.h>
 #include <linux/iio/kfifo_buf.h>
+#include <linux/sched.h>
 
 struct iio_kfifo {
 	struct iio_buffer buffer;
@@ -22,7 +23,8 @@ static inline int __iio_allocate_kfifo(struct iio_kfifo *buf,
 		return -EINVAL;
 
 	__iio_update_buffer(&buf->buffer, bytes_per_datum, length);
-	return kfifo_alloc(&buf->kf, bytes_per_datum*length, GFP_KERNEL);
+	return __kfifo_alloc((struct __kfifo *)&buf->kf, length,
+			     bytes_per_datum, GFP_KERNEL);
 }
 
 static int iio_request_update_kfifo(struct iio_buffer *r)
@@ -35,6 +37,7 @@ static int iio_request_update_kfifo(struct iio_buffer *r)
 	kfifo_free(&buf->kf);
 	ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum,
 				   buf->buffer.length);
+	r->stufftoread = false;
 error_ret:
 	return ret;
 }
@@ -81,6 +84,9 @@ static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd)
 
 static int iio_set_length_kfifo(struct iio_buffer *r, int length)
 {
+	/* Avoid an invalid state */
+	if (length < 2)
+		length = 2;
 	if (r->length != length) {
 		r->length = length;
 		iio_mark_update_needed_kfifo(r);
@@ -89,14 +95,16 @@ static int iio_set_length_kfifo(struct iio_buffer *r, int length)
 }
 
 static int iio_store_to_kfifo(struct iio_buffer *r,
-			      u8 *data,
-			      s64 timestamp)
+			      u8 *data)
 {
 	int ret;
 	struct iio_kfifo *kf = iio_to_kfifo(r);
-	ret = kfifo_in(&kf->kf, data, r->bytes_per_datum);
-	if (ret != r->bytes_per_datum)
+	ret = kfifo_in(&kf->kf, data, 1);
+	if (ret != 1)
 		return -EBUSY;
+	r->stufftoread = true;
+	wake_up_interruptible(&r->pollq);
+
 	return 0;
 }
 
@@ -106,11 +114,18 @@ static int iio_read_first_n_kfifo(struct iio_buffer *r,
 	int ret, copied;
 	struct iio_kfifo *kf = iio_to_kfifo(r);
 
-	if (n < r->bytes_per_datum)
+	if (n < r->bytes_per_datum || r->bytes_per_datum == 0)
 		return -EINVAL;
 
-	n = rounddown(n, r->bytes_per_datum);
 	ret = kfifo_to_user(&kf->kf, buf, n, &copied);
+	if (ret < 0)
+		return ret;
+
+	if (kfifo_is_empty(&kf->kf))
+		r->stufftoread = false;
+	/* verify it is still empty to avoid race */
+	if (!kfifo_is_empty(&kf->kf))
+		r->stufftoread = true;
 
 	return copied;
 }
@@ -136,7 +151,7 @@ struct iio_buffer *iio_kfifo_allocate(struct iio_dev *indio_dev)
 	iio_buffer_init(&kf->buffer);
 	kf->buffer.attrs = &iio_kfifo_attribute_group;
 	kf->buffer.access = &kfifo_access_funcs;
-
+	kf->buffer.length = 2;
 	return &kf->buffer;
 }
 EXPORT_SYMBOL(iio_kfifo_allocate);
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index 91d15d2f694f..1763c9bcb98a 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -42,4 +42,14 @@ config VCNL4000
 	 To compile this driver as a module, choose M here: the
 	 module will be called vcnl4000.
 
+config HID_SENSOR_ALS
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	tristate "HID ALS"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  Ambient light sensor.
+
 endmenu
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index 13f8a782d292..21a8f0df1407 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_ADJD_S311)		+= adjd_s311.o
 obj-$(CONFIG_SENSORS_LM3533)	+= lm3533-als.o
 obj-$(CONFIG_VCNL4000)		+= vcnl4000.o
+obj-$(CONFIG_HID_SENSOR_ALS)	+= hid-sensor-als.o
diff --git a/drivers/iio/light/adjd_s311.c b/drivers/iio/light/adjd_s311.c
index 9a99f43094f0..164b62b91a4b 100644
--- a/drivers/iio/light/adjd_s311.c
+++ b/drivers/iio/light/adjd_s311.c
@@ -187,7 +187,7 @@ static irqreturn_t adjd_s311_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*(s64 *)((u8 *)data->buffer + ALIGN(len, sizeof(s64)))
 			= time_ns;
-	iio_push_to_buffer(buffer, (u8 *)data->buffer, time_ns);
+	iio_push_to_buffer(buffer, (u8 *)data->buffer);
 
 done:
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
new file mode 100644
index 000000000000..96e3691e42c4
--- /dev/null
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -0,0 +1,385 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "../common/hid-sensors/hid-sensor-attributes.h"
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+/*Format: HID-SENSOR-usage_id_in_hex*/
+/*Usage ID from spec for Accelerometer-3D: 0x200041*/
+#define DRIVER_NAME "HID-SENSOR-200041"
+
+#define CHANNEL_SCAN_INDEX_ILLUM 0
+
+struct als_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_iio_common common_attributes;
+	struct hid_sensor_hub_attribute_info als_illum;
+	u32 illum;
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec als_channels[] = {
+	{
+		.type = IIO_INTENSITY,
+		.modified = 1,
+		.channel2 = IIO_MOD_LIGHT_BOTH,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_ILLUM,
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void als_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+					int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	channels[channel].scan_type.storagebits = sizeof(u32) * 8;
+}
+
+/* Channel read_raw handler */
+static int als_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct als_state *als_state = iio_priv(indio_dev);
+	int report_id = -1;
+	u32 address;
+	int ret;
+	int ret_type;
+
+	*val = 0;
+	*val2 = 0;
+	switch (mask) {
+	case 0:
+		switch (chan->scan_index) {
+		case  CHANNEL_SCAN_INDEX_ILLUM:
+			report_id = als_state->als_illum.report_id;
+			address =
+			HID_USAGE_SENSOR_LIGHT_ILLUM;
+			break;
+		default:
+			report_id = -1;
+			break;
+		}
+		if (report_id >= 0)
+			*val = sensor_hub_input_attr_get_raw_value(
+				als_state->common_attributes.hsdev,
+				HID_USAGE_SENSOR_ALS, address,
+				report_id);
+		else {
+			*val = 0;
+			return -EINVAL;
+		}
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = als_state->als_illum.units;
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = hid_sensor_convert_exponent(
+				als_state->als_illum.unit_expo);
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_read_samp_freq_value(
+				&als_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_read_raw_hyst_value(
+				&als_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int als_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct als_state *als_state = iio_priv(indio_dev);
+	int ret = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&als_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&als_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int als_write_raw_get_fmt(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       long mask)
+{
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static const struct iio_info als_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &als_read_raw,
+	.write_raw = &als_write_raw,
+	.write_raw_get_fmt = &als_write_raw_get_fmt,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	int datum_sz;
+
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n");
+	if (!buffer) {
+		dev_err(&indio_dev->dev, "Buffer == NULL\n");
+		return;
+	}
+	datum_sz = buffer->access->get_bytes_per_datum(buffer);
+	if (len > datum_sz) {
+		dev_err(&indio_dev->dev, "Datum size mismatch %d:%d\n", len,
+				datum_sz);
+		return;
+	}
+	iio_push_to_buffer(buffer, (u8 *)data);
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int als_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct als_state *als_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "als_proc_event [%d]\n",
+				als_state->common_attributes.data_ready);
+	if (als_state->common_attributes.data_ready)
+		hid_sensor_push_data(indio_dev,
+				(u8 *)&als_state->illum,
+				sizeof(als_state->illum));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int als_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct als_state *als_state = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_LIGHT_ILLUM:
+		als_state->illum = *(u32 *)raw_data;
+		ret = 0;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/* Parse report which is specific to an usage id*/
+static int als_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct als_state *st)
+{
+	int ret;
+
+	ret = sensor_hub_input_get_attribute_info(hsdev, HID_INPUT_REPORT,
+			usage_id,
+			HID_USAGE_SENSOR_LIGHT_ILLUM,
+			&st->als_illum);
+	if (ret < 0)
+		return ret;
+	als_adjust_channel_bit_mask(channels, CHANNEL_SCAN_INDEX_ILLUM,
+					st->als_illum.size);
+
+	dev_dbg(&pdev->dev, "als %x:%x\n", st->als_illum.index,
+			st->als_illum.report_id);
+
+	return ret;
+}
+
+/* Function to initialize the processing for usage id */
+static int __devinit hid_als_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	static const char *name = "als";
+	struct iio_dev *indio_dev;
+	struct als_state *als_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = iio_device_alloc(sizeof(struct als_state));
+	if (indio_dev == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	platform_set_drvdata(pdev, indio_dev);
+
+	als_state = iio_priv(indio_dev);
+	als_state->common_attributes.hsdev = hsdev;
+	als_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev, HID_USAGE_SENSOR_ALS,
+					&als_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		goto error_free_dev;
+	}
+
+	channels = kmemdup(als_channels,
+					sizeof(als_channels),
+					GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		goto error_free_dev;
+	}
+
+	ret = als_parse_report(pdev, hsdev, channels,
+				HID_USAGE_SENSOR_ALS, als_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		goto error_free_dev_mem;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels =
+				ARRAY_SIZE(als_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &als_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		goto error_free_dev_mem;
+	}
+	als_state->common_attributes.data_ready = false;
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+				&als_state->common_attributes);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	als_state->callbacks.send_event = als_proc_event;
+	als_state->callbacks.capture_sample = als_capture_sample;
+	als_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_ALS,
+					&als_state->callbacks);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return ret;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(indio_dev);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_free_dev_mem:
+	kfree(indio_dev->channels);
+error_free_dev:
+	iio_device_free(indio_dev);
+error_ret:
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int __devinit hid_als_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_ALS);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	kfree(indio_dev->channels);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver hid_als_platform_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= hid_als_probe,
+	.remove		= hid_als_remove,
+};
+module_platform_driver(hid_als_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor ALS");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
new file mode 100644
index 000000000000..c1f0cdd57037
--- /dev/null
+++ b/drivers/iio/magnetometer/Kconfig
@@ -0,0 +1,16 @@
+#
+# Magnetometer sensors
+#
+menu "Magnetometer sensors"
+
+config HID_SENSOR_MAGNETOMETER_3D
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	tristate "HID Magenetometer 3D"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  Magnetometer 3D.
+
+endmenu
diff --git a/drivers/iio/magnetometer/Makefile b/drivers/iio/magnetometer/Makefile
new file mode 100644
index 000000000000..60dc4f2b1963
--- /dev/null
+++ b/drivers/iio/magnetometer/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for industrial I/O Magnetometer sensor drivers
+#
+
+obj-$(CONFIG_HID_SENSOR_MAGNETOMETER_3D) += hid-sensor-magn-3d.o
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
new file mode 100644
index 000000000000..c4f0d274f577
--- /dev/null
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -0,0 +1,419 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2012, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "../common/hid-sensors/hid-sensor-attributes.h"
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+/*Format: HID-SENSOR-usage_id_in_hex*/
+/*Usage ID from spec for Magnetometer-3D: 0x200083*/
+#define DRIVER_NAME "HID-SENSOR-200083"
+
+enum magn_3d_channel {
+	CHANNEL_SCAN_INDEX_X,
+	CHANNEL_SCAN_INDEX_Y,
+	CHANNEL_SCAN_INDEX_Z,
+	MAGN_3D_CHANNEL_MAX,
+};
+
+struct magn_3d_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_iio_common common_attributes;
+	struct hid_sensor_hub_attribute_info magn[MAGN_3D_CHANNEL_MAX];
+	u32 magn_val[MAGN_3D_CHANNEL_MAX];
+};
+
+static const u32 magn_3d_addresses[MAGN_3D_CHANNEL_MAX] = {
+	HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS,
+	HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS,
+	HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec magn_3d_channels[] = {
+	{
+		.type = IIO_MAGN,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_X,
+	}, {
+		.type = IIO_MAGN,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Y,
+	}, {
+		.type = IIO_MAGN,
+		.modified = 1,
+		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SHARED_BIT |
+		IIO_CHAN_INFO_HYSTERESIS_SHARED_BIT,
+		.scan_index = CHANNEL_SCAN_INDEX_Z,
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void magn_3d_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+						int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	channels[channel].scan_type.storagebits = sizeof(u32) * 8;
+}
+
+/* Channel read_raw handler */
+static int magn_3d_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2,
+			      long mask)
+{
+	struct magn_3d_state *magn_state = iio_priv(indio_dev);
+	int report_id = -1;
+	u32 address;
+	int ret;
+	int ret_type;
+
+	*val = 0;
+	*val2 = 0;
+	switch (mask) {
+	case 0:
+		report_id =
+			magn_state->magn[chan->scan_index].report_id;
+		address = magn_3d_addresses[chan->scan_index];
+		if (report_id >= 0)
+			*val = sensor_hub_input_attr_get_raw_value(
+				magn_state->common_attributes.hsdev,
+				HID_USAGE_SENSOR_COMPASS_3D, address,
+				report_id);
+		else {
+			*val = 0;
+			return -EINVAL;
+		}
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = magn_state->magn[CHANNEL_SCAN_INDEX_X].units;
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = hid_sensor_convert_exponent(
+			magn_state->magn[CHANNEL_SCAN_INDEX_X].unit_expo);
+		ret_type = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_read_samp_freq_value(
+			&magn_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_read_raw_hyst_value(
+			&magn_state->common_attributes, val, val2);
+		ret_type = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int magn_3d_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct magn_3d_state *magn_state = iio_priv(indio_dev);
+	int ret = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&magn_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&magn_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int magn_3d_write_raw_get_fmt(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       long mask)
+{
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static const struct iio_info magn_3d_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &magn_3d_read_raw,
+	.write_raw = &magn_3d_write_raw,
+	.write_raw_get_fmt = &magn_3d_write_raw_get_fmt,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	int datum_sz;
+
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n");
+	if (!buffer) {
+		dev_err(&indio_dev->dev, "Buffer == NULL\n");
+		return;
+	}
+	datum_sz = buffer->access->get_bytes_per_datum(buffer);
+	if (len > datum_sz) {
+		dev_err(&indio_dev->dev, "Datum size mismatch %d:%d\n", len,
+				datum_sz);
+		return;
+	}
+	iio_push_to_buffer(buffer, (u8 *)data);
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int magn_3d_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct magn_3d_state *magn_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "magn_3d_proc_event [%d]\n",
+				magn_state->common_attributes.data_ready);
+	if (magn_state->common_attributes.data_ready)
+		hid_sensor_push_data(indio_dev,
+				(u8 *)magn_state->magn_val,
+				sizeof(magn_state->magn_val));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int magn_3d_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct magn_3d_state *magn_state = iio_priv(indio_dev);
+	int offset;
+	int ret = -EINVAL;
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS:
+	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS:
+	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS:
+		offset = usage_id - HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS;
+		magn_state->magn_val[CHANNEL_SCAN_INDEX_X + offset] =
+						*(u32 *)raw_data;
+		ret = 0;
+	break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/* Parse report which is specific to an usage id*/
+static int magn_3d_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct magn_3d_state *st)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i <= CHANNEL_SCAN_INDEX_Z; ++i) {
+		ret = sensor_hub_input_get_attribute_info(hsdev,
+				HID_INPUT_REPORT,
+				usage_id,
+				HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS + i,
+				&st->magn[CHANNEL_SCAN_INDEX_X + i]);
+		if (ret < 0)
+			break;
+		magn_3d_adjust_channel_bit_mask(channels,
+				CHANNEL_SCAN_INDEX_X + i,
+				st->magn[CHANNEL_SCAN_INDEX_X + i].size);
+	}
+	dev_dbg(&pdev->dev, "magn_3d %x:%x, %x:%x, %x:%x\n",
+			st->magn[0].index,
+			st->magn[0].report_id,
+			st->magn[1].index, st->magn[1].report_id,
+			st->magn[2].index, st->magn[2].report_id);
+
+	return ret;
+}
+
+/* Function to initialize the processing for usage id */
+static int __devinit hid_magn_3d_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	static char *name = "magn_3d";
+	struct iio_dev *indio_dev;
+	struct magn_3d_state *magn_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = iio_device_alloc(sizeof(struct magn_3d_state));
+	if (indio_dev == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	platform_set_drvdata(pdev, indio_dev);
+
+	magn_state = iio_priv(indio_dev);
+	magn_state->common_attributes.hsdev = hsdev;
+	magn_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+				HID_USAGE_SENSOR_COMPASS_3D,
+				&magn_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		goto error_free_dev;
+	}
+
+	channels = kmemdup(magn_3d_channels,
+					sizeof(magn_3d_channels),
+					GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		goto error_free_dev;
+	}
+
+	ret = magn_3d_parse_report(pdev, hsdev, channels,
+				HID_USAGE_SENSOR_COMPASS_3D, magn_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		goto error_free_dev_mem;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels = ARRAY_SIZE(magn_3d_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &magn_3d_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		goto error_free_dev_mem;
+	}
+	magn_state->common_attributes.data_ready = false;
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+					&magn_state->common_attributes);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	magn_state->callbacks.send_event = magn_3d_proc_event;
+	magn_state->callbacks.capture_sample = magn_3d_capture_sample;
+	magn_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_COMPASS_3D,
+					&magn_state->callbacks);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return ret;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(indio_dev);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_free_dev_mem:
+	kfree(indio_dev->channels);
+error_free_dev:
+	iio_device_free(indio_dev);
+error_ret:
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int __devinit hid_magn_3d_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_COMPASS_3D);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	kfree(indio_dev->channels);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver hid_magn_3d_platform_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= hid_magn_3d_probe,
+	.remove		= hid_magn_3d_remove,
+};
+module_platform_driver(hid_magn_3d_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Magnetometer 3D");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 51f42061dae9..6cfd4d8fd0bd 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -1361,11 +1361,11 @@ static int abort_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 	struct tid_info *t = dev->rdev.lldi.tids;
 
 	ep = lookup_tid(t, tid);
-	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 	if (!ep) {
 		printk(KERN_WARNING MOD "Abort rpl to freed endpoint\n");
 		return 0;
 	}
+	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 	mutex_lock(&ep->com.mutex);
 	switch (ep->com.state) {
 	case ABORTING:
diff --git a/drivers/infiniband/hw/ehca/ehca_irq.c b/drivers/infiniband/hw/ehca/ehca_irq.c
index 53589000fd07..8615d7cf7e01 100644
--- a/drivers/infiniband/hw/ehca/ehca_irq.c
+++ b/drivers/infiniband/hw/ehca/ehca_irq.c
@@ -42,6 +42,7 @@
  */
 
 #include <linux/slab.h>
+#include <linux/smpboot.h>
 
 #include "ehca_classes.h"
 #include "ehca_irq.h"
@@ -652,7 +653,7 @@ void ehca_tasklet_eq(unsigned long data)
 	ehca_process_eq((struct ehca_shca*)data, 1);
 }
 
-static inline int find_next_online_cpu(struct ehca_comp_pool *pool)
+static int find_next_online_cpu(struct ehca_comp_pool *pool)
 {
 	int cpu;
 	unsigned long flags;
@@ -662,17 +663,20 @@ static inline int find_next_online_cpu(struct ehca_comp_pool *pool)
 		ehca_dmp(cpu_online_mask, cpumask_size(), "");
 
 	spin_lock_irqsave(&pool->last_cpu_lock, flags);
-	cpu = cpumask_next(pool->last_cpu, cpu_online_mask);
-	if (cpu >= nr_cpu_ids)
-		cpu = cpumask_first(cpu_online_mask);
-	pool->last_cpu = cpu;
+	do {
+		cpu = cpumask_next(pool->last_cpu, cpu_online_mask);
+		if (cpu >= nr_cpu_ids)
+			cpu = cpumask_first(cpu_online_mask);
+		pool->last_cpu = cpu;
+	} while (!per_cpu_ptr(pool->cpu_comp_tasks, cpu)->active);
 	spin_unlock_irqrestore(&pool->last_cpu_lock, flags);
 
 	return cpu;
 }
 
 static void __queue_comp_task(struct ehca_cq *__cq,
-			      struct ehca_cpu_comp_task *cct)
+			      struct ehca_cpu_comp_task *cct,
+			      struct task_struct *thread)
 {
 	unsigned long flags;
 
@@ -683,7 +687,7 @@ static void __queue_comp_task(struct ehca_cq *__cq,
 		__cq->nr_callbacks++;
 		list_add_tail(&__cq->entry, &cct->cq_list);
 		cct->cq_jobs++;
-		wake_up(&cct->wait_queue);
+		wake_up_process(thread);
 	} else
 		__cq->nr_callbacks++;
 
@@ -695,6 +699,7 @@ static void queue_comp_task(struct ehca_cq *__cq)
 {
 	int cpu_id;
 	struct ehca_cpu_comp_task *cct;
+	struct task_struct *thread;
 	int cq_jobs;
 	unsigned long flags;
 
@@ -702,7 +707,8 @@ static void queue_comp_task(struct ehca_cq *__cq)
 	BUG_ON(!cpu_online(cpu_id));
 
 	cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu_id);
-	BUG_ON(!cct);
+	thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu_id);
+	BUG_ON(!cct || !thread);
 
 	spin_lock_irqsave(&cct->task_lock, flags);
 	cq_jobs = cct->cq_jobs;
@@ -710,28 +716,25 @@ static void queue_comp_task(struct ehca_cq *__cq)
 	if (cq_jobs > 0) {
 		cpu_id = find_next_online_cpu(pool);
 		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu_id);
-		BUG_ON(!cct);
+		thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu_id);
+		BUG_ON(!cct || !thread);
 	}
-
-	__queue_comp_task(__cq, cct);
+	__queue_comp_task(__cq, cct, thread);
 }
 
 static void run_comp_task(struct ehca_cpu_comp_task *cct)
 {
 	struct ehca_cq *cq;
-	unsigned long flags;
-
-	spin_lock_irqsave(&cct->task_lock, flags);
 
 	while (!list_empty(&cct->cq_list)) {
 		cq = list_entry(cct->cq_list.next, struct ehca_cq, entry);
-		spin_unlock_irqrestore(&cct->task_lock, flags);
+		spin_unlock_irq(&cct->task_lock);
 
 		comp_event_callback(cq);
 		if (atomic_dec_and_test(&cq->nr_events))
 			wake_up(&cq->wait_completion);
 
-		spin_lock_irqsave(&cct->task_lock, flags);
+		spin_lock_irq(&cct->task_lock);
 		spin_lock(&cq->task_lock);
 		cq->nr_callbacks--;
 		if (!cq->nr_callbacks) {
@@ -740,159 +743,76 @@ static void run_comp_task(struct ehca_cpu_comp_task *cct)
 		}
 		spin_unlock(&cq->task_lock);
 	}
-
-	spin_unlock_irqrestore(&cct->task_lock, flags);
 }
 
-static int comp_task(void *__cct)
+static void comp_task_park(unsigned int cpu)
 {
-	struct ehca_cpu_comp_task *cct = __cct;
-	int cql_empty;
-	DECLARE_WAITQUEUE(wait, current);
-
-	set_current_state(TASK_INTERRUPTIBLE);
-	while (!kthread_should_stop()) {
-		add_wait_queue(&cct->wait_queue, &wait);
-
-		spin_lock_irq(&cct->task_lock);
-		cql_empty = list_empty(&cct->cq_list);
-		spin_unlock_irq(&cct->task_lock);
-		if (cql_empty)
-			schedule();
-		else
-			__set_current_state(TASK_RUNNING);
-
-		remove_wait_queue(&cct->wait_queue, &wait);
+	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
+	struct ehca_cpu_comp_task *target;
+	struct task_struct *thread;
+	struct ehca_cq *cq, *tmp;
+	LIST_HEAD(list);
 
-		spin_lock_irq(&cct->task_lock);
-		cql_empty = list_empty(&cct->cq_list);
-		spin_unlock_irq(&cct->task_lock);
-		if (!cql_empty)
-			run_comp_task(__cct);
+	spin_lock_irq(&cct->task_lock);
+	cct->cq_jobs = 0;
+	cct->active = 0;
+	list_splice_init(&cct->cq_list, &list);
+	spin_unlock_irq(&cct->task_lock);
 
-		set_current_state(TASK_INTERRUPTIBLE);
+	cpu = find_next_online_cpu(pool);
+	target = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
+	thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu);
+	spin_lock_irq(&target->task_lock);
+	list_for_each_entry_safe(cq, tmp, &list, entry) {
+		list_del(&cq->entry);
+		__queue_comp_task(cq, target, thread);
 	}
-	__set_current_state(TASK_RUNNING);
-
-	return 0;
-}
-
-static struct task_struct *create_comp_task(struct ehca_comp_pool *pool,
-					    int cpu)
-{
-	struct ehca_cpu_comp_task *cct;
-
-	cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-	spin_lock_init(&cct->task_lock);
-	INIT_LIST_HEAD(&cct->cq_list);
-	init_waitqueue_head(&cct->wait_queue);
-	cct->task = kthread_create_on_node(comp_task, cct, cpu_to_node(cpu),
-					   "ehca_comp/%d", cpu);
-
-	return cct->task;
+	spin_unlock_irq(&target->task_lock);
 }
 
-static void destroy_comp_task(struct ehca_comp_pool *pool,
-			      int cpu)
+static void comp_task_stop(unsigned int cpu, bool online)
 {
-	struct ehca_cpu_comp_task *cct;
-	struct task_struct *task;
-	unsigned long flags_cct;
-
-	cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-
-	spin_lock_irqsave(&cct->task_lock, flags_cct);
+	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
 
-	task = cct->task;
-	cct->task = NULL;
+	spin_lock_irq(&cct->task_lock);
 	cct->cq_jobs = 0;
-
-	spin_unlock_irqrestore(&cct->task_lock, flags_cct);
-
-	if (task)
-		kthread_stop(task);
+	cct->active = 0;
+	WARN_ON(!list_empty(&cct->cq_list));
+	spin_unlock_irq(&cct->task_lock);
 }
 
-static void __cpuinit take_over_work(struct ehca_comp_pool *pool, int cpu)
+static int comp_task_should_run(unsigned int cpu)
 {
 	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-	LIST_HEAD(list);
-	struct ehca_cq *cq;
-	unsigned long flags_cct;
-
-	spin_lock_irqsave(&cct->task_lock, flags_cct);
-
-	list_splice_init(&cct->cq_list, &list);
-
-	while (!list_empty(&list)) {
-		cq = list_entry(cct->cq_list.next, struct ehca_cq, entry);
-
-		list_del(&cq->entry);
-		__queue_comp_task(cq, this_cpu_ptr(pool->cpu_comp_tasks));
-	}
-
-	spin_unlock_irqrestore(&cct->task_lock, flags_cct);
 
+	return cct->cq_jobs;
 }
 
-static int __cpuinit comp_pool_callback(struct notifier_block *nfb,
-					unsigned long action,
-					void *hcpu)
+static void comp_task(unsigned int cpu)
 {
-	unsigned int cpu = (unsigned long)hcpu;
-	struct ehca_cpu_comp_task *cct;
+	struct ehca_cpu_comp_task *cct = this_cpu_ptr(pool->cpu_comp_tasks);
+	int cql_empty;
 
-	switch (action) {
-	case CPU_UP_PREPARE:
-	case CPU_UP_PREPARE_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_PREPARE)", cpu);
-		if (!create_comp_task(pool, cpu)) {
-			ehca_gen_err("Can't create comp_task for cpu: %x", cpu);
-			return notifier_from_errno(-ENOMEM);
-		}
-		break;
-	case CPU_UP_CANCELED:
-	case CPU_UP_CANCELED_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_CANCELED)", cpu);
-		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-		kthread_bind(cct->task, cpumask_any(cpu_online_mask));
-		destroy_comp_task(pool, cpu);
-		break;
-	case CPU_ONLINE:
-	case CPU_ONLINE_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_ONLINE)", cpu);
-		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-		kthread_bind(cct->task, cpu);
-		wake_up_process(cct->task);
-		break;
-	case CPU_DOWN_PREPARE:
-	case CPU_DOWN_PREPARE_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_DOWN_PREPARE)", cpu);
-		break;
-	case CPU_DOWN_FAILED:
-	case CPU_DOWN_FAILED_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_DOWN_FAILED)", cpu);
-		break;
-	case CPU_DEAD:
-	case CPU_DEAD_FROZEN:
-		ehca_gen_dbg("CPU: %x (CPU_DEAD)", cpu);
-		destroy_comp_task(pool, cpu);
-		take_over_work(pool, cpu);
-		break;
+	spin_lock_irq(&cct->task_lock);
+	cql_empty = list_empty(&cct->cq_list);
+	if (!cql_empty) {
+		__set_current_state(TASK_RUNNING);
+		run_comp_task(cct);
 	}
-
-	return NOTIFY_OK;
+	spin_unlock_irq(&cct->task_lock);
 }
 
-static struct notifier_block comp_pool_callback_nb __cpuinitdata = {
-	.notifier_call	= comp_pool_callback,
-	.priority	= 0,
+static struct smp_hotplug_thread comp_pool_threads = {
+	.thread_should_run	= comp_task_should_run,
+	.thread_fn		= comp_task,
+	.thread_comm		= "ehca_comp/%u",
+	.cleanup		= comp_task_stop,
+	.park			= comp_task_park,
 };
 
 int ehca_create_comp_pool(void)
 {
-	int cpu;
-	struct task_struct *task;
+	int cpu, ret = -ENOMEM;
 
 	if (!ehca_scaling_code)
 		return 0;
@@ -905,38 +825,46 @@ int ehca_create_comp_pool(void)
 	pool->last_cpu = cpumask_any(cpu_online_mask);
 
 	pool->cpu_comp_tasks = alloc_percpu(struct ehca_cpu_comp_task);
-	if (pool->cpu_comp_tasks == NULL) {
-		kfree(pool);
-		return -EINVAL;
-	}
+	if (!pool->cpu_comp_tasks)
+		goto out_pool;
 
-	for_each_online_cpu(cpu) {
-		task = create_comp_task(pool, cpu);
-		if (task) {
-			kthread_bind(task, cpu);
-			wake_up_process(task);
-		}
+	pool->cpu_comp_threads = alloc_percpu(struct task_struct *);
+	if (!pool->cpu_comp_threads)
+		goto out_tasks;
+
+	for_each_present_cpu(cpu) {
+		struct ehca_cpu_comp_task *cct;
+
+		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
+		spin_lock_init(&cct->task_lock);
+		INIT_LIST_HEAD(&cct->cq_list);
 	}
 
-	register_hotcpu_notifier(&comp_pool_callback_nb);
+	comp_pool_threads.store = pool->cpu_comp_threads;
+	ret = smpboot_register_percpu_thread(&comp_pool_threads);
+	if (ret)
+		goto out_threads;
 
-	printk(KERN_INFO "eHCA scaling code enabled\n");
+	pr_info("eHCA scaling code enabled\n");
+	return ret;
 
-	return 0;
+out_threads:
+	free_percpu(pool->cpu_comp_threads);
+out_tasks:
+	free_percpu(pool->cpu_comp_tasks);
+out_pool:
+	kfree(pool);
+	return ret;
 }
 
 void ehca_destroy_comp_pool(void)
 {
-	int i;
-
 	if (!ehca_scaling_code)
 		return;
 
-	unregister_hotcpu_notifier(&comp_pool_callback_nb);
-
-	for_each_online_cpu(i)
-		destroy_comp_task(pool, i);
+	smpboot_unregister_percpu_thread(&comp_pool_threads);
 
+	free_percpu(pool->cpu_comp_threads);
 	free_percpu(pool->cpu_comp_tasks);
 	kfree(pool);
 }
diff --git a/drivers/infiniband/hw/ehca/ehca_irq.h b/drivers/infiniband/hw/ehca/ehca_irq.h
index 3346cb06cea6..5370199f08c7 100644
--- a/drivers/infiniband/hw/ehca/ehca_irq.h
+++ b/drivers/infiniband/hw/ehca/ehca_irq.h
@@ -58,15 +58,15 @@ void ehca_tasklet_eq(unsigned long data);
 void ehca_process_eq(struct ehca_shca *shca, int is_irq);
 
 struct ehca_cpu_comp_task {
-	wait_queue_head_t wait_queue;
 	struct list_head cq_list;
-	struct task_struct *task;
 	spinlock_t task_lock;
 	int cq_jobs;
+	int active;
 };
 
 struct ehca_comp_pool {
-	struct ehca_cpu_comp_task *cpu_comp_tasks;
+	struct ehca_cpu_comp_task __percpu *cpu_comp_tasks;
+	struct task_struct * __percpu *cpu_comp_threads;
 	int last_cpu;
 	spinlock_t last_cpu_lock;
 };
diff --git a/drivers/infiniband/hw/mthca/mthca_reset.c b/drivers/infiniband/hw/mthca/mthca_reset.c
index 4fa3534ec233..74c6a9426047 100644
--- a/drivers/infiniband/hw/mthca/mthca_reset.c
+++ b/drivers/infiniband/hw/mthca/mthca_reset.c
@@ -241,16 +241,16 @@ good:
 
 	if (hca_pcie_cap) {
 		devctl = hca_header[(hca_pcie_cap + PCI_EXP_DEVCTL) / 4];
-		if (pci_write_config_word(mdev->pdev, hca_pcie_cap + PCI_EXP_DEVCTL,
-					   devctl)) {
+		if (pcie_capability_write_word(mdev->pdev, PCI_EXP_DEVCTL,
+					       devctl)) {
 			err = -ENODEV;
 			mthca_err(mdev, "Couldn't restore HCA PCI Express "
 				  "Device Control register, aborting.\n");
 			goto out;
 		}
 		linkctl = hca_header[(hca_pcie_cap + PCI_EXP_LNKCTL) / 4];
-		if (pci_write_config_word(mdev->pdev, hca_pcie_cap + PCI_EXP_LNKCTL,
-					   linkctl)) {
+		if (pcie_capability_write_word(mdev->pdev, PCI_EXP_LNKCTL,
+					       linkctl)) {
 			err = -ENODEV;
 			mthca_err(mdev, "Couldn't restore HCA PCI Express "
 				  "Link control register, aborting.\n");
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index cb5b7f7d4d38..b29a4246ef41 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -2219,7 +2219,6 @@ static bool ocrdma_poll_success_scqe(struct ocrdma_qp *qp,
 	u32 wqe_idx;
 
 	if (!qp->wqe_wr_id_tbl[tail].signaled) {
-		expand = true;	/* CQE cannot be consumed yet */
 		*polled = false;    /* WC cannot be consumed yet */
 	} else {
 		ibwc->status = IB_WC_SUCCESS;
@@ -2227,10 +2226,11 @@ static bool ocrdma_poll_success_scqe(struct ocrdma_qp *qp,
 		ibwc->qp = &qp->ibqp;
 		ocrdma_update_wc(qp, ibwc, tail);
 		*polled = true;
-		wqe_idx = le32_to_cpu(cqe->wq.wqeidx) &	OCRDMA_CQE_WQEIDX_MASK;
-		if (tail != wqe_idx)
-			expand = true; /* Coalesced CQE can't be consumed yet */
 	}
+	wqe_idx = le32_to_cpu(cqe->wq.wqeidx) &	OCRDMA_CQE_WQEIDX_MASK;
+	if (tail != wqe_idx)
+		expand = true; /* Coalesced CQE can't be consumed yet */
+
 	ocrdma_hwq_inc_tail(&qp->sq);
 	return expand;
 }
diff --git a/drivers/infiniband/hw/qib/qib.h b/drivers/infiniband/hw/qib/qib.h
index 7b1b86690024..4d11575c2010 100644
--- a/drivers/infiniband/hw/qib/qib.h
+++ b/drivers/infiniband/hw/qib/qib.h
@@ -87,7 +87,7 @@ struct qlogic_ib_stats {
 };
 
 extern struct qlogic_ib_stats qib_stats;
-extern struct pci_error_handlers qib_pci_err_handler;
+extern const struct pci_error_handlers qib_pci_err_handler;
 extern struct pci_driver qib_driver;
 
 #define QIB_CHIP_SWVERSION QIB_CHIP_VERS_MAJ
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index 19f1e6c45fb6..ccb119143d20 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -471,9 +471,10 @@ static int subn_get_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 		if (port_num != port) {
 			ibp = to_iport(ibdev, port_num);
 			ret = check_mkey(ibp, smp, 0);
-			if (ret)
+			if (ret) {
 				ret = IB_MAD_RESULT_FAILURE;
 				goto bail;
+			}
 		}
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_pcie.c b/drivers/infiniband/hw/qib/qib_pcie.c
index 062c301ebf53..c574ec7c85e6 100644
--- a/drivers/infiniband/hw/qib/qib_pcie.c
+++ b/drivers/infiniband/hw/qib/qib_pcie.c
@@ -273,10 +273,9 @@ int qib_pcie_params(struct qib_devdata *dd, u32 minw, u32 *nent,
 		    struct qib_msix_entry *entry)
 {
 	u16 linkstat, speed;
-	int pos = 0, pose, ret = 1;
+	int pos = 0, ret = 1;
 
-	pose = pci_pcie_cap(dd->pcidev);
-	if (!pose) {
+	if (!pci_is_pcie(dd->pcidev)) {
 		qib_dev_err(dd, "Can't find PCI Express capability!\n");
 		/* set up something... */
 		dd->lbus_width = 1;
@@ -298,7 +297,7 @@ int qib_pcie_params(struct qib_devdata *dd, u32 minw, u32 *nent,
 	if (!pos)
 		qib_enable_intx(dd->pcidev);
 
-	pci_read_config_word(dd->pcidev, pose + PCI_EXP_LNKSTA, &linkstat);
+	pcie_capability_read_word(dd->pcidev, PCI_EXP_LNKSTA, &linkstat);
 	/*
 	 * speed is bits 0-3, linkwidth is bits 4-8
 	 * no defines for them in headers
@@ -516,7 +515,6 @@ static int qib_tune_pcie_coalesce(struct qib_devdata *dd)
 {
 	int r;
 	struct pci_dev *parent;
-	int ppos;
 	u16 devid;
 	u32 mask, bits, val;
 
@@ -529,8 +527,7 @@ static int qib_tune_pcie_coalesce(struct qib_devdata *dd)
 		qib_devinfo(dd->pcidev, "Parent not root\n");
 		return 1;
 	}
-	ppos = pci_pcie_cap(parent);
-	if (!ppos)
+	if (!pci_is_pcie(parent))
 		return 1;
 	if (parent->vendor != 0x8086)
 		return 1;
@@ -587,7 +584,6 @@ static int qib_tune_pcie_caps(struct qib_devdata *dd)
 {
 	int ret = 1; /* Assume the worst */
 	struct pci_dev *parent;
-	int ppos, epos;
 	u16 pcaps, pctl, ecaps, ectl;
 	int rc_sup, ep_sup;
 	int rc_cur, ep_cur;
@@ -598,19 +594,15 @@ static int qib_tune_pcie_caps(struct qib_devdata *dd)
 		qib_devinfo(dd->pcidev, "Parent not root\n");
 		goto bail;
 	}
-	ppos = pci_pcie_cap(parent);
-	if (ppos) {
-		pci_read_config_word(parent, ppos + PCI_EXP_DEVCAP, &pcaps);
-		pci_read_config_word(parent, ppos + PCI_EXP_DEVCTL, &pctl);
-	} else
+
+	if (!pci_is_pcie(parent) || !pci_is_pcie(dd->pcidev))
 		goto bail;
+	pcie_capability_read_word(parent, PCI_EXP_DEVCAP, &pcaps);
+	pcie_capability_read_word(parent, PCI_EXP_DEVCTL, &pctl);
 	/* Find out supported and configured values for endpoint (us) */
-	epos = pci_pcie_cap(dd->pcidev);
-	if (epos) {
-		pci_read_config_word(dd->pcidev, epos + PCI_EXP_DEVCAP, &ecaps);
-		pci_read_config_word(dd->pcidev, epos + PCI_EXP_DEVCTL, &ectl);
-	} else
-		goto bail;
+	pcie_capability_read_word(dd->pcidev, PCI_EXP_DEVCAP, &ecaps);
+	pcie_capability_read_word(dd->pcidev, PCI_EXP_DEVCTL, &ectl);
+
 	ret = 0;
 	/* Find max payload supported by root, endpoint */
 	rc_sup = fld2val(pcaps, PCI_EXP_DEVCAP_PAYLOAD);
@@ -629,14 +621,14 @@ static int qib_tune_pcie_caps(struct qib_devdata *dd)
 		rc_cur = rc_sup;
 		pctl = (pctl & ~PCI_EXP_DEVCTL_PAYLOAD) |
 			val2fld(rc_cur, PCI_EXP_DEVCTL_PAYLOAD);
-		pci_write_config_word(parent, ppos + PCI_EXP_DEVCTL, pctl);
+		pcie_capability_write_word(parent, PCI_EXP_DEVCTL, pctl);
 	}
 	/* If less than (allowed, supported), bump endpoint payload */
 	if (rc_sup > ep_cur) {
 		ep_cur = rc_sup;
 		ectl = (ectl & ~PCI_EXP_DEVCTL_PAYLOAD) |
 			val2fld(ep_cur, PCI_EXP_DEVCTL_PAYLOAD);
-		pci_write_config_word(dd->pcidev, epos + PCI_EXP_DEVCTL, ectl);
+		pcie_capability_write_word(dd->pcidev, PCI_EXP_DEVCTL, ectl);
 	}
 
 	/*
@@ -654,13 +646,13 @@ static int qib_tune_pcie_caps(struct qib_devdata *dd)
 		rc_cur = rc_sup;
 		pctl = (pctl & ~PCI_EXP_DEVCTL_READRQ) |
 			val2fld(rc_cur, PCI_EXP_DEVCTL_READRQ);
-		pci_write_config_word(parent, ppos + PCI_EXP_DEVCTL, pctl);
+		pcie_capability_write_word(parent, PCI_EXP_DEVCTL, pctl);
 	}
 	if (rc_sup > ep_cur) {
 		ep_cur = rc_sup;
 		ectl = (ectl & ~PCI_EXP_DEVCTL_READRQ) |
 			val2fld(ep_cur, PCI_EXP_DEVCTL_READRQ);
-		pci_write_config_word(dd->pcidev, epos + PCI_EXP_DEVCTL, ectl);
+		pcie_capability_write_word(dd->pcidev, PCI_EXP_DEVCTL, ectl);
 	}
 bail:
 	return ret;
@@ -753,7 +745,7 @@ qib_pci_resume(struct pci_dev *pdev)
 	qib_init(dd, 1); /* same as re-init after reset */
 }
 
-struct pci_error_handlers qib_pci_err_handler = {
+const struct pci_error_handlers qib_pci_err_handler = {
 	.error_detected = qib_pci_error_detected,
 	.mmio_enabled = qib_pci_mmio_enabled,
 	.link_reset = qib_pci_link_reset,
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index ca43901ed861..0af216d21f87 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -262,7 +262,10 @@ struct ipoib_ethtool_st {
 	u16     max_coalesced_frames;
 };
 
+struct ipoib_neigh_table;
+
 struct ipoib_neigh_hash {
+	struct ipoib_neigh_table       *ntbl;
 	struct ipoib_neigh __rcu      **buckets;
 	struct rcu_head			rcu;
 	u32				mask;
@@ -271,9 +274,9 @@ struct ipoib_neigh_hash {
 
 struct ipoib_neigh_table {
 	struct ipoib_neigh_hash __rcu  *htbl;
-	rwlock_t			rwlock;
 	atomic_t			entries;
 	struct completion		flushed;
+	struct completion		deleted;
 };
 
 /*
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 3e2085a3ee47..1e19b5ae7c47 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -546,15 +546,15 @@ static void neigh_add_path(struct sk_buff *skb, u8 *daddr,
 	struct ipoib_neigh *neigh;
 	unsigned long flags;
 
+	spin_lock_irqsave(&priv->lock, flags);
 	neigh = ipoib_neigh_alloc(daddr, dev);
 	if (!neigh) {
+		spin_unlock_irqrestore(&priv->lock, flags);
 		++dev->stats.tx_dropped;
 		dev_kfree_skb_any(skb);
 		return;
 	}
 
-	spin_lock_irqsave(&priv->lock, flags);
-
 	path = __path_find(dev, daddr + 4);
 	if (!path) {
 		path = path_rec_create(dev, daddr + 4);
@@ -863,10 +863,10 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 	if (test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
 		return;
 
-	write_lock_bh(&ntbl->rwlock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	htbl = rcu_dereference_protected(ntbl->htbl,
-					 lockdep_is_held(&ntbl->rwlock));
+					 lockdep_is_held(&priv->lock));
 
 	if (!htbl)
 		goto out_unlock;
@@ -883,16 +883,14 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
 
 		while ((neigh = rcu_dereference_protected(*np,
-							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+							  lockdep_is_held(&priv->lock))) != NULL) {
 			/* was the neigh idle for two GC periods */
 			if (time_after(neigh_obsolete, neigh->alive)) {
 				rcu_assign_pointer(*np,
 						   rcu_dereference_protected(neigh->hnext,
-									     lockdep_is_held(&ntbl->rwlock)));
+									     lockdep_is_held(&priv->lock)));
 				/* remove from path/mc list */
-				spin_lock_irqsave(&priv->lock, flags);
 				list_del(&neigh->list);
-				spin_unlock_irqrestore(&priv->lock, flags);
 				call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
 			} else {
 				np = &neigh->hnext;
@@ -902,7 +900,7 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 	}
 
 out_unlock:
-	write_unlock_bh(&ntbl->rwlock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
 static void ipoib_reap_neigh(struct work_struct *work)
@@ -947,10 +945,8 @@ struct ipoib_neigh *ipoib_neigh_alloc(u8 *daddr,
 	struct ipoib_neigh *neigh;
 	u32 hash_val;
 
-	write_lock_bh(&ntbl->rwlock);
-
 	htbl = rcu_dereference_protected(ntbl->htbl,
-					 lockdep_is_held(&ntbl->rwlock));
+					 lockdep_is_held(&priv->lock));
 	if (!htbl) {
 		neigh = NULL;
 		goto out_unlock;
@@ -961,10 +957,10 @@ struct ipoib_neigh *ipoib_neigh_alloc(u8 *daddr,
 	 */
 	hash_val = ipoib_addr_hash(htbl, daddr);
 	for (neigh = rcu_dereference_protected(htbl->buckets[hash_val],
-					       lockdep_is_held(&ntbl->rwlock));
+					       lockdep_is_held(&priv->lock));
 	     neigh != NULL;
 	     neigh = rcu_dereference_protected(neigh->hnext,
-					       lockdep_is_held(&ntbl->rwlock))) {
+					       lockdep_is_held(&priv->lock))) {
 		if (memcmp(daddr, neigh->daddr, INFINIBAND_ALEN) == 0) {
 			/* found, take one ref on behalf of the caller */
 			if (!atomic_inc_not_zero(&neigh->refcnt)) {
@@ -987,12 +983,11 @@ struct ipoib_neigh *ipoib_neigh_alloc(u8 *daddr,
 	/* put in hash */
 	rcu_assign_pointer(neigh->hnext,
 			   rcu_dereference_protected(htbl->buckets[hash_val],
-						     lockdep_is_held(&ntbl->rwlock)));
+						     lockdep_is_held(&priv->lock)));
 	rcu_assign_pointer(htbl->buckets[hash_val], neigh);
 	atomic_inc(&ntbl->entries);
 
 out_unlock:
-	write_unlock_bh(&ntbl->rwlock);
 
 	return neigh;
 }
@@ -1040,35 +1035,29 @@ void ipoib_neigh_free(struct ipoib_neigh *neigh)
 	struct ipoib_neigh *n;
 	u32 hash_val;
 
-	write_lock_bh(&ntbl->rwlock);
-
 	htbl = rcu_dereference_protected(ntbl->htbl,
-					lockdep_is_held(&ntbl->rwlock));
+					lockdep_is_held(&priv->lock));
 	if (!htbl)
-		goto out_unlock;
+		return;
 
 	hash_val = ipoib_addr_hash(htbl, neigh->daddr);
 	np = &htbl->buckets[hash_val];
 	for (n = rcu_dereference_protected(*np,
-					    lockdep_is_held(&ntbl->rwlock));
+					    lockdep_is_held(&priv->lock));
 	     n != NULL;
 	     n = rcu_dereference_protected(*np,
-					lockdep_is_held(&ntbl->rwlock))) {
+					lockdep_is_held(&priv->lock))) {
 		if (n == neigh) {
 			/* found */
 			rcu_assign_pointer(*np,
 					   rcu_dereference_protected(neigh->hnext,
-								     lockdep_is_held(&ntbl->rwlock)));
+								     lockdep_is_held(&priv->lock)));
 			call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
-			goto out_unlock;
+			return;
 		} else {
 			np = &n->hnext;
 		}
 	}
-
-out_unlock:
-	write_unlock_bh(&ntbl->rwlock);
-
 }
 
 static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
@@ -1080,7 +1069,6 @@ static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
 
 	clear_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags);
 	ntbl->htbl = NULL;
-	rwlock_init(&ntbl->rwlock);
 	htbl = kzalloc(sizeof(*htbl), GFP_KERNEL);
 	if (!htbl)
 		return -ENOMEM;
@@ -1095,6 +1083,7 @@ static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
 	htbl->mask = (size - 1);
 	htbl->buckets = buckets;
 	ntbl->htbl = htbl;
+	htbl->ntbl = ntbl;
 	atomic_set(&ntbl->entries, 0);
 
 	/* start garbage collection */
@@ -1111,9 +1100,11 @@ static void neigh_hash_free_rcu(struct rcu_head *head)
 						    struct ipoib_neigh_hash,
 						    rcu);
 	struct ipoib_neigh __rcu **buckets = htbl->buckets;
+	struct ipoib_neigh_table *ntbl = htbl->ntbl;
 
 	kfree(buckets);
 	kfree(htbl);
+	complete(&ntbl->deleted);
 }
 
 void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid)
@@ -1125,10 +1116,10 @@ void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid)
 	int i;
 
 	/* remove all neigh connected to a given path or mcast */
-	write_lock_bh(&ntbl->rwlock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	htbl = rcu_dereference_protected(ntbl->htbl,
-					 lockdep_is_held(&ntbl->rwlock));
+					 lockdep_is_held(&priv->lock));
 
 	if (!htbl)
 		goto out_unlock;
@@ -1138,16 +1129,14 @@ void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid)
 		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
 
 		while ((neigh = rcu_dereference_protected(*np,
-							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+							  lockdep_is_held(&priv->lock))) != NULL) {
 			/* delete neighs belong to this parent */
 			if (!memcmp(gid, neigh->daddr + 4, sizeof (union ib_gid))) {
 				rcu_assign_pointer(*np,
 						   rcu_dereference_protected(neigh->hnext,
-									     lockdep_is_held(&ntbl->rwlock)));
+									     lockdep_is_held(&priv->lock)));
 				/* remove from parent list */
-				spin_lock_irqsave(&priv->lock, flags);
 				list_del(&neigh->list);
-				spin_unlock_irqrestore(&priv->lock, flags);
 				call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
 			} else {
 				np = &neigh->hnext;
@@ -1156,7 +1145,7 @@ void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid)
 		}
 	}
 out_unlock:
-	write_unlock_bh(&ntbl->rwlock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
 static void ipoib_flush_neighs(struct ipoib_dev_priv *priv)
@@ -1164,37 +1153,44 @@ static void ipoib_flush_neighs(struct ipoib_dev_priv *priv)
 	struct ipoib_neigh_table *ntbl = &priv->ntbl;
 	struct ipoib_neigh_hash *htbl;
 	unsigned long flags;
-	int i;
+	int i, wait_flushed = 0;
 
-	write_lock_bh(&ntbl->rwlock);
+	init_completion(&priv->ntbl.flushed);
+
+	spin_lock_irqsave(&priv->lock, flags);
 
 	htbl = rcu_dereference_protected(ntbl->htbl,
-					lockdep_is_held(&ntbl->rwlock));
+					lockdep_is_held(&priv->lock));
 	if (!htbl)
 		goto out_unlock;
 
+	wait_flushed = atomic_read(&priv->ntbl.entries);
+	if (!wait_flushed)
+		goto free_htbl;
+
 	for (i = 0; i < htbl->size; i++) {
 		struct ipoib_neigh *neigh;
 		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
 
 		while ((neigh = rcu_dereference_protected(*np,
-							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+				       lockdep_is_held(&priv->lock))) != NULL) {
 			rcu_assign_pointer(*np,
 					   rcu_dereference_protected(neigh->hnext,
-								     lockdep_is_held(&ntbl->rwlock)));
+								     lockdep_is_held(&priv->lock)));
 			/* remove from path/mc list */
-			spin_lock_irqsave(&priv->lock, flags);
 			list_del(&neigh->list);
-			spin_unlock_irqrestore(&priv->lock, flags);
 			call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
 		}
 	}
 
+free_htbl:
 	rcu_assign_pointer(ntbl->htbl, NULL);
 	call_rcu(&htbl->rcu, neigh_hash_free_rcu);
 
 out_unlock:
-	write_unlock_bh(&ntbl->rwlock);
+	spin_unlock_irqrestore(&priv->lock, flags);
+	if (wait_flushed)
+		wait_for_completion(&priv->ntbl.flushed);
 }
 
 static void ipoib_neigh_hash_uninit(struct net_device *dev)
@@ -1203,7 +1199,7 @@ static void ipoib_neigh_hash_uninit(struct net_device *dev)
 	int stopped;
 
 	ipoib_dbg(priv, "ipoib_neigh_hash_uninit\n");
-	init_completion(&priv->ntbl.flushed);
+	init_completion(&priv->ntbl.deleted);
 	set_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags);
 
 	/* Stop GC if called at init fail need to cancel work */
@@ -1211,10 +1207,9 @@ static void ipoib_neigh_hash_uninit(struct net_device *dev)
 	if (!stopped)
 		cancel_delayed_work(&priv->neigh_reap_task);
 
-	if (atomic_read(&priv->ntbl.entries)) {
-		ipoib_flush_neighs(priv);
-		wait_for_completion(&priv->ntbl.flushed);
-	}
+	ipoib_flush_neighs(priv);
+
+	wait_for_completion(&priv->ntbl.deleted);
 }
 
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index 13f4aa7593c8..75367249f447 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -707,9 +707,7 @@ out:
 		neigh = ipoib_neigh_get(dev, daddr);
 		spin_lock_irqsave(&priv->lock, flags);
 		if (!neigh) {
-			spin_unlock_irqrestore(&priv->lock, flags);
 			neigh = ipoib_neigh_alloc(daddr, dev);
-			spin_lock_irqsave(&priv->lock, flags);
 			if (neigh) {
 				kref_get(&mcast->ah->ref);
 				neigh->ah	= mcast->ah;
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 6c58bfff01a3..118d0300f1fb 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -54,16 +54,9 @@ struct evdev_client {
 static struct evdev *evdev_table[EVDEV_MINORS];
 static DEFINE_MUTEX(evdev_table_mutex);
 
-static void evdev_pass_event(struct evdev_client *client,
-			     struct input_event *event,
-			     ktime_t mono, ktime_t real)
+static void __pass_event(struct evdev_client *client,
+			 const struct input_event *event)
 {
-	event->time = ktime_to_timeval(client->clkid == CLOCK_MONOTONIC ?
-					mono : real);
-
-	/* Interrupts are disabled, just acquire the lock. */
-	spin_lock(&client->buffer_lock);
-
 	client->buffer[client->head++] = *event;
 	client->head &= client->bufsize - 1;
 
@@ -86,42 +79,74 @@ static void evdev_pass_event(struct evdev_client *client,
 		client->packet_head = client->head;
 		kill_fasync(&client->fasync, SIGIO, POLL_IN);
 	}
+}
+
+static void evdev_pass_values(struct evdev_client *client,
+			const struct input_value *vals, unsigned int count,
+			ktime_t mono, ktime_t real)
+{
+	struct evdev *evdev = client->evdev;
+	const struct input_value *v;
+	struct input_event event;
+	bool wakeup = false;
+
+	event.time = ktime_to_timeval(client->clkid == CLOCK_MONOTONIC ?
+				      mono : real);
+
+	/* Interrupts are disabled, just acquire the lock. */
+	spin_lock(&client->buffer_lock);
+
+	for (v = vals; v != vals + count; v++) {
+		event.type = v->type;
+		event.code = v->code;
+		event.value = v->value;
+		__pass_event(client, &event);
+		if (v->type == EV_SYN && v->code == SYN_REPORT)
+			wakeup = true;
+	}
 
 	spin_unlock(&client->buffer_lock);
+
+	if (wakeup)
+		wake_up_interruptible(&evdev->wait);
 }
 
 /*
- * Pass incoming event to all connected clients.
+ * Pass incoming events to all connected clients.
  */
-static void evdev_event(struct input_handle *handle,
-			unsigned int type, unsigned int code, int value)
+static void evdev_events(struct input_handle *handle,
+			 const struct input_value *vals, unsigned int count)
 {
 	struct evdev *evdev = handle->private;
 	struct evdev_client *client;
-	struct input_event event;
 	ktime_t time_mono, time_real;
 
 	time_mono = ktime_get();
 	time_real = ktime_sub(time_mono, ktime_get_monotonic_offset());
 
-	event.type = type;
-	event.code = code;
-	event.value = value;
-
 	rcu_read_lock();
 
 	client = rcu_dereference(evdev->grab);
 
 	if (client)
-		evdev_pass_event(client, &event, time_mono, time_real);
+		evdev_pass_values(client, vals, count, time_mono, time_real);
 	else
 		list_for_each_entry_rcu(client, &evdev->client_list, node)
-			evdev_pass_event(client, &event, time_mono, time_real);
+			evdev_pass_values(client, vals, count,
+					  time_mono, time_real);
 
 	rcu_read_unlock();
+}
 
-	if (type == EV_SYN && code == SYN_REPORT)
-		wake_up_interruptible(&evdev->wait);
+/*
+ * Pass incoming event to all connected clients.
+ */
+static void evdev_event(struct input_handle *handle,
+			unsigned int type, unsigned int code, int value)
+{
+	struct input_value vals[] = { { type, code, value } };
+
+	evdev_events(handle, vals, 1);
 }
 
 static int evdev_fasync(int fd, struct file *file, int on)
@@ -653,20 +678,22 @@ static int evdev_handle_mt_request(struct input_dev *dev,
 				   unsigned int size,
 				   int __user *ip)
 {
-	const struct input_mt_slot *mt = dev->mt;
+	const struct input_mt *mt = dev->mt;
 	unsigned int code;
 	int max_slots;
 	int i;
 
 	if (get_user(code, &ip[0]))
 		return -EFAULT;
-	if (!input_is_mt_value(code))
+	if (!mt || !input_is_mt_value(code))
 		return -EINVAL;
 
 	max_slots = (size - sizeof(__u32)) / sizeof(__s32);
-	for (i = 0; i < dev->mtsize && i < max_slots; i++)
-		if (put_user(input_mt_get_value(&mt[i], code), &ip[1 + i]))
+	for (i = 0; i < mt->num_slots && i < max_slots; i++) {
+		int value = input_mt_get_value(&mt->slots[i], code);
+		if (put_user(value, &ip[1 + i]))
 			return -EFAULT;
+	}
 
 	return 0;
 }
@@ -1048,6 +1075,7 @@ MODULE_DEVICE_TABLE(input, evdev_ids);
 
 static struct input_handler evdev_handler = {
 	.event		= evdev_event,
+	.events		= evdev_events,
 	.connect	= evdev_connect,
 	.disconnect	= evdev_disconnect,
 	.fops		= &evdev_fops,
diff --git a/drivers/input/input-mt.c b/drivers/input/input-mt.c
index 70a16c7da8cc..c0ec7d42c3be 100644
--- a/drivers/input/input-mt.c
+++ b/drivers/input/input-mt.c
@@ -14,6 +14,14 @@
 
 #define TRKID_SGN	((TRKID_MAX + 1) >> 1)
 
+static void copy_abs(struct input_dev *dev, unsigned int dst, unsigned int src)
+{
+	if (dev->absinfo && test_bit(src, dev->absbit)) {
+		dev->absinfo[dst] = dev->absinfo[src];
+		dev->absbit[BIT_WORD(dst)] |= BIT_MASK(dst);
+	}
+}
+
 /**
  * input_mt_init_slots() - initialize MT input slots
  * @dev: input device supporting MT events and finger tracking
@@ -25,29 +33,63 @@
  * May be called repeatedly. Returns -EINVAL if attempting to
  * reinitialize with a different number of slots.
  */
-int input_mt_init_slots(struct input_dev *dev, unsigned int num_slots)
+int input_mt_init_slots(struct input_dev *dev, unsigned int num_slots,
+			unsigned int flags)
 {
+	struct input_mt *mt = dev->mt;
 	int i;
 
 	if (!num_slots)
 		return 0;
-	if (dev->mt)
-		return dev->mtsize != num_slots ? -EINVAL : 0;
+	if (mt)
+		return mt->num_slots != num_slots ? -EINVAL : 0;
 
-	dev->mt = kcalloc(num_slots, sizeof(struct input_mt_slot), GFP_KERNEL);
-	if (!dev->mt)
-		return -ENOMEM;
+	mt = kzalloc(sizeof(*mt) + num_slots * sizeof(*mt->slots), GFP_KERNEL);
+	if (!mt)
+		goto err_mem;
 
-	dev->mtsize = num_slots;
+	mt->num_slots = num_slots;
+	mt->flags = flags;
 	input_set_abs_params(dev, ABS_MT_SLOT, 0, num_slots - 1, 0, 0);
 	input_set_abs_params(dev, ABS_MT_TRACKING_ID, 0, TRKID_MAX, 0, 0);
-	input_set_events_per_packet(dev, 6 * num_slots);
+
+	if (flags & (INPUT_MT_POINTER | INPUT_MT_DIRECT)) {
+		__set_bit(EV_KEY, dev->evbit);
+		__set_bit(BTN_TOUCH, dev->keybit);
+
+		copy_abs(dev, ABS_X, ABS_MT_POSITION_X);
+		copy_abs(dev, ABS_Y, ABS_MT_POSITION_Y);
+		copy_abs(dev, ABS_PRESSURE, ABS_MT_PRESSURE);
+	}
+	if (flags & INPUT_MT_POINTER) {
+		__set_bit(BTN_TOOL_FINGER, dev->keybit);
+		__set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
+		if (num_slots >= 3)
+			__set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
+		if (num_slots >= 4)
+			__set_bit(BTN_TOOL_QUADTAP, dev->keybit);
+		if (num_slots >= 5)
+			__set_bit(BTN_TOOL_QUINTTAP, dev->keybit);
+		__set_bit(INPUT_PROP_POINTER, dev->propbit);
+	}
+	if (flags & INPUT_MT_DIRECT)
+		__set_bit(INPUT_PROP_DIRECT, dev->propbit);
+	if (flags & INPUT_MT_TRACK) {
+		unsigned int n2 = num_slots * num_slots;
+		mt->red = kcalloc(n2, sizeof(*mt->red), GFP_KERNEL);
+		if (!mt->red)
+			goto err_mem;
+	}
 
 	/* Mark slots as 'unused' */
 	for (i = 0; i < num_slots; i++)
-		input_mt_set_value(&dev->mt[i], ABS_MT_TRACKING_ID, -1);
+		input_mt_set_value(&mt->slots[i], ABS_MT_TRACKING_ID, -1);
 
+	dev->mt = mt;
 	return 0;
+err_mem:
+	kfree(mt);
+	return -ENOMEM;
 }
 EXPORT_SYMBOL(input_mt_init_slots);
 
@@ -60,11 +102,11 @@ EXPORT_SYMBOL(input_mt_init_slots);
  */
 void input_mt_destroy_slots(struct input_dev *dev)
 {
-	kfree(dev->mt);
+	if (dev->mt) {
+		kfree(dev->mt->red);
+		kfree(dev->mt);
+	}
 	dev->mt = NULL;
-	dev->mtsize = 0;
-	dev->slot = 0;
-	dev->trkid = 0;
 }
 EXPORT_SYMBOL(input_mt_destroy_slots);
 
@@ -83,18 +125,24 @@ EXPORT_SYMBOL(input_mt_destroy_slots);
 void input_mt_report_slot_state(struct input_dev *dev,
 				unsigned int tool_type, bool active)
 {
-	struct input_mt_slot *mt;
+	struct input_mt *mt = dev->mt;
+	struct input_mt_slot *slot;
 	int id;
 
-	if (!dev->mt || !active) {
+	if (!mt)
+		return;
+
+	slot = &mt->slots[mt->slot];
+	slot->frame = mt->frame;
+
+	if (!active) {
 		input_event(dev, EV_ABS, ABS_MT_TRACKING_ID, -1);
 		return;
 	}
 
-	mt = &dev->mt[dev->slot];
-	id = input_mt_get_value(mt, ABS_MT_TRACKING_ID);
-	if (id < 0 || input_mt_get_value(mt, ABS_MT_TOOL_TYPE) != tool_type)
-		id = input_mt_new_trkid(dev);
+	id = input_mt_get_value(slot, ABS_MT_TRACKING_ID);
+	if (id < 0 || input_mt_get_value(slot, ABS_MT_TOOL_TYPE) != tool_type)
+		id = input_mt_new_trkid(mt);
 
 	input_event(dev, EV_ABS, ABS_MT_TRACKING_ID, id);
 	input_event(dev, EV_ABS, ABS_MT_TOOL_TYPE, tool_type);
@@ -135,13 +183,19 @@ EXPORT_SYMBOL(input_mt_report_finger_count);
  */
 void input_mt_report_pointer_emulation(struct input_dev *dev, bool use_count)
 {
-	struct input_mt_slot *oldest = NULL;
-	int oldid = dev->trkid;
-	int count = 0;
-	int i;
+	struct input_mt *mt = dev->mt;
+	struct input_mt_slot *oldest;
+	int oldid, count, i;
+
+	if (!mt)
+		return;
+
+	oldest = 0;
+	oldid = mt->trkid;
+	count = 0;
 
-	for (i = 0; i < dev->mtsize; ++i) {
-		struct input_mt_slot *ps = &dev->mt[i];
+	for (i = 0; i < mt->num_slots; ++i) {
+		struct input_mt_slot *ps = &mt->slots[i];
 		int id = input_mt_get_value(ps, ABS_MT_TRACKING_ID);
 
 		if (id < 0)
@@ -160,13 +214,208 @@ void input_mt_report_pointer_emulation(struct input_dev *dev, bool use_count)
 	if (oldest) {
 		int x = input_mt_get_value(oldest, ABS_MT_POSITION_X);
 		int y = input_mt_get_value(oldest, ABS_MT_POSITION_Y);
-		int p = input_mt_get_value(oldest, ABS_MT_PRESSURE);
 
 		input_event(dev, EV_ABS, ABS_X, x);
 		input_event(dev, EV_ABS, ABS_Y, y);
-		input_event(dev, EV_ABS, ABS_PRESSURE, p);
+
+		if (test_bit(ABS_MT_PRESSURE, dev->absbit)) {
+			int p = input_mt_get_value(oldest, ABS_MT_PRESSURE);
+			input_event(dev, EV_ABS, ABS_PRESSURE, p);
+		}
 	} else {
-		input_event(dev, EV_ABS, ABS_PRESSURE, 0);
+		if (test_bit(ABS_MT_PRESSURE, dev->absbit))
+			input_event(dev, EV_ABS, ABS_PRESSURE, 0);
 	}
 }
 EXPORT_SYMBOL(input_mt_report_pointer_emulation);
+
+/**
+ * input_mt_sync_frame() - synchronize mt frame
+ * @dev: input device with allocated MT slots
+ *
+ * Close the frame and prepare the internal state for a new one.
+ * Depending on the flags, marks unused slots as inactive and performs
+ * pointer emulation.
+ */
+void input_mt_sync_frame(struct input_dev *dev)
+{
+	struct input_mt *mt = dev->mt;
+	struct input_mt_slot *s;
+
+	if (!mt)
+		return;
+
+	if (mt->flags & INPUT_MT_DROP_UNUSED) {
+		for (s = mt->slots; s != mt->slots + mt->num_slots; s++) {
+			if (s->frame == mt->frame)
+				continue;
+			input_mt_slot(dev, s - mt->slots);
+			input_event(dev, EV_ABS, ABS_MT_TRACKING_ID, -1);
+		}
+	}
+
+	input_mt_report_pointer_emulation(dev, (mt->flags & INPUT_MT_POINTER));
+
+	mt->frame++;
+}
+EXPORT_SYMBOL(input_mt_sync_frame);
+
+static int adjust_dual(int *begin, int step, int *end, int eq)
+{
+	int f, *p, s, c;
+
+	if (begin == end)
+		return 0;
+
+	f = *begin;
+	p = begin + step;
+	s = p == end ? f + 1 : *p;
+
+	for (; p != end; p += step)
+		if (*p < f)
+			s = f, f = *p;
+		else if (*p < s)
+			s = *p;
+
+	c = (f + s + 1) / 2;
+	if (c == 0 || (c > 0 && !eq))
+		return 0;
+	if (s < 0)
+		c *= 2;
+
+	for (p = begin; p != end; p += step)
+		*p -= c;
+
+	return (c < s && s <= 0) || (f >= 0 && f < c);
+}
+
+static void find_reduced_matrix(int *w, int nr, int nc, int nrc)
+{
+	int i, k, sum;
+
+	for (k = 0; k < nrc; k++) {
+		for (i = 0; i < nr; i++)
+			adjust_dual(w + i, nr, w + i + nrc, nr <= nc);
+		sum = 0;
+		for (i = 0; i < nrc; i += nr)
+			sum += adjust_dual(w + i, 1, w + i + nr, nc <= nr);
+		if (!sum)
+			break;
+	}
+}
+
+static int input_mt_set_matrix(struct input_mt *mt,
+			       const struct input_mt_pos *pos, int num_pos)
+{
+	const struct input_mt_pos *p;
+	struct input_mt_slot *s;
+	int *w = mt->red;
+	int x, y;
+
+	for (s = mt->slots; s != mt->slots + mt->num_slots; s++) {
+		if (!input_mt_is_active(s))
+			continue;
+		x = input_mt_get_value(s, ABS_MT_POSITION_X);
+		y = input_mt_get_value(s, ABS_MT_POSITION_Y);
+		for (p = pos; p != pos + num_pos; p++) {
+			int dx = x - p->x, dy = y - p->y;
+			*w++ = dx * dx + dy * dy;
+		}
+	}
+
+	return w - mt->red;
+}
+
+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;
+
+	for (p = slots; p != slots + num_pos; p++)
+		*p = -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 (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;
+				break;
+			}
+	}
+}
+
+/**
+ * input_mt_assign_slots() - perform a best-match assignment
+ * @dev: input device with allocated MT slots
+ * @slots: the slot assignment to be filled
+ * @pos: the position array to match
+ * @num_pos: number of positions
+ *
+ * Performs a best match against the current contacts and returns
+ * the slot assignment list. New contacts are assigned to unused
+ * slots.
+ *
+ * Returns zero on success, or negative error in case of failure.
+ */
+int input_mt_assign_slots(struct input_dev *dev, int *slots,
+			  const struct input_mt_pos *pos, int num_pos)
+{
+	struct input_mt *mt = dev->mt;
+	int nrc;
+
+	if (!mt || !mt->red)
+		return -ENXIO;
+	if (num_pos > mt->num_slots)
+		return -EINVAL;
+	if (num_pos < 1)
+		return 0;
+
+	nrc = input_mt_set_matrix(mt, pos, num_pos);
+	find_reduced_matrix(mt->red, num_pos, nrc / num_pos, nrc);
+	input_mt_set_slots(mt, slots, num_pos);
+
+	return 0;
+}
+EXPORT_SYMBOL(input_mt_assign_slots);
+
+/**
+ * input_mt_get_slot_by_key() - return slot matching key
+ * @dev: input device with allocated MT slots
+ * @key: the key of the sought slot
+ *
+ * Returns the slot of the given key, if it exists, otherwise
+ * set the key on the first unused slot and return.
+ *
+ * If no available slot can be found, -1 is returned.
+ */
+int input_mt_get_slot_by_key(struct input_dev *dev, int key)
+{
+	struct input_mt *mt = dev->mt;
+	struct input_mt_slot *s;
+
+	if (!mt)
+		return -1;
+
+	for (s = mt->slots; s != mt->slots + mt->num_slots; s++)
+		if (input_mt_is_active(s) && s->key == key)
+			return s - mt->slots;
+
+	for (s = mt->slots; s != mt->slots + mt->num_slots; s++)
+		if (!input_mt_is_active(s)) {
+			s->key = key;
+			return s - mt->slots;
+		}
+
+	return -1;
+}
+EXPORT_SYMBOL(input_mt_get_slot_by_key);
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 8921c6180c51..5244f3d05b12 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -47,6 +47,8 @@ static DEFINE_MUTEX(input_mutex);
 
 static struct input_handler *input_table[8];
 
+static const struct input_value input_value_sync = { EV_SYN, SYN_REPORT, 1 };
+
 static inline int is_event_supported(unsigned int code,
 				     unsigned long *bm, unsigned int max)
 {
@@ -69,42 +71,102 @@ static int input_defuzz_abs_event(int value, int old_val, int fuzz)
 	return value;
 }
 
+static void input_start_autorepeat(struct input_dev *dev, int code)
+{
+	if (test_bit(EV_REP, dev->evbit) &&
+	    dev->rep[REP_PERIOD] && dev->rep[REP_DELAY] &&
+	    dev->timer.data) {
+		dev->repeat_key = code;
+		mod_timer(&dev->timer,
+			  jiffies + msecs_to_jiffies(dev->rep[REP_DELAY]));
+	}
+}
+
+static void input_stop_autorepeat(struct input_dev *dev)
+{
+	del_timer(&dev->timer);
+}
+
 /*
  * Pass event first through all filters and then, if event has not been
  * filtered out, through all open handles. This function is called with
  * dev->event_lock held and interrupts disabled.
  */
-static void input_pass_event(struct input_dev *dev,
-			     unsigned int type, unsigned int code, int value)
+static unsigned int input_to_handler(struct input_handle *handle,
+			struct input_value *vals, unsigned int count)
+{
+	struct input_handler *handler = handle->handler;
+	struct input_value *end = vals;
+	struct input_value *v;
+
+	for (v = vals; v != vals + count; v++) {
+		if (handler->filter &&
+		    handler->filter(handle, v->type, v->code, v->value))
+			continue;
+		if (end != v)
+			*end = *v;
+		end++;
+	}
+
+	count = end - vals;
+	if (!count)
+		return 0;
+
+	if (handler->events)
+		handler->events(handle, vals, count);
+	else if (handler->event)
+		for (v = vals; v != end; v++)
+			handler->event(handle, v->type, v->code, v->value);
+
+	return count;
+}
+
+/*
+ * Pass values first through all filters and then, if event has not been
+ * filtered out, through all open handles. This function is called with
+ * dev->event_lock held and interrupts disabled.
+ */
+static void input_pass_values(struct input_dev *dev,
+			      struct input_value *vals, unsigned int count)
 {
-	struct input_handler *handler;
 	struct input_handle *handle;
+	struct input_value *v;
+
+	if (!count)
+		return;
 
 	rcu_read_lock();
 
 	handle = rcu_dereference(dev->grab);
-	if (handle)
-		handle->handler->event(handle, type, code, value);
-	else {
-		bool filtered = false;
-
-		list_for_each_entry_rcu(handle, &dev->h_list, d_node) {
-			if (!handle->open)
-				continue;
+	if (handle) {
+		count = input_to_handler(handle, vals, count);
+	} else {
+		list_for_each_entry_rcu(handle, &dev->h_list, d_node)
+			if (handle->open)
+				count = input_to_handler(handle, vals, count);
+	}
 
-			handler = handle->handler;
-			if (!handler->filter) {
-				if (filtered)
-					break;
+	rcu_read_unlock();
 
-				handler->event(handle, type, code, value);
+	add_input_randomness(vals->type, vals->code, vals->value);
 
-			} else if (handler->filter(handle, type, code, value))
-				filtered = true;
+	/* trigger auto repeat for key events */
+	for (v = vals; v != vals + count; v++) {
+		if (v->type == EV_KEY && v->value != 2) {
+			if (v->value)
+				input_start_autorepeat(dev, v->code);
+			else
+				input_stop_autorepeat(dev);
 		}
 	}
+}
 
-	rcu_read_unlock();
+static void input_pass_event(struct input_dev *dev,
+			     unsigned int type, unsigned int code, int value)
+{
+	struct input_value vals[] = { { type, code, value } };
+
+	input_pass_values(dev, vals, ARRAY_SIZE(vals));
 }
 
 /*
@@ -121,18 +183,12 @@ static void input_repeat_key(unsigned long data)
 
 	if (test_bit(dev->repeat_key, dev->key) &&
 	    is_event_supported(dev->repeat_key, dev->keybit, KEY_MAX)) {
+		struct input_value vals[] =  {
+			{ EV_KEY, dev->repeat_key, 2 },
+			input_value_sync
+		};
 
-		input_pass_event(dev, EV_KEY, dev->repeat_key, 2);
-
-		if (dev->sync) {
-			/*
-			 * Only send SYN_REPORT if we are not in a middle
-			 * of driver parsing a new hardware packet.
-			 * Otherwise assume that the driver will send
-			 * SYN_REPORT once it's done.
-			 */
-			input_pass_event(dev, EV_SYN, SYN_REPORT, 1);
-		}
+		input_pass_values(dev, vals, ARRAY_SIZE(vals));
 
 		if (dev->rep[REP_PERIOD])
 			mod_timer(&dev->timer, jiffies +
@@ -142,30 +198,17 @@ static void input_repeat_key(unsigned long data)
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 }
 
-static void input_start_autorepeat(struct input_dev *dev, int code)
-{
-	if (test_bit(EV_REP, dev->evbit) &&
-	    dev->rep[REP_PERIOD] && dev->rep[REP_DELAY] &&
-	    dev->timer.data) {
-		dev->repeat_key = code;
-		mod_timer(&dev->timer,
-			  jiffies + msecs_to_jiffies(dev->rep[REP_DELAY]));
-	}
-}
-
-static void input_stop_autorepeat(struct input_dev *dev)
-{
-	del_timer(&dev->timer);
-}
-
 #define INPUT_IGNORE_EVENT	0
 #define INPUT_PASS_TO_HANDLERS	1
 #define INPUT_PASS_TO_DEVICE	2
+#define INPUT_SLOT		4
+#define INPUT_FLUSH		8
 #define INPUT_PASS_TO_ALL	(INPUT_PASS_TO_HANDLERS | INPUT_PASS_TO_DEVICE)
 
 static int input_handle_abs_event(struct input_dev *dev,
 				  unsigned int code, int *pval)
 {
+	struct input_mt *mt = dev->mt;
 	bool is_mt_event;
 	int *pold;
 
@@ -174,8 +217,8 @@ static int input_handle_abs_event(struct input_dev *dev,
 		 * "Stage" the event; we'll flush it later, when we
 		 * get actual touch data.
 		 */
-		if (*pval >= 0 && *pval < dev->mtsize)
-			dev->slot = *pval;
+		if (mt && *pval >= 0 && *pval < mt->num_slots)
+			mt->slot = *pval;
 
 		return INPUT_IGNORE_EVENT;
 	}
@@ -184,9 +227,8 @@ static int input_handle_abs_event(struct input_dev *dev,
 
 	if (!is_mt_event) {
 		pold = &dev->absinfo[code].value;
-	} else if (dev->mt) {
-		struct input_mt_slot *mtslot = &dev->mt[dev->slot];
-		pold = &mtslot->abs[code - ABS_MT_FIRST];
+	} else if (mt) {
+		pold = &mt->slots[mt->slot].abs[code - ABS_MT_FIRST];
 	} else {
 		/*
 		 * Bypass filtering for multi-touch events when
@@ -205,16 +247,16 @@ static int input_handle_abs_event(struct input_dev *dev,
 	}
 
 	/* Flush pending "slot" event */
-	if (is_mt_event && dev->slot != input_abs_get_val(dev, ABS_MT_SLOT)) {
-		input_abs_set_val(dev, ABS_MT_SLOT, dev->slot);
-		input_pass_event(dev, EV_ABS, ABS_MT_SLOT, dev->slot);
+	if (is_mt_event && mt && mt->slot != input_abs_get_val(dev, ABS_MT_SLOT)) {
+		input_abs_set_val(dev, ABS_MT_SLOT, mt->slot);
+		return INPUT_PASS_TO_HANDLERS | INPUT_SLOT;
 	}
 
 	return INPUT_PASS_TO_HANDLERS;
 }
 
-static void input_handle_event(struct input_dev *dev,
-			       unsigned int type, unsigned int code, int value)
+static int input_get_disposition(struct input_dev *dev,
+			  unsigned int type, unsigned int code, int value)
 {
 	int disposition = INPUT_IGNORE_EVENT;
 
@@ -227,37 +269,34 @@ static void input_handle_event(struct input_dev *dev,
 			break;
 
 		case SYN_REPORT:
-			if (!dev->sync) {
-				dev->sync = true;
-				disposition = INPUT_PASS_TO_HANDLERS;
-			}
+			disposition = INPUT_PASS_TO_HANDLERS | INPUT_FLUSH;
 			break;
 		case SYN_MT_REPORT:
-			dev->sync = false;
 			disposition = INPUT_PASS_TO_HANDLERS;
 			break;
 		}
 		break;
 
 	case EV_KEY:
-		if (is_event_supported(code, dev->keybit, KEY_MAX) &&
-		    !!test_bit(code, dev->key) != value) {
+		if (is_event_supported(code, dev->keybit, KEY_MAX)) {
 
-			if (value != 2) {
-				__change_bit(code, dev->key);
-				if (value)
-					input_start_autorepeat(dev, code);
-				else
-					input_stop_autorepeat(dev);
+			/* auto-repeat bypasses state updates */
+			if (value == 2) {
+				disposition = INPUT_PASS_TO_HANDLERS;
+				break;
 			}
 
-			disposition = INPUT_PASS_TO_HANDLERS;
+			if (!!test_bit(code, dev->key) != !!value) {
+
+				__change_bit(code, dev->key);
+				disposition = INPUT_PASS_TO_HANDLERS;
+			}
 		}
 		break;
 
 	case EV_SW:
 		if (is_event_supported(code, dev->swbit, SW_MAX) &&
-		    !!test_bit(code, dev->sw) != value) {
+		    !!test_bit(code, dev->sw) != !!value) {
 
 			__change_bit(code, dev->sw);
 			disposition = INPUT_PASS_TO_HANDLERS;
@@ -284,7 +323,7 @@ static void input_handle_event(struct input_dev *dev,
 
 	case EV_LED:
 		if (is_event_supported(code, dev->ledbit, LED_MAX) &&
-		    !!test_bit(code, dev->led) != value) {
+		    !!test_bit(code, dev->led) != !!value) {
 
 			__change_bit(code, dev->led);
 			disposition = INPUT_PASS_TO_ALL;
@@ -317,14 +356,48 @@ static void input_handle_event(struct input_dev *dev,
 		break;
 	}
 
-	if (disposition != INPUT_IGNORE_EVENT && type != EV_SYN)
-		dev->sync = false;
+	return disposition;
+}
+
+static void input_handle_event(struct input_dev *dev,
+			       unsigned int type, unsigned int code, int value)
+{
+	int disposition;
+
+	disposition = input_get_disposition(dev, type, code, value);
 
 	if ((disposition & INPUT_PASS_TO_DEVICE) && dev->event)
 		dev->event(dev, type, code, value);
 
-	if (disposition & INPUT_PASS_TO_HANDLERS)
-		input_pass_event(dev, type, code, value);
+	if (!dev->vals)
+		return;
+
+	if (disposition & INPUT_PASS_TO_HANDLERS) {
+		struct input_value *v;
+
+		if (disposition & INPUT_SLOT) {
+			v = &dev->vals[dev->num_vals++];
+			v->type = EV_ABS;
+			v->code = ABS_MT_SLOT;
+			v->value = dev->mt->slot;
+		}
+
+		v = &dev->vals[dev->num_vals++];
+		v->type = type;
+		v->code = code;
+		v->value = value;
+	}
+
+	if (disposition & INPUT_FLUSH) {
+		if (dev->num_vals >= 2)
+			input_pass_values(dev, dev->vals, dev->num_vals);
+		dev->num_vals = 0;
+	} else if (dev->num_vals >= dev->max_vals - 2) {
+		dev->vals[dev->num_vals++] = input_value_sync;
+		input_pass_values(dev, dev->vals, dev->num_vals);
+		dev->num_vals = 0;
+	}
+
 }
 
 /**
@@ -352,7 +425,6 @@ void input_event(struct input_dev *dev,
 	if (is_event_supported(type, dev->evbit, EV_MAX)) {
 
 		spin_lock_irqsave(&dev->event_lock, flags);
-		add_input_randomness(type, code, value);
 		input_handle_event(dev, type, code, value);
 		spin_unlock_irqrestore(&dev->event_lock, flags);
 	}
@@ -831,10 +903,12 @@ int input_set_keycode(struct input_dev *dev,
 	if (test_bit(EV_KEY, dev->evbit) &&
 	    !is_event_supported(old_keycode, dev->keybit, KEY_MAX) &&
 	    __test_and_clear_bit(old_keycode, dev->key)) {
+		struct input_value vals[] =  {
+			{ EV_KEY, old_keycode, 0 },
+			input_value_sync
+		};
 
-		input_pass_event(dev, EV_KEY, old_keycode, 0);
-		if (dev->sync)
-			input_pass_event(dev, EV_SYN, SYN_REPORT, 1);
+		input_pass_values(dev, vals, ARRAY_SIZE(vals));
 	}
 
  out:
@@ -1416,6 +1490,7 @@ static void input_dev_release(struct device *device)
 	input_ff_destroy(dev);
 	input_mt_destroy_slots(dev);
 	kfree(dev->absinfo);
+	kfree(dev->vals);
 	kfree(dev);
 
 	module_put(THIS_MODULE);
@@ -1751,8 +1826,8 @@ static unsigned int input_estimate_events_per_packet(struct input_dev *dev)
 	int i;
 	unsigned int events;
 
-	if (dev->mtsize) {
-		mt_slots = dev->mtsize;
+	if (dev->mt) {
+		mt_slots = dev->mt->num_slots;
 	} else if (test_bit(ABS_MT_TRACKING_ID, dev->absbit)) {
 		mt_slots = dev->absinfo[ABS_MT_TRACKING_ID].maximum -
 			   dev->absinfo[ABS_MT_TRACKING_ID].minimum + 1,
@@ -1778,6 +1853,9 @@ static unsigned int input_estimate_events_per_packet(struct input_dev *dev)
 		if (test_bit(i, dev->relbit))
 			events++;
 
+	/* Make room for KEY and MSC events */
+	events += 7;
+
 	return events;
 }
 
@@ -1816,6 +1894,7 @@ int input_register_device(struct input_dev *dev)
 {
 	static atomic_t input_no = ATOMIC_INIT(0);
 	struct input_handler *handler;
+	unsigned int packet_size;
 	const char *path;
 	int error;
 
@@ -1828,9 +1907,14 @@ int input_register_device(struct input_dev *dev)
 	/* Make sure that bitmasks not mentioned in dev->evbit are clean. */
 	input_cleanse_bitmasks(dev);
 
-	if (!dev->hint_events_per_packet)
-		dev->hint_events_per_packet =
-				input_estimate_events_per_packet(dev);
+	packet_size = input_estimate_events_per_packet(dev);
+	if (dev->hint_events_per_packet < packet_size)
+		dev->hint_events_per_packet = packet_size;
+
+	dev->max_vals = max(dev->hint_events_per_packet, packet_size) + 2;
+	dev->vals = kcalloc(dev->max_vals, sizeof(*dev->vals), GFP_KERNEL);
+	if (!dev->vals)
+		return -ENOMEM;
 
 	/*
 	 * If delay and period are pre-set by the driver, then autorepeating
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index ce68e361558c..cdc252612c0b 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -516,9 +516,9 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 	input_set_drvdata(input_dev, keypad);
 
 	/* Ensure that the keypad will stay dormant until opened */
-	clk_enable(keypad->clk);
+	clk_prepare_enable(keypad->clk);
 	imx_keypad_inhibit(keypad);
-	clk_disable(keypad->clk);
+	clk_disable_unprepare(keypad->clk);
 
 	error = request_irq(irq, imx_keypad_irq_handler, 0,
 			    pdev->name, keypad);
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index f06231b7cab1..84ec691c05aa 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -74,8 +74,8 @@ static int __devinit ab8500_ponkey_probe(struct platform_device *pdev)
 
 	ponkey->idev = input;
 	ponkey->ab8500 = ab8500;
-	ponkey->irq_dbf = ab8500_irq_get_virq(ab8500, irq_dbf);
-	ponkey->irq_dbr = ab8500_irq_get_virq(ab8500, irq_dbr);
+	ponkey->irq_dbf = irq_dbf;
+	ponkey->irq_dbr = irq_dbr;
 
 	input->name = "AB8500 POn(PowerOn) Key";
 	input->dev.parent = &pdev->dev;
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 736056897e50..6b1797503e34 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -405,7 +405,7 @@ static int uinput_setup_device(struct uinput_device *udev, const char __user *bu
 			goto exit;
 		if (test_bit(ABS_MT_SLOT, dev->absbit)) {
 			int nslot = input_abs_get_max(dev, ABS_MT_SLOT) + 1;
-			input_mt_init_slots(dev, nslot);
+			input_mt_init_slots(dev, nslot, 0);
 		} else if (test_bit(ABS_MT_POSITION_X, dev->absbit)) {
 			input_set_events_per_packet(dev, 60);
 		}
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 4a1347e91bdc..cf5af1f495ec 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -1620,7 +1620,7 @@ int alps_init(struct psmouse *psmouse)
 	case ALPS_PROTO_V3:
 	case ALPS_PROTO_V4:
 		set_bit(INPUT_PROP_SEMI_MT, dev1->propbit);
-		input_mt_init_slots(dev1, 2);
+		input_mt_init_slots(dev1, 2, 0);
 		input_set_abs_params(dev1, ABS_MT_POSITION_X, 0, ALPS_V3_X_MAX, 0, 0);
 		input_set_abs_params(dev1, ABS_MT_POSITION_Y, 0, ALPS_V3_Y_MAX, 0, 0);
 
diff --git a/drivers/input/mouse/bcm5974.c b/drivers/input/mouse/bcm5974.c
index d528c23e194f..3a78f235fa3e 100644
--- a/drivers/input/mouse/bcm5974.c
+++ b/drivers/input/mouse/bcm5974.c
@@ -40,6 +40,7 @@
 #include <linux/usb/input.h>
 #include <linux/hid.h>
 #include <linux/mutex.h>
+#include <linux/input/mt.h>
 
 #define USB_VENDOR_ID_APPLE		0x05ac
 
@@ -183,26 +184,26 @@ struct tp_finger {
 	__le16 abs_y;		/* absolute y coodinate */
 	__le16 rel_x;		/* relative x coodinate */
 	__le16 rel_y;		/* relative y coodinate */
-	__le16 size_major;	/* finger size, major axis? */
-	__le16 size_minor;	/* finger size, minor axis? */
+	__le16 tool_major;	/* tool area, major axis */
+	__le16 tool_minor;	/* tool area, minor axis */
 	__le16 orientation;	/* 16384 when point, else 15 bit angle */
-	__le16 force_major;	/* trackpad force, major axis? */
-	__le16 force_minor;	/* trackpad force, minor axis? */
+	__le16 touch_major;	/* touch area, major axis */
+	__le16 touch_minor;	/* touch area, minor axis */
 	__le16 unused[3];	/* zeros */
 	__le16 multi;		/* one finger: varies, more fingers: constant */
 } __attribute__((packed,aligned(2)));
 
 /* trackpad finger data size, empirically at least ten fingers */
+#define MAX_FINGERS		16
 #define SIZEOF_FINGER		sizeof(struct tp_finger)
-#define SIZEOF_ALL_FINGERS	(16 * SIZEOF_FINGER)
+#define SIZEOF_ALL_FINGERS	(MAX_FINGERS * SIZEOF_FINGER)
 #define MAX_FINGER_ORIENTATION	16384
 
 /* device-specific parameters */
 struct bcm5974_param {
-	int dim;		/* logical dimension */
-	int fuzz;		/* logical noise value */
-	int devmin;		/* device minimum reading */
-	int devmax;		/* device maximum reading */
+	int snratio;		/* signal-to-noise ratio */
+	int min;		/* device minimum reading */
+	int max;		/* device maximum reading */
 };
 
 /* device-specific configuration */
@@ -219,6 +220,7 @@ struct bcm5974_config {
 	struct bcm5974_param w;	/* finger width limits */
 	struct bcm5974_param x;	/* horizontal limits */
 	struct bcm5974_param y;	/* vertical limits */
+	struct bcm5974_param o;	/* orientation limits */
 };
 
 /* logical device structure */
@@ -234,23 +236,16 @@ struct bcm5974 {
 	struct bt_data *bt_data;	/* button transferred data */
 	struct urb *tp_urb;		/* trackpad usb request block */
 	u8 *tp_data;			/* trackpad transferred data */
-	int fingers;			/* number of fingers on trackpad */
+	const struct tp_finger *index[MAX_FINGERS];	/* finger index data */
+	struct input_mt_pos pos[MAX_FINGERS];		/* position array */
+	int slots[MAX_FINGERS];				/* slot assignments */
 };
 
-/* logical dimensions */
-#define DIM_PRESSURE	256		/* maximum finger pressure */
-#define DIM_WIDTH	16		/* maximum finger width */
-#define DIM_X		1280		/* maximum trackpad x value */
-#define DIM_Y		800		/* maximum trackpad y value */
-
 /* logical signal quality */
 #define SN_PRESSURE	45		/* pressure signal-to-noise ratio */
-#define SN_WIDTH	100		/* width signal-to-noise ratio */
+#define SN_WIDTH	25		/* width signal-to-noise ratio */
 #define SN_COORD	250		/* coordinate signal-to-noise ratio */
-
-/* pressure thresholds */
-#define PRESSURE_LOW	(2 * DIM_PRESSURE / SN_PRESSURE)
-#define PRESSURE_HIGH	(3 * PRESSURE_LOW)
+#define SN_ORIENT	10		/* orientation signal-to-noise ratio */
 
 /* device constants */
 static const struct bcm5974_config bcm5974_config_table[] = {
@@ -261,10 +256,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		0,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE1, FINGER_TYPE1, FINGER_TYPE1 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 256 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4824, 5342 },
-		{ DIM_Y, DIM_Y / SN_COORD, -172, 5820 }
+		{ SN_PRESSURE, 0, 256 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4824, 5342 },
+		{ SN_COORD, -172, 5820 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING2_ANSI,
@@ -273,10 +269,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		0,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE1, FINGER_TYPE1, FINGER_TYPE1 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 256 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4824, 4824 },
-		{ DIM_Y, DIM_Y / SN_COORD, -172, 4290 }
+		{ SN_PRESSURE, 0, 256 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4824, 4824 },
+		{ SN_COORD, -172, 4290 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING3_ANSI,
@@ -285,10 +282,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4460, 5166 },
-		{ DIM_Y, DIM_Y / SN_COORD, -75, 6700 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4460, 5166 },
+		{ SN_COORD, -75, 6700 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING4_ANSI,
@@ -297,10 +295,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4620, 5140 },
-		{ DIM_Y, DIM_Y / SN_COORD, -150, 6600 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4620, 5140 },
+		{ SN_COORD, -150, 6600 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING4A_ANSI,
@@ -309,10 +308,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4616, 5112 },
-		{ DIM_Y, DIM_Y / SN_COORD, -142, 5234 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4616, 5112 },
+		{ SN_COORD, -142, 5234 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING5_ANSI,
@@ -321,10 +321,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4415, 5050 },
-		{ DIM_Y, DIM_Y / SN_COORD, -55, 6680 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4415, 5050 },
+		{ SN_COORD, -55, 6680 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING6_ANSI,
@@ -333,10 +334,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4620, 5140 },
-		{ DIM_Y, DIM_Y / SN_COORD, -150, 6600 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4620, 5140 },
+		{ SN_COORD, -150, 6600 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI,
@@ -345,10 +347,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4750, 5280 },
-		{ DIM_Y, DIM_Y / SN_COORD, -150, 6730 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4750, 5280 },
+		{ SN_COORD, -150, 6730 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING6A_ANSI,
@@ -357,10 +360,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4620, 5140 },
-		{ DIM_Y, DIM_Y / SN_COORD, -150, 6600 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4620, 5140 },
+		{ SN_COORD, -150, 6600 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{
 		USB_DEVICE_ID_APPLE_WELLSPRING7_ANSI,
@@ -369,10 +373,11 @@ static const struct bcm5974_config bcm5974_config_table[] = {
 		HAS_INTEGRATED_BUTTON,
 		0x84, sizeof(struct bt_data),
 		0x81, TYPE2, FINGER_TYPE2, FINGER_TYPE2 + SIZEOF_ALL_FINGERS,
-		{ DIM_PRESSURE, DIM_PRESSURE / SN_PRESSURE, 0, 300 },
-		{ DIM_WIDTH, DIM_WIDTH / SN_WIDTH, 0, 2048 },
-		{ DIM_X, DIM_X / SN_COORD, -4750, 5280 },
-		{ DIM_Y, DIM_Y / SN_COORD, -150, 6730 }
+		{ SN_PRESSURE, 0, 300 },
+		{ SN_WIDTH, 0, 2048 },
+		{ SN_COORD, -4750, 5280 },
+		{ SN_COORD, -150, 6730 },
+		{ SN_ORIENT, -MAX_FINGER_ORIENTATION, MAX_FINGER_ORIENTATION }
 	},
 	{}
 };
@@ -396,18 +401,11 @@ static inline int raw2int(__le16 x)
 	return (signed short)le16_to_cpu(x);
 }
 
-/* scale device data to logical dimensions (asserts devmin < devmax) */
-static inline int int2scale(const struct bcm5974_param *p, int x)
-{
-	return x * p->dim / (p->devmax - p->devmin);
-}
-
-/* all logical value ranges are [0,dim). */
-static inline int int2bound(const struct bcm5974_param *p, int x)
+static void set_abs(struct input_dev *input, unsigned int code,
+		    const struct bcm5974_param *p)
 {
-	int s = int2scale(p, x);
-
-	return clamp_val(s, 0, p->dim - 1);
+	int fuzz = p->snratio ? (p->max - p->min) / p->snratio : 0;
+	input_set_abs_params(input, code, p->min, p->max, fuzz, 0);
 }
 
 /* setup which logical events to report */
@@ -416,48 +414,30 @@ static void setup_events_to_report(struct input_dev *input_dev,
 {
 	__set_bit(EV_ABS, input_dev->evbit);
 
-	input_set_abs_params(input_dev, ABS_PRESSURE,
-				0, cfg->p.dim, cfg->p.fuzz, 0);
-	input_set_abs_params(input_dev, ABS_TOOL_WIDTH,
-				0, cfg->w.dim, cfg->w.fuzz, 0);
-	input_set_abs_params(input_dev, ABS_X,
-				0, cfg->x.dim, cfg->x.fuzz, 0);
-	input_set_abs_params(input_dev, ABS_Y,
-				0, cfg->y.dim, cfg->y.fuzz, 0);
+	/* for synaptics only */
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, 256, 5, 0);
+	input_set_abs_params(input_dev, ABS_TOOL_WIDTH, 0, 16, 0, 0);
 
 	/* finger touch area */
-	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
-			     cfg->w.devmin, cfg->w.devmax, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR,
-			     cfg->w.devmin, cfg->w.devmax, 0, 0);
+	set_abs(input_dev, ABS_MT_TOUCH_MAJOR, &cfg->w);
+	set_abs(input_dev, ABS_MT_TOUCH_MINOR, &cfg->w);
 	/* finger approach area */
-	input_set_abs_params(input_dev, ABS_MT_WIDTH_MAJOR,
-			     cfg->w.devmin, cfg->w.devmax, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_WIDTH_MINOR,
-			     cfg->w.devmin, cfg->w.devmax, 0, 0);
+	set_abs(input_dev, ABS_MT_WIDTH_MAJOR, &cfg->w);
+	set_abs(input_dev, ABS_MT_WIDTH_MINOR, &cfg->w);
 	/* finger orientation */
-	input_set_abs_params(input_dev, ABS_MT_ORIENTATION,
-			     -MAX_FINGER_ORIENTATION,
-			     MAX_FINGER_ORIENTATION, 0, 0);
+	set_abs(input_dev, ABS_MT_ORIENTATION, &cfg->o);
 	/* finger position */
-	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
-			     cfg->x.devmin, cfg->x.devmax, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
-			     cfg->y.devmin, cfg->y.devmax, 0, 0);
+	set_abs(input_dev, ABS_MT_POSITION_X, &cfg->x);
+	set_abs(input_dev, ABS_MT_POSITION_Y, &cfg->y);
 
 	__set_bit(EV_KEY, input_dev->evbit);
-	__set_bit(BTN_TOUCH, input_dev->keybit);
-	__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
-	__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
-	__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
-	__set_bit(BTN_TOOL_QUADTAP, input_dev->keybit);
 	__set_bit(BTN_LEFT, input_dev->keybit);
 
-	__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 	if (cfg->caps & HAS_INTEGRATED_BUTTON)
 		__set_bit(INPUT_PROP_BUTTONPAD, input_dev->propbit);
 
-	input_set_events_per_packet(input_dev, 60);
+	input_mt_init_slots(input_dev, MAX_FINGERS,
+		INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED | INPUT_MT_TRACK);
 }
 
 /* report button data as logical button state */
@@ -477,24 +457,44 @@ static int report_bt_state(struct bcm5974 *dev, int size)
 	return 0;
 }
 
-static void report_finger_data(struct input_dev *input,
-			       const struct bcm5974_config *cfg,
+static void report_finger_data(struct input_dev *input, int slot,
+			       const struct input_mt_pos *pos,
 			       const struct tp_finger *f)
 {
+	input_mt_slot(input, slot);
+	input_mt_report_slot_state(input, MT_TOOL_FINGER, true);
+
 	input_report_abs(input, ABS_MT_TOUCH_MAJOR,
-			 raw2int(f->force_major) << 1);
+			 raw2int(f->touch_major) << 1);
 	input_report_abs(input, ABS_MT_TOUCH_MINOR,
-			 raw2int(f->force_minor) << 1);
+			 raw2int(f->touch_minor) << 1);
 	input_report_abs(input, ABS_MT_WIDTH_MAJOR,
-			 raw2int(f->size_major) << 1);
+			 raw2int(f->tool_major) << 1);
 	input_report_abs(input, ABS_MT_WIDTH_MINOR,
-			 raw2int(f->size_minor) << 1);
+			 raw2int(f->tool_minor) << 1);
 	input_report_abs(input, ABS_MT_ORIENTATION,
 			 MAX_FINGER_ORIENTATION - raw2int(f->orientation));
-	input_report_abs(input, ABS_MT_POSITION_X, raw2int(f->abs_x));
-	input_report_abs(input, ABS_MT_POSITION_Y,
-			 cfg->y.devmin + cfg->y.devmax - raw2int(f->abs_y));
-	input_mt_sync(input);
+	input_report_abs(input, ABS_MT_POSITION_X, pos->x);
+	input_report_abs(input, ABS_MT_POSITION_Y, pos->y);
+}
+
+static void report_synaptics_data(struct input_dev *input,
+				  const struct bcm5974_config *cfg,
+				  const struct tp_finger *f, int raw_n)
+{
+	int abs_p = 0, abs_w = 0;
+
+	if (raw_n) {
+		int p = raw2int(f->touch_major);
+		int w = raw2int(f->tool_major);
+		if (p > 0 && raw2int(f->origin)) {
+			abs_p = clamp_val(256 * p / cfg->p.max, 0, 255);
+			abs_w = clamp_val(16 * w / cfg->w.max, 0, 15);
+		}
+	}
+
+	input_report_abs(input, ABS_PRESSURE, abs_p);
+	input_report_abs(input, ABS_TOOL_WIDTH, abs_w);
 }
 
 /* report trackpad data as logical trackpad state */
@@ -503,9 +503,7 @@ static int report_tp_state(struct bcm5974 *dev, int size)
 	const struct bcm5974_config *c = &dev->cfg;
 	const struct tp_finger *f;
 	struct input_dev *input = dev->input;
-	int raw_p, raw_w, raw_x, raw_y, raw_n, i;
-	int ptest, origin, ibt = 0, nmin = 0, nmax = 0;
-	int abs_p = 0, abs_w = 0, abs_x = 0, abs_y = 0;
+	int raw_n, i, n = 0;
 
 	if (size < c->tp_offset || (size - c->tp_offset) % SIZEOF_FINGER != 0)
 		return -EIO;
@@ -514,76 +512,29 @@ static int report_tp_state(struct bcm5974 *dev, int size)
 	f = (const struct tp_finger *)(dev->tp_data + c->tp_offset);
 	raw_n = (size - c->tp_offset) / SIZEOF_FINGER;
 
-	/* always track the first finger; when detached, start over */
-	if (raw_n) {
-
-		/* report raw trackpad data */
-		for (i = 0; i < raw_n; i++)
-			report_finger_data(input, c, &f[i]);
-
-		raw_p = raw2int(f->force_major);
-		raw_w = raw2int(f->size_major);
-		raw_x = raw2int(f->abs_x);
-		raw_y = raw2int(f->abs_y);
-
-		dprintk(9,
-			"bcm5974: "
-			"raw: p: %+05d w: %+05d x: %+05d y: %+05d n: %d\n",
-			raw_p, raw_w, raw_x, raw_y, raw_n);
-
-		ptest = int2bound(&c->p, raw_p);
-		origin = raw2int(f->origin);
-
-		/* while tracking finger still valid, count all fingers */
-		if (ptest > PRESSURE_LOW && origin) {
-			abs_p = ptest;
-			abs_w = int2bound(&c->w, raw_w);
-			abs_x = int2bound(&c->x, raw_x - c->x.devmin);
-			abs_y = int2bound(&c->y, c->y.devmax - raw_y);
-			while (raw_n--) {
-				ptest = int2bound(&c->p,
-						  raw2int(f->force_major));
-				if (ptest > PRESSURE_LOW)
-					nmax++;
-				if (ptest > PRESSURE_HIGH)
-					nmin++;
-				f++;
-			}
-		}
+	for (i = 0; i < raw_n; i++) {
+		if (raw2int(f[i].touch_major) == 0)
+			continue;
+		dev->pos[n].x = raw2int(f[i].abs_x);
+		dev->pos[n].y = c->y.min + c->y.max - raw2int(f[i].abs_y);
+		dev->index[n++] = &f[i];
 	}
 
-	/* set the integrated button if applicable */
-	if (c->tp_type == TYPE2)
-		ibt = raw2int(dev->tp_data[BUTTON_TYPE2]);
-
-	if (dev->fingers < nmin)
-		dev->fingers = nmin;
-	if (dev->fingers > nmax)
-		dev->fingers = nmax;
-
-	input_report_key(input, BTN_TOUCH, dev->fingers > 0);
-	input_report_key(input, BTN_TOOL_FINGER, dev->fingers == 1);
-	input_report_key(input, BTN_TOOL_DOUBLETAP, dev->fingers == 2);
-	input_report_key(input, BTN_TOOL_TRIPLETAP, dev->fingers == 3);
-	input_report_key(input, BTN_TOOL_QUADTAP, dev->fingers > 3);
-
-	input_report_abs(input, ABS_PRESSURE, abs_p);
-	input_report_abs(input, ABS_TOOL_WIDTH, abs_w);
+	input_mt_assign_slots(input, dev->slots, dev->pos, n);
 
-	if (abs_p) {
-		input_report_abs(input, ABS_X, abs_x);
-		input_report_abs(input, ABS_Y, abs_y);
+	for (i = 0; i < n; i++)
+		report_finger_data(input, dev->slots[i],
+				   &dev->pos[i], dev->index[i]);
 
-		dprintk(8,
-			"bcm5974: abs: p: %+05d w: %+05d x: %+05d y: %+05d "
-			"nmin: %d nmax: %d n: %d ibt: %d\n", abs_p, abs_w,
-			abs_x, abs_y, nmin, nmax, dev->fingers, ibt);
+	input_mt_sync_frame(input);
 
-	}
+	report_synaptics_data(input, c, f, raw_n);
 
 	/* type 2 reports button events via ibt only */
-	if (c->tp_type == TYPE2)
+	if (c->tp_type == TYPE2) {
+		int ibt = raw2int(dev->tp_data[BUTTON_TYPE2]);
 		input_report_key(input, BTN_LEFT, ibt);
+	}
 
 	input_sync(input);
 
@@ -742,9 +693,11 @@ static int bcm5974_start_traffic(struct bcm5974 *dev)
 		goto err_out;
 	}
 
-	error = usb_submit_urb(dev->bt_urb, GFP_KERNEL);
-	if (error)
-		goto err_reset_mode;
+	if (dev->bt_urb) {
+		error = usb_submit_urb(dev->bt_urb, GFP_KERNEL);
+		if (error)
+			goto err_reset_mode;
+	}
 
 	error = usb_submit_urb(dev->tp_urb, GFP_KERNEL);
 	if (error)
@@ -868,19 +821,23 @@ static int bcm5974_probe(struct usb_interface *iface,
 	mutex_init(&dev->pm_mutex);
 
 	/* setup urbs */
-	dev->bt_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!dev->bt_urb)
-		goto err_free_devs;
+	if (cfg->tp_type == TYPE1) {
+		dev->bt_urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!dev->bt_urb)
+			goto err_free_devs;
+	}
 
 	dev->tp_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->tp_urb)
 		goto err_free_bt_urb;
 
-	dev->bt_data = usb_alloc_coherent(dev->udev,
+	if (dev->bt_urb) {
+		dev->bt_data = usb_alloc_coherent(dev->udev,
 					  dev->cfg.bt_datalen, GFP_KERNEL,
 					  &dev->bt_urb->transfer_dma);
-	if (!dev->bt_data)
-		goto err_free_urb;
+		if (!dev->bt_data)
+			goto err_free_urb;
+	}
 
 	dev->tp_data = usb_alloc_coherent(dev->udev,
 					  dev->cfg.tp_datalen, GFP_KERNEL,
@@ -888,10 +845,11 @@ static int bcm5974_probe(struct usb_interface *iface,
 	if (!dev->tp_data)
 		goto err_free_bt_buffer;
 
-	usb_fill_int_urb(dev->bt_urb, udev,
-			 usb_rcvintpipe(udev, cfg->bt_ep),
-			 dev->bt_data, dev->cfg.bt_datalen,
-			 bcm5974_irq_button, dev, 1);
+	if (dev->bt_urb)
+		usb_fill_int_urb(dev->bt_urb, udev,
+				 usb_rcvintpipe(udev, cfg->bt_ep),
+				 dev->bt_data, dev->cfg.bt_datalen,
+				 bcm5974_irq_button, dev, 1);
 
 	usb_fill_int_urb(dev->tp_urb, udev,
 			 usb_rcvintpipe(udev, cfg->tp_ep),
@@ -929,8 +887,9 @@ err_free_buffer:
 	usb_free_coherent(dev->udev, dev->cfg.tp_datalen,
 		dev->tp_data, dev->tp_urb->transfer_dma);
 err_free_bt_buffer:
-	usb_free_coherent(dev->udev, dev->cfg.bt_datalen,
-		dev->bt_data, dev->bt_urb->transfer_dma);
+	if (dev->bt_urb)
+		usb_free_coherent(dev->udev, dev->cfg.bt_datalen,
+				  dev->bt_data, dev->bt_urb->transfer_dma);
 err_free_urb:
 	usb_free_urb(dev->tp_urb);
 err_free_bt_urb:
@@ -951,8 +910,9 @@ static void bcm5974_disconnect(struct usb_interface *iface)
 	input_unregister_device(dev->input);
 	usb_free_coherent(dev->udev, dev->cfg.tp_datalen,
 			  dev->tp_data, dev->tp_urb->transfer_dma);
-	usb_free_coherent(dev->udev, dev->cfg.bt_datalen,
-			  dev->bt_data, dev->bt_urb->transfer_dma);
+	if (dev->bt_urb)
+		usb_free_coherent(dev->udev, dev->cfg.bt_datalen,
+				  dev->bt_data, dev->bt_urb->transfer_dma);
 	usb_free_urb(dev->tp_urb);
 	usb_free_urb(dev->bt_urb);
 	kfree(dev);
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index 479011004a11..1e8e42fb03a4 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -1004,7 +1004,7 @@ static int elantech_set_input_params(struct psmouse *psmouse)
 			input_set_abs_params(dev, ABS_TOOL_WIDTH, ETP_WMIN_V2,
 					     ETP_WMAX_V2, 0, 0);
 		}
-		input_mt_init_slots(dev, 2);
+		input_mt_init_slots(dev, 2, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_X, x_min, x_max, 0, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_Y, y_min, y_max, 0, 0);
 		break;
@@ -1035,7 +1035,7 @@ static int elantech_set_input_params(struct psmouse *psmouse)
 		input_set_abs_params(dev, ABS_TOOL_WIDTH, ETP_WMIN_V2,
 				     ETP_WMAX_V2, 0, 0);
 		/* Multitouch capable pad, up to 5 fingers. */
-		input_mt_init_slots(dev, ETP_MAX_FINGERS);
+		input_mt_init_slots(dev, ETP_MAX_FINGERS, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_X, x_min, x_max, 0, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_Y, y_min, y_max, 0, 0);
 		input_abs_set_res(dev, ABS_MT_POSITION_X, x_res);
diff --git a/drivers/input/mouse/sentelic.c b/drivers/input/mouse/sentelic.c
index 3f5649f19082..e582922bacf7 100644
--- a/drivers/input/mouse/sentelic.c
+++ b/drivers/input/mouse/sentelic.c
@@ -721,6 +721,17 @@ static psmouse_ret_t fsp_process_byte(struct psmouse *psmouse)
 
 	switch (psmouse->packet[0] >> FSP_PKT_TYPE_SHIFT) {
 	case FSP_PKT_TYPE_ABS:
+
+		if ((packet[0] == 0x48 || packet[0] == 0x49) &&
+		    packet[1] == 0 && packet[2] == 0) {
+			/*
+			 * Ignore coordinate noise when finger leaving the
+			 * surface, otherwise cursor may jump to upper-left
+			 * corner.
+			 */
+			packet[3] &= 0xf0;
+		}
+
 		abs_x = GET_ABS_X(packet);
 		abs_y = GET_ABS_Y(packet);
 
@@ -960,7 +971,7 @@ static int fsp_set_input_params(struct psmouse *psmouse)
 
 		input_set_abs_params(dev, ABS_X, 0, abs_x, 0, 0);
 		input_set_abs_params(dev, ABS_Y, 0, abs_y, 0, 0);
-		input_mt_init_slots(dev, 2);
+		input_mt_init_slots(dev, 2, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_X, 0, abs_x, 0, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_Y, 0, abs_y, 0, 0);
 	}
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index 14eaecea2b70..37033ade79d3 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -1232,7 +1232,7 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	input_set_abs_params(dev, ABS_PRESSURE, 0, 255, 0, 0);
 
 	if (SYN_CAP_IMAGE_SENSOR(priv->ext_cap_0c)) {
-		input_mt_init_slots(dev, 2);
+		input_mt_init_slots(dev, 2, 0);
 		set_abs_position_params(dev, priv, ABS_MT_POSITION_X,
 					ABS_MT_POSITION_Y);
 		/* Image sensors can report per-contact pressure */
@@ -1244,7 +1244,7 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	} else if (SYN_CAP_ADV_GESTURE(priv->ext_cap_0c)) {
 		/* Non-image sensors with AGM use semi-mt */
 		__set_bit(INPUT_PROP_SEMI_MT, dev->propbit);
-		input_mt_init_slots(dev, 2);
+		input_mt_init_slots(dev, 2, 0);
 		set_abs_position_params(dev, priv, ABS_MT_POSITION_X,
 					ABS_MT_POSITION_Y);
 	}
diff --git a/drivers/input/serio/ambakmi.c b/drivers/input/serio/ambakmi.c
index 2ffd110bd5bc..2e77246c2e5a 100644
--- a/drivers/input/serio/ambakmi.c
+++ b/drivers/input/serio/ambakmi.c
@@ -72,7 +72,7 @@ static int amba_kmi_open(struct serio *io)
 	unsigned int divisor;
 	int ret;
 
-	ret = clk_enable(kmi->clk);
+	ret = clk_prepare_enable(kmi->clk);
 	if (ret)
 		goto out;
 
@@ -92,7 +92,7 @@ static int amba_kmi_open(struct serio *io)
 	return 0;
 
  clk_disable:
-	clk_disable(kmi->clk);
+	clk_disable_unprepare(kmi->clk);
  out:
 	return ret;
 }
@@ -104,7 +104,7 @@ static void amba_kmi_close(struct serio *io)
 	writeb(0, KMICR);
 
 	free_irq(kmi->irq, kmi);
-	clk_disable(kmi->clk);
+	clk_disable_unprepare(kmi->clk);
 }
 
 static int __devinit amba_kmi_probe(struct amba_device *dev,
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 6918773ce024..d6cc77a53c7e 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -335,6 +335,12 @@ static const struct dmi_system_id __initconst i8042_dmi_nomux_table[] = {
 	},
 	{
 		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "SATELLITE C850D"),
+		},
+	},
+	{
+		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ALIENWARE"),
 			DMI_MATCH(DMI_PRODUCT_NAME, "Sentia"),
 		},
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 532d067a9e07..2a81ce375f75 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -1530,7 +1530,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 			__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
 			__set_bit(BTN_TOOL_QUADTAP, input_dev->keybit);
 
-			input_mt_init_slots(input_dev, features->touch_max);
+			input_mt_init_slots(input_dev, features->touch_max, 0);
 
 			input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
 			                     0, 255, 0, 0);
@@ -1575,7 +1575,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	case TABLETPC2FG:
 		if (features->device_type == BTN_TOOL_FINGER) {
-			input_mt_init_slots(input_dev, features->touch_max);
+			input_mt_init_slots(input_dev, features->touch_max, 0);
 			input_set_abs_params(input_dev, ABS_MT_TOOL_TYPE,
 					0, MT_TOOL_MAX, 0, 0);
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1631,7 +1631,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
 			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
-			input_mt_init_slots(input_dev, features->touch_max);
+			input_mt_init_slots(input_dev, features->touch_max, 0);
 
 			if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
 				__set_bit(BTN_TOOL_TRIPLETAP,
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 4623cc69fc60..e92615d0b1b0 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -1152,7 +1152,7 @@ static int __devinit mxt_probe(struct i2c_client *client,
 
 	/* For multi touch */
 	num_mt_slots = data->T9_reportid_max - data->T9_reportid_min + 1;
-	error = input_mt_init_slots(input_dev, num_mt_slots);
+	error = input_mt_init_slots(input_dev, num_mt_slots, 0);
 	if (error)
 		goto err_free_object;
 	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
diff --git a/drivers/input/touchscreen/cyttsp_core.c b/drivers/input/touchscreen/cyttsp_core.c
index f030d9ec795d..8e60437ac85b 100644
--- a/drivers/input/touchscreen/cyttsp_core.c
+++ b/drivers/input/touchscreen/cyttsp_core.c
@@ -571,7 +571,7 @@ struct cyttsp *cyttsp_probe(const struct cyttsp_bus_ops *bus_ops,
 	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
 			     0, CY_MAXZ, 0, 0);
 
-	input_mt_init_slots(input_dev, CY_MAX_ID);
+	input_mt_init_slots(input_dev, CY_MAX_ID, 0);
 
 	error = request_threaded_irq(ts->irq, NULL, cyttsp_irq,
 				     IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index b06a5e3a665e..099d144ab7c9 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -566,9 +566,12 @@ static ssize_t edt_ft5x06_debugfs_raw_data_read(struct file *file,
 	}
 
 	read = min_t(size_t, count, tsdata->raw_bufsize - *off);
-	error = copy_to_user(buf, tsdata->raw_buffer + *off, read);
-	if (!error)
-		*off += read;
+	if (copy_to_user(buf, tsdata->raw_buffer + *off, read)) {
+		error = -EFAULT;
+		goto out;
+	}
+
+	*off += read;
 out:
 	mutex_unlock(&tsdata->mutex);
 	return error ?: read;
@@ -779,7 +782,7 @@ static int __devinit 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);
-	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS);
+	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS, 0);
 	if (error) {
 		dev_err(&client->dev, "Unable to init MT slots.\n");
 		goto err_free_mem;
diff --git a/drivers/input/touchscreen/egalax_ts.c b/drivers/input/touchscreen/egalax_ts.c
index 70524dd34f42..c1e3460f1195 100644
--- a/drivers/input/touchscreen/egalax_ts.c
+++ b/drivers/input/touchscreen/egalax_ts.c
@@ -204,7 +204,7 @@ static int __devinit egalax_ts_probe(struct i2c_client *client,
 			     ABS_MT_POSITION_X, 0, EGALAX_MAX_X, 0, 0);
 	input_set_abs_params(input_dev,
 			     ABS_MT_POSITION_X, 0, EGALAX_MAX_Y, 0, 0);
-	input_mt_init_slots(input_dev, MAX_SUPPORT_POINTS);
+	input_mt_init_slots(input_dev, MAX_SUPPORT_POINTS, 0);
 
 	input_set_drvdata(input_dev, ts);
 
diff --git a/drivers/input/touchscreen/ili210x.c b/drivers/input/touchscreen/ili210x.c
index c0044175a921..4ac69760ec08 100644
--- a/drivers/input/touchscreen/ili210x.c
+++ b/drivers/input/touchscreen/ili210x.c
@@ -252,7 +252,7 @@ static int __devinit ili210x_i2c_probe(struct i2c_client *client,
 	input_set_abs_params(input, ABS_Y, 0, ymax, 0, 0);
 
 	/* Multi touch */
-	input_mt_init_slots(input, MAX_TOUCHES);
+	input_mt_init_slots(input, MAX_TOUCHES, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_X, 0, xmax, 0, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_Y, 0, ymax, 0, 0);
 
diff --git a/drivers/input/touchscreen/mms114.c b/drivers/input/touchscreen/mms114.c
index 49c44bbf548d..560cf09d1c5a 100644
--- a/drivers/input/touchscreen/mms114.c
+++ b/drivers/input/touchscreen/mms114.c
@@ -404,7 +404,7 @@ static int __devinit mms114_probe(struct i2c_client *client,
 	input_set_abs_params(input_dev, ABS_Y, 0, data->pdata->y_size, 0, 0);
 
 	/* For multi touch */
-	input_mt_init_slots(input_dev, MMS114_MAX_TOUCH);
+	input_mt_init_slots(input_dev, MMS114_MAX_TOUCH, 0);
 	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
 			     0, MMS114_MAX_AREA, 0, 0);
 	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
diff --git a/drivers/input/touchscreen/penmount.c b/drivers/input/touchscreen/penmount.c
index 4ccde45b9da2..b49f0b836925 100644
--- a/drivers/input/touchscreen/penmount.c
+++ b/drivers/input/touchscreen/penmount.c
@@ -264,7 +264,7 @@ static int pm_connect(struct serio *serio, struct serio_driver *drv)
 	input_set_abs_params(pm->dev, ABS_Y, 0, max_y, 0, 0);
 
 	if (pm->maxcontacts > 1) {
-		input_mt_init_slots(pm->dev, pm->maxcontacts);
+		input_mt_init_slots(pm->dev, pm->maxcontacts, 0);
 		input_set_abs_params(pm->dev,
 				     ABS_MT_POSITION_X, 0, max_x, 0, 0);
 		input_set_abs_params(pm->dev,
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index e32709e0dd65..721fdb3597ca 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -304,6 +304,45 @@ static int e2i_read_data(struct usbtouch_usb *dev, unsigned char *pkt)
 #define EGALAX_PKT_TYPE_REPT		0x80
 #define EGALAX_PKT_TYPE_DIAG		0x0A
 
+static int egalax_init(struct usbtouch_usb *usbtouch)
+{
+	int ret, i;
+	unsigned char *buf;
+	struct usb_device *udev = interface_to_usbdev(usbtouch->interface);
+
+	/*
+	 * An eGalax diagnostic packet kicks the device into using the right
+	 * protocol.  We send a "check active" packet.  The response will be
+	 * read later and ignored.
+	 */
+
+	buf = kmalloc(3, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	buf[0] = EGALAX_PKT_TYPE_DIAG;
+	buf[1] = 1;	/* length */
+	buf[2] = 'A';	/* command - check active */
+
+	for (i = 0; i < 3; i++) {
+		ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				      0,
+				      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+				      0, 0, buf, 3,
+				      USB_CTRL_SET_TIMEOUT);
+		if (ret >= 0) {
+			ret = 0;
+			break;
+		}
+		if (ret != -EPIPE)
+			break;
+	}
+
+	kfree(buf);
+
+	return ret;
+}
+
 static int egalax_read_data(struct usbtouch_usb *dev, unsigned char *pkt)
 {
 	if ((pkt[0] & EGALAX_PKT_TYPE_MASK) != EGALAX_PKT_TYPE_REPT)
@@ -1056,6 +1095,7 @@ static struct usbtouch_device_info usbtouch_dev_info[] = {
 		.process_pkt	= usbtouch_process_multi,
 		.get_pkt_len	= egalax_get_pkt_len,
 		.read_data	= egalax_read_data,
+		.init		= egalax_init,
 	},
 #endif
 
diff --git a/drivers/input/touchscreen/wacom_w8001.c b/drivers/input/touchscreen/wacom_w8001.c
index 8f9ad2f893b8..9a83be6b6584 100644
--- a/drivers/input/touchscreen/wacom_w8001.c
+++ b/drivers/input/touchscreen/wacom_w8001.c
@@ -471,7 +471,7 @@ static int w8001_setup(struct w8001 *w8001)
 		case 5:
 			w8001->pktlen = W8001_PKTLEN_TOUCH2FG;
 
-			input_mt_init_slots(dev, 2);
+			input_mt_init_slots(dev, 2, 0);
 			input_set_abs_params(dev, ABS_MT_POSITION_X,
 						0, touch.x, 0, 0);
 			input_set_abs_params(dev, ABS_MT_POSITION_Y,
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index b64502dfa9f4..e89daf1b21b4 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -266,7 +266,7 @@ static void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
 
 static int iommu_init_device(struct device *dev)
 {
-	struct pci_dev *dma_pdev, *pdev = to_pci_dev(dev);
+	struct pci_dev *dma_pdev = NULL, *pdev = to_pci_dev(dev);
 	struct iommu_dev_data *dev_data;
 	struct iommu_group *group;
 	u16 alias;
@@ -293,7 +293,9 @@ static int iommu_init_device(struct device *dev)
 		dev_data->alias_data = alias_data;
 
 		dma_pdev = pci_get_bus_and_slot(alias >> 8, alias & 0xff);
-	} else
+	}
+
+	if (dma_pdev == NULL)
 		dma_pdev = pci_dev_get(pdev);
 
 	/* Account for quirked devices */
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 2297ec193eb4..db820d7dd0bc 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -2351,7 +2351,7 @@ static int iommu_should_identity_map(struct pci_dev *pdev, int startup)
 			return 0;
 		if (pdev->class >> 8 == PCI_CLASS_BRIDGE_PCI)
 			return 0;
-	} else if (pdev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+	} else if (pci_pcie_type(pdev) == PCI_EXP_TYPE_PCI_BRIDGE)
 		return 0;
 
 	/* 
@@ -3546,10 +3546,10 @@ found:
 		struct pci_dev *bridge = bus->self;
 
 		if (!bridge || !pci_is_pcie(bridge) ||
-		    bridge->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+		    pci_pcie_type(bridge) == PCI_EXP_TYPE_PCI_BRIDGE)
 			return 0;
 
-		if (bridge->pcie_type == PCI_EXP_TYPE_ROOT_PORT) {
+		if (pci_pcie_type(bridge) == PCI_EXP_TYPE_ROOT_PORT) {
 			for (i = 0; i < atsru->devices_cnt; i++)
 				if (atsru->devices[i] == bridge)
 					return 1;
diff --git a/drivers/isdn/capi/capi.c b/drivers/isdn/capi/capi.c
index 38c4bd87b2c9..c679867c2ccd 100644
--- a/drivers/isdn/capi/capi.c
+++ b/drivers/isdn/capi/capi.c
@@ -234,7 +234,8 @@ static struct capiminor *capiminor_alloc(struct capi20_appl *ap, u32 ncci)
 
 	mp->minor = minor;
 
-	dev = tty_register_device(capinc_tty_driver, minor, NULL);
+	dev = tty_port_register_device(&mp->port, capinc_tty_driver, minor,
+			NULL);
 	if (IS_ERR(dev))
 		goto err_out2;
 
diff --git a/drivers/isdn/gigaset/interface.c b/drivers/isdn/gigaset/interface.c
index a6d9fd2858f7..67abf3ff45e8 100644
--- a/drivers/isdn/gigaset/interface.c
+++ b/drivers/isdn/gigaset/interface.c
@@ -446,8 +446,8 @@ static void if_set_termios(struct tty_struct *tty, struct ktermios *old)
 		goto out;
 	}
 
-	iflag = tty->termios->c_iflag;
-	cflag = tty->termios->c_cflag;
+	iflag = tty->termios.c_iflag;
+	cflag = tty->termios.c_cflag;
 	old_cflag = old ? old->c_cflag : cflag;
 	gig_dbg(DEBUG_IF, "%u: iflag %x cflag %x old %x",
 		cs->minor_index, iflag, cflag, old_cflag);
@@ -524,7 +524,8 @@ void gigaset_if_init(struct cardstate *cs)
 	tasklet_init(&cs->if_wake_tasklet, if_wake, (unsigned long) cs);
 
 	mutex_lock(&cs->mutex);
-	cs->tty_dev = tty_register_device(drv->tty, cs->minor_index, NULL);
+	cs->tty_dev = tty_port_register_device(&cs->port, drv->tty,
+			cs->minor_index, NULL);
 
 	if (!IS_ERR(cs->tty_dev))
 		dev_set_drvdata(cs->tty_dev, cs);
diff --git a/drivers/isdn/i4l/isdn_tty.c b/drivers/isdn/i4l/isdn_tty.c
index 7bc50670d7d9..b817809f763c 100644
--- a/drivers/isdn/i4l/isdn_tty.c
+++ b/drivers/isdn/i4l/isdn_tty.c
@@ -1009,15 +1009,15 @@ isdn_tty_change_speed(modem_info *info)
 		quot;
 	int i;
 
-	if (!port->tty || !port->tty->termios)
+	if (!port->tty)
 		return;
-	cflag = port->tty->termios->c_cflag;
+	cflag = port->tty->termios.c_cflag;
 
 	quot = i = cflag & CBAUD;
 	if (i & CBAUDEX) {
 		i &= ~CBAUDEX;
 		if (i < 1 || i > 2)
-			port->tty->termios->c_cflag &= ~CBAUDEX;
+			port->tty->termios.c_cflag &= ~CBAUDEX;
 		else
 			i += 15;
 	}
@@ -1097,7 +1097,7 @@ isdn_tty_shutdown(modem_info *info)
 #endif
 	isdn_unlock_drivers();
 	info->msr &= ~UART_MSR_RI;
-	if (!info->port.tty || (info->port.tty->termios->c_cflag & HUPCL)) {
+	if (!info->port.tty || (info->port.tty->termios.c_cflag & HUPCL)) {
 		info->mcr &= ~(UART_MCR_DTR | UART_MCR_RTS);
 		if (info->emu.mdmreg[REG_DTRHUP] & BIT_DTRHUP) {
 			isdn_tty_modem_reset_regs(info, 0);
@@ -1469,13 +1469,13 @@ isdn_tty_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	if (!old_termios)
 		isdn_tty_change_speed(info);
 	else {
-		if (tty->termios->c_cflag == old_termios->c_cflag &&
-		    tty->termios->c_ispeed == old_termios->c_ispeed &&
-		    tty->termios->c_ospeed == old_termios->c_ospeed)
+		if (tty->termios.c_cflag == old_termios->c_cflag &&
+		    tty->termios.c_ispeed == old_termios->c_ispeed &&
+		    tty->termios.c_ospeed == old_termios->c_ospeed)
 			return;
 		isdn_tty_change_speed(info);
 		if ((old_termios->c_cflag & CRTSCTS) &&
-		    !(tty->termios->c_cflag & CRTSCTS))
+		    !(tty->termios.c_cflag & CRTSCTS))
 			tty->hw_stopped = 0;
 	}
 }
@@ -1486,6 +1486,18 @@ isdn_tty_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
  * ------------------------------------------------------------
  */
 
+static int isdn_tty_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	modem_info *info = &dev->mdm.info[tty->index];
+
+	if (isdn_tty_paranoia_check(info, tty->name, __func__))
+		return -ENODEV;
+
+	tty->driver_data = info;
+
+	return tty_port_install(&info->port, driver, tty);
+}
+
 /*
  * This routine is called whenever a serial port is opened.  It
  * enables interrupts for a serial port, linking in its async structure into
@@ -1495,22 +1507,16 @@ isdn_tty_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 static int
 isdn_tty_open(struct tty_struct *tty, struct file *filp)
 {
-	struct tty_port *port;
-	modem_info *info;
+	modem_info *info = tty->driver_data;
+	struct tty_port *port = &info->port;
 	int retval;
 
-	info = &dev->mdm.info[tty->index];
-	if (isdn_tty_paranoia_check(info, tty->name, "isdn_tty_open"))
-		return -ENODEV;
-	port = &info->port;
 #ifdef ISDN_DEBUG_MODEM_OPEN
 	printk(KERN_DEBUG "isdn_tty_open %s, count = %d\n", tty->name,
 	       port->count);
 #endif
 	port->count++;
-	tty->driver_data = info;
 	port->tty = tty;
-	tty->port = port;
 	/*
 	 * Start up serial port
 	 */
@@ -1738,6 +1744,7 @@ modem_write_profile(atemu *m)
 }
 
 static const struct tty_operations modem_ops = {
+	.install = isdn_tty_install,
 	.open = isdn_tty_open,
 	.close = isdn_tty_close,
 	.write = isdn_tty_write,
@@ -1782,7 +1789,7 @@ isdn_tty_modem_init(void)
 	m->tty_modem->subtype = SERIAL_TYPE_NORMAL;
 	m->tty_modem->init_termios = tty_std_termios;
 	m->tty_modem->init_termios.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
-	m->tty_modem->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	m->tty_modem->flags = TTY_DRIVER_REAL_RAW;
 	m->tty_modem->driver_name = "isdn_tty";
 	tty_set_operations(m->tty_modem, &modem_ops);
 	retval = tty_register_driver(m->tty_modem);
diff --git a/drivers/lguest/x86/core.c b/drivers/lguest/x86/core.c
index 39809035320a..4af12e1844d5 100644
--- a/drivers/lguest/x86/core.c
+++ b/drivers/lguest/x86/core.c
@@ -203,8 +203,8 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 	 * we set it now, so we can trap and pass that trap to the Guest if it
 	 * uses the FPU.
 	 */
-	if (cpu->ts)
-		unlazy_fpu(current);
+	if (cpu->ts && user_has_fpu())
+		stts();
 
 	/*
 	 * SYSENTER is an optimized way of doing system calls.  We can't allow
@@ -234,6 +234,10 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 	 if (boot_cpu_has(X86_FEATURE_SEP))
 		wrmsr(MSR_IA32_SYSENTER_CS, __KERNEL_CS, 0);
 
+	/* Clear the host TS bit if it was set above. */
+	if (cpu->ts && user_has_fpu())
+		clts();
+
 	/*
 	 * If the Guest page faulted, then the cr2 register will tell us the
 	 * bad virtual address.  We have to grab this now, because once we
@@ -249,7 +253,7 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 	 * a different CPU. So all the critical stuff should be done
 	 * before this.
 	 */
-	else if (cpu->regs->trapnum == 7)
+	else if (cpu->regs->trapnum == 7 && !user_has_fpu())
 		math_state_restore();
 }
 
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index d8abb90a6c2f..034233eefc82 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -1555,6 +1555,7 @@ static int multipath_ioctl(struct dm_target *ti, unsigned int cmd,
 			   unsigned long arg)
 {
 	struct multipath *m = ti->private;
+	struct pgpath *pgpath;
 	struct block_device *bdev;
 	fmode_t mode;
 	unsigned long flags;
@@ -1570,12 +1571,14 @@ again:
 	if (!m->current_pgpath)
 		__choose_pgpath(m, 0);
 
-	if (m->current_pgpath) {
-		bdev = m->current_pgpath->path.dev->bdev;
-		mode = m->current_pgpath->path.dev->mode;
+	pgpath = m->current_pgpath;
+
+	if (pgpath) {
+		bdev = pgpath->path.dev->bdev;
+		mode = pgpath->path.dev->mode;
 	}
 
-	if (m->queue_io)
+	if ((pgpath && m->queue_io) || (!pgpath && m->queue_if_no_path))
 		r = -EAGAIN;
 	else if (!bdev)
 		r = -EIO;
diff --git a/drivers/md/dm-table.c b/drivers/md/dm-table.c
index f90069029aae..100368eb7991 100644
--- a/drivers/md/dm-table.c
+++ b/drivers/md/dm-table.c
@@ -1212,6 +1212,41 @@ struct dm_target *dm_table_find_target(struct dm_table *t, sector_t sector)
 	return &t->targets[(KEYS_PER_NODE * n) + k];
 }
 
+static int count_device(struct dm_target *ti, struct dm_dev *dev,
+			sector_t start, sector_t len, void *data)
+{
+	unsigned *num_devices = data;
+
+	(*num_devices)++;
+
+	return 0;
+}
+
+/*
+ * Check whether a table has no data devices attached using each
+ * target's iterate_devices method.
+ * Returns false if the result is unknown because a target doesn't
+ * support iterate_devices.
+ */
+bool dm_table_has_no_data_devices(struct dm_table *table)
+{
+	struct dm_target *uninitialized_var(ti);
+	unsigned i = 0, num_devices = 0;
+
+	while (i < dm_table_get_num_targets(table)) {
+		ti = dm_table_get_target(table, i++);
+
+		if (!ti->type->iterate_devices)
+			return false;
+
+		ti->type->iterate_devices(ti, count_device, &num_devices);
+		if (num_devices)
+			return false;
+	}
+
+	return true;
+}
+
 /*
  * Establish the new table's queue_limits and validate them.
  */
@@ -1354,17 +1389,25 @@ static int device_is_nonrot(struct dm_target *ti, struct dm_dev *dev,
 	return q && blk_queue_nonrot(q);
 }
 
-static bool dm_table_is_nonrot(struct dm_table *t)
+static int device_is_not_random(struct dm_target *ti, struct dm_dev *dev,
+			     sector_t start, sector_t len, void *data)
+{
+	struct request_queue *q = bdev_get_queue(dev->bdev);
+
+	return q && !blk_queue_add_random(q);
+}
+
+static bool dm_table_all_devices_attribute(struct dm_table *t,
+					   iterate_devices_callout_fn func)
 {
 	struct dm_target *ti;
 	unsigned i = 0;
 
-	/* Ensure that all underlying device are non-rotational. */
 	while (i < dm_table_get_num_targets(t)) {
 		ti = dm_table_get_target(t, i++);
 
 		if (!ti->type->iterate_devices ||
-		    !ti->type->iterate_devices(ti, device_is_nonrot, NULL))
+		    !ti->type->iterate_devices(ti, func, NULL))
 			return 0;
 	}
 
@@ -1396,7 +1439,8 @@ void dm_table_set_restrictions(struct dm_table *t, struct request_queue *q,
 	if (!dm_table_discard_zeroes_data(t))
 		q->limits.discard_zeroes_data = 0;
 
-	if (dm_table_is_nonrot(t))
+	/* Ensure that all underlying devices are non-rotational. */
+	if (dm_table_all_devices_attribute(t, device_is_nonrot))
 		queue_flag_set_unlocked(QUEUE_FLAG_NONROT, q);
 	else
 		queue_flag_clear_unlocked(QUEUE_FLAG_NONROT, q);
@@ -1404,6 +1448,15 @@ void dm_table_set_restrictions(struct dm_table *t, struct request_queue *q,
 	dm_table_set_integrity(t);
 
 	/*
+	 * Determine whether or not this queue's I/O timings contribute
+	 * to the entropy pool, Only request-based targets use this.
+	 * Clear QUEUE_FLAG_ADD_RANDOM if any underlying device does not
+	 * have it set.
+	 */
+	if (blk_queue_add_random(q) && dm_table_all_devices_attribute(t, device_is_not_random))
+		queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, q);
+
+	/*
 	 * QUEUE_FLAG_STACKABLE must be set after all queue settings are
 	 * visible to other CPUs because, once the flag is set, incoming bios
 	 * are processed by request-based dm, which refers to the queue
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index af1fc3b2c2ad..c29410af1e22 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -509,9 +509,9 @@ enum pool_mode {
 struct pool_features {
 	enum pool_mode mode;
 
-	unsigned zero_new_blocks:1;
-	unsigned discard_enabled:1;
-	unsigned discard_passdown:1;
+	bool zero_new_blocks:1;
+	bool discard_enabled:1;
+	bool discard_passdown:1;
 };
 
 struct thin_c;
@@ -580,7 +580,8 @@ struct pool_c {
 	struct dm_target_callbacks callbacks;
 
 	dm_block_t low_water_blocks;
-	struct pool_features pf;
+	struct pool_features requested_pf; /* Features requested during table load */
+	struct pool_features adjusted_pf;  /* Features used after adjusting for constituent devices */
 };
 
 /*
@@ -1839,6 +1840,47 @@ static void __requeue_bios(struct pool *pool)
 /*----------------------------------------------------------------
  * Binding of control targets to a pool object
  *--------------------------------------------------------------*/
+static bool data_dev_supports_discard(struct pool_c *pt)
+{
+	struct request_queue *q = bdev_get_queue(pt->data_dev->bdev);
+
+	return q && blk_queue_discard(q);
+}
+
+/*
+ * If discard_passdown was enabled verify that the data device
+ * supports discards.  Disable discard_passdown if not.
+ */
+static void disable_passdown_if_not_supported(struct pool_c *pt)
+{
+	struct pool *pool = pt->pool;
+	struct block_device *data_bdev = pt->data_dev->bdev;
+	struct queue_limits *data_limits = &bdev_get_queue(data_bdev)->limits;
+	sector_t block_size = pool->sectors_per_block << SECTOR_SHIFT;
+	const char *reason = NULL;
+	char buf[BDEVNAME_SIZE];
+
+	if (!pt->adjusted_pf.discard_passdown)
+		return;
+
+	if (!data_dev_supports_discard(pt))
+		reason = "discard unsupported";
+
+	else if (data_limits->max_discard_sectors < pool->sectors_per_block)
+		reason = "max discard sectors smaller than a block";
+
+	else if (data_limits->discard_granularity > block_size)
+		reason = "discard granularity larger than a block";
+
+	else if (block_size & (data_limits->discard_granularity - 1))
+		reason = "discard granularity not a factor of block size";
+
+	if (reason) {
+		DMWARN("Data device (%s) %s: Disabling discard passdown.", bdevname(data_bdev, buf), reason);
+		pt->adjusted_pf.discard_passdown = false;
+	}
+}
+
 static int bind_control_target(struct pool *pool, struct dm_target *ti)
 {
 	struct pool_c *pt = ti->private;
@@ -1847,31 +1889,16 @@ static int bind_control_target(struct pool *pool, struct dm_target *ti)
 	 * We want to make sure that degraded pools are never upgraded.
 	 */
 	enum pool_mode old_mode = pool->pf.mode;
-	enum pool_mode new_mode = pt->pf.mode;
+	enum pool_mode new_mode = pt->adjusted_pf.mode;
 
 	if (old_mode > new_mode)
 		new_mode = old_mode;
 
 	pool->ti = ti;
 	pool->low_water_blocks = pt->low_water_blocks;
-	pool->pf = pt->pf;
-	set_pool_mode(pool, new_mode);
+	pool->pf = pt->adjusted_pf;
 
-	/*
-	 * If discard_passdown was enabled verify that the data device
-	 * supports discards.  Disable discard_passdown if not; otherwise
-	 * -EOPNOTSUPP will be returned.
-	 */
-	/* FIXME: pull this out into a sep fn. */
-	if (pt->pf.discard_passdown) {
-		struct request_queue *q = bdev_get_queue(pt->data_dev->bdev);
-		if (!q || !blk_queue_discard(q)) {
-			char buf[BDEVNAME_SIZE];
-			DMWARN("Discard unsupported by data device (%s): Disabling discard passdown.",
-			       bdevname(pt->data_dev->bdev, buf));
-			pool->pf.discard_passdown = 0;
-		}
-	}
+	set_pool_mode(pool, new_mode);
 
 	return 0;
 }
@@ -1889,9 +1916,9 @@ static void unbind_control_target(struct pool *pool, struct dm_target *ti)
 static void pool_features_init(struct pool_features *pf)
 {
 	pf->mode = PM_WRITE;
-	pf->zero_new_blocks = 1;
-	pf->discard_enabled = 1;
-	pf->discard_passdown = 1;
+	pf->zero_new_blocks = true;
+	pf->discard_enabled = true;
+	pf->discard_passdown = true;
 }
 
 static void __pool_destroy(struct pool *pool)
@@ -2119,13 +2146,13 @@ static int parse_pool_features(struct dm_arg_set *as, struct pool_features *pf,
 		argc--;
 
 		if (!strcasecmp(arg_name, "skip_block_zeroing"))
-			pf->zero_new_blocks = 0;
+			pf->zero_new_blocks = false;
 
 		else if (!strcasecmp(arg_name, "ignore_discard"))
-			pf->discard_enabled = 0;
+			pf->discard_enabled = false;
 
 		else if (!strcasecmp(arg_name, "no_discard_passdown"))
-			pf->discard_passdown = 0;
+			pf->discard_passdown = false;
 
 		else if (!strcasecmp(arg_name, "read_only"))
 			pf->mode = PM_READ_ONLY;
@@ -2259,8 +2286,9 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	pt->metadata_dev = metadata_dev;
 	pt->data_dev = data_dev;
 	pt->low_water_blocks = low_water_blocks;
-	pt->pf = pf;
+	pt->adjusted_pf = pt->requested_pf = pf;
 	ti->num_flush_requests = 1;
+
 	/*
 	 * Only need to enable discards if the pool should pass
 	 * them down to the data device.  The thin device's discard
@@ -2268,12 +2296,14 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	 */
 	if (pf.discard_enabled && pf.discard_passdown) {
 		ti->num_discard_requests = 1;
+
 		/*
 		 * Setting 'discards_supported' circumvents the normal
 		 * stacking of discard limits (this keeps the pool and
 		 * thin devices' discard limits consistent).
 		 */
 		ti->discards_supported = true;
+		ti->discard_zeroes_data_unsupported = true;
 	}
 	ti->private = pt;
 
@@ -2703,7 +2733,7 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 		       format_dev_t(buf2, pt->data_dev->bdev->bd_dev),
 		       (unsigned long)pool->sectors_per_block,
 		       (unsigned long long)pt->low_water_blocks);
-		emit_flags(&pt->pf, result, sz, maxlen);
+		emit_flags(&pt->requested_pf, result, sz, maxlen);
 		break;
 	}
 
@@ -2732,20 +2762,21 @@ static int pool_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
 	return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
 }
 
-static void set_discard_limits(struct pool *pool, struct queue_limits *limits)
+static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
 {
-	/*
-	 * FIXME: these limits may be incompatible with the pool's data device
-	 */
+	struct pool *pool = pt->pool;
+	struct queue_limits *data_limits;
+
 	limits->max_discard_sectors = pool->sectors_per_block;
 
 	/*
-	 * This is just a hint, and not enforced.  We have to cope with
-	 * bios that cover a block partially.  A discard that spans a block
-	 * boundary is not sent to this target.
+	 * discard_granularity is just a hint, and not enforced.
 	 */
-	limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
-	limits->discard_zeroes_data = pool->pf.zero_new_blocks;
+	if (pt->adjusted_pf.discard_passdown) {
+		data_limits = &bdev_get_queue(pt->data_dev->bdev)->limits;
+		limits->discard_granularity = data_limits->discard_granularity;
+	} else
+		limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
 }
 
 static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
@@ -2755,15 +2786,25 @@ static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 	blk_limits_io_min(limits, 0);
 	blk_limits_io_opt(limits, pool->sectors_per_block << SECTOR_SHIFT);
-	if (pool->pf.discard_enabled)
-		set_discard_limits(pool, limits);
+
+	/*
+	 * pt->adjusted_pf is a staging area for the actual features to use.
+	 * They get transferred to the live pool in bind_control_target()
+	 * called from pool_preresume().
+	 */
+	if (!pt->adjusted_pf.discard_enabled)
+		return;
+
+	disable_passdown_if_not_supported(pt);
+
+	set_discard_limits(pt, limits);
 }
 
 static struct target_type pool_target = {
 	.name = "thin-pool",
 	.features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
 		    DM_TARGET_IMMUTABLE,
-	.version = {1, 3, 0},
+	.version = {1, 4, 0},
 	.module = THIS_MODULE,
 	.ctr = pool_ctr,
 	.dtr = pool_dtr,
@@ -3042,19 +3083,19 @@ static int thin_iterate_devices(struct dm_target *ti,
 	return 0;
 }
 
+/*
+ * A thin device always inherits its queue limits from its pool.
+ */
 static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
 {
 	struct thin_c *tc = ti->private;
-	struct pool *pool = tc->pool;
 
-	blk_limits_io_min(limits, 0);
-	blk_limits_io_opt(limits, pool->sectors_per_block << SECTOR_SHIFT);
-	set_discard_limits(pool, limits);
+	*limits = bdev_get_queue(tc->pool_dev->bdev)->limits;
 }
 
 static struct target_type thin_target = {
 	.name = "thin",
-	.version = {1, 3, 0},
+	.version = {1, 4, 0},
 	.module	= THIS_MODULE,
 	.ctr = thin_ctr,
 	.dtr = thin_dtr,
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 254d19268ad2..892ae2766aa6 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -718,8 +718,8 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	v->hash_dev_block_bits = ffs(num) - 1;
 
 	if (sscanf(argv[5], "%llu%c", &num_ll, &dummy) != 1 ||
-	    num_ll << (v->data_dev_block_bits - SECTOR_SHIFT) !=
-	    (sector_t)num_ll << (v->data_dev_block_bits - SECTOR_SHIFT)) {
+	    (sector_t)(num_ll << (v->data_dev_block_bits - SECTOR_SHIFT))
+	    >> (v->data_dev_block_bits - SECTOR_SHIFT) != num_ll) {
 		ti->error = "Invalid data blocks";
 		r = -EINVAL;
 		goto bad;
@@ -733,8 +733,8 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	}
 
 	if (sscanf(argv[6], "%llu%c", &num_ll, &dummy) != 1 ||
-	    num_ll << (v->hash_dev_block_bits - SECTOR_SHIFT) !=
-	    (sector_t)num_ll << (v->hash_dev_block_bits - SECTOR_SHIFT)) {
+	    (sector_t)(num_ll << (v->hash_dev_block_bits - SECTOR_SHIFT))
+	    >> (v->hash_dev_block_bits - SECTOR_SHIFT) != num_ll) {
 		ti->error = "Invalid hash start";
 		r = -EINVAL;
 		goto bad;
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 4e09b6ff5b49..67ffa391edcf 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -865,10 +865,14 @@ static void dm_done(struct request *clone, int error, bool mapped)
 {
 	int r = error;
 	struct dm_rq_target_io *tio = clone->end_io_data;
-	dm_request_endio_fn rq_end_io = tio->ti->type->rq_end_io;
+	dm_request_endio_fn rq_end_io = NULL;
 
-	if (mapped && rq_end_io)
-		r = rq_end_io(tio->ti, clone, error, &tio->info);
+	if (tio->ti) {
+		rq_end_io = tio->ti->type->rq_end_io;
+
+		if (mapped && rq_end_io)
+			r = rq_end_io(tio->ti, clone, error, &tio->info);
+	}
 
 	if (r <= 0)
 		/* The target wants to complete the I/O */
@@ -1588,15 +1592,6 @@ static int map_request(struct dm_target *ti, struct request *clone,
 	int r, requeued = 0;
 	struct dm_rq_target_io *tio = clone->end_io_data;
 
-	/*
-	 * Hold the md reference here for the in-flight I/O.
-	 * We can't rely on the reference count by device opener,
-	 * because the device may be closed during the request completion
-	 * when all bios are completed.
-	 * See the comment in rq_completed() too.
-	 */
-	dm_get(md);
-
 	tio->ti = ti;
 	r = ti->type->map_rq(ti, clone, &tio->info);
 	switch (r) {
@@ -1628,6 +1623,26 @@ static int map_request(struct dm_target *ti, struct request *clone,
 	return requeued;
 }
 
+static struct request *dm_start_request(struct mapped_device *md, struct request *orig)
+{
+	struct request *clone;
+
+	blk_start_request(orig);
+	clone = orig->special;
+	atomic_inc(&md->pending[rq_data_dir(clone)]);
+
+	/*
+	 * Hold the md reference here for the in-flight I/O.
+	 * We can't rely on the reference count by device opener,
+	 * because the device may be closed during the request completion
+	 * when all bios are completed.
+	 * See the comment in rq_completed() too.
+	 */
+	dm_get(md);
+
+	return clone;
+}
+
 /*
  * q->request_fn for request-based dm.
  * Called with the queue lock held.
@@ -1657,14 +1672,21 @@ static void dm_request_fn(struct request_queue *q)
 			pos = blk_rq_pos(rq);
 
 		ti = dm_table_find_target(map, pos);
-		BUG_ON(!dm_target_is_valid(ti));
+		if (!dm_target_is_valid(ti)) {
+			/*
+			 * Must perform setup, that dm_done() requires,
+			 * before calling dm_kill_unmapped_request
+			 */
+			DMERR_LIMIT("request attempted access beyond the end of device");
+			clone = dm_start_request(md, rq);
+			dm_kill_unmapped_request(clone, -EIO);
+			continue;
+		}
 
 		if (ti->type->busy && ti->type->busy(ti))
 			goto delay_and_out;
 
-		blk_start_request(rq);
-		clone = rq->special;
-		atomic_inc(&md->pending[rq_data_dir(clone)]);
+		clone = dm_start_request(md, rq);
 
 		spin_unlock(q->queue_lock);
 		if (map_request(ti, clone, md))
@@ -1684,8 +1706,6 @@ delay_and_out:
 	blk_delay_queue(q, HZ / 10);
 out:
 	dm_table_put(map);
-
-	return;
 }
 
 int dm_underlying_device_busy(struct request_queue *q)
@@ -2409,7 +2429,7 @@ static void dm_queue_flush(struct mapped_device *md)
  */
 struct dm_table *dm_swap_table(struct mapped_device *md, struct dm_table *table)
 {
-	struct dm_table *map = ERR_PTR(-EINVAL);
+	struct dm_table *live_map, *map = ERR_PTR(-EINVAL);
 	struct queue_limits limits;
 	int r;
 
@@ -2419,6 +2439,19 @@ struct dm_table *dm_swap_table(struct mapped_device *md, struct dm_table *table)
 	if (!dm_suspended_md(md))
 		goto out;
 
+	/*
+	 * If the new table has no data devices, retain the existing limits.
+	 * This helps multipath with queue_if_no_path if all paths disappear,
+	 * then new I/O is queued based on these limits, and then some paths
+	 * reappear.
+	 */
+	if (dm_table_has_no_data_devices(table)) {
+		live_map = dm_get_live_table(md);
+		if (live_map)
+			limits = md->queue->limits;
+		dm_table_put(live_map);
+	}
+
 	r = dm_calculate_queue_limits(table, &limits);
 	if (r) {
 		map = ERR_PTR(r);
diff --git a/drivers/md/dm.h b/drivers/md/dm.h
index 52eef493d266..6a99fefaa743 100644
--- a/drivers/md/dm.h
+++ b/drivers/md/dm.h
@@ -54,6 +54,7 @@ void dm_table_event_callback(struct dm_table *t,
 			     void (*fn)(void *), void *context);
 struct dm_target *dm_table_get_target(struct dm_table *t, unsigned int index);
 struct dm_target *dm_table_find_target(struct dm_table *t, sector_t sector);
+bool dm_table_has_no_data_devices(struct dm_table *table);
 int dm_calculate_queue_limits(struct dm_table *table,
 			      struct queue_limits *limits);
 void dm_table_set_restrictions(struct dm_table *t, struct request_queue *q,
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 3f6203a4c7ea..308e87b417e0 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -7619,6 +7619,8 @@ static int remove_and_add_spares(struct mddev *mddev)
 			}
 		}
 	}
+	if (removed)
+		set_bit(MD_CHANGE_DEVS, &mddev->flags);
 	return spares;
 }
 
@@ -7632,9 +7634,11 @@ static void reap_sync_thread(struct mddev *mddev)
 	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery)) {
 		/* success...*/
 		/* activate any spares */
-		if (mddev->pers->spare_active(mddev))
+		if (mddev->pers->spare_active(mddev)) {
 			sysfs_notify(&mddev->kobj, NULL,
 				     "degraded");
+			set_bit(MD_CHANGE_DEVS, &mddev->flags);
+		}
 	}
 	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
 	    mddev->pers->finish_reshape)
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 1c2eb38f3c51..0138a727c1f3 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1512,14 +1512,16 @@ static int _enough(struct r10conf *conf, struct geom *geo, int ignore)
 	do {
 		int n = conf->copies;
 		int cnt = 0;
+		int this = first;
 		while (n--) {
-			if (conf->mirrors[first].rdev &&
-			    first != ignore)
+			if (conf->mirrors[this].rdev &&
+			    this != ignore)
 				cnt++;
-			first = (first+1) % geo->raid_disks;
+			this = (this+1) % geo->raid_disks;
 		}
 		if (cnt == 0)
 			return 0;
+		first = (first + geo->near_copies) % geo->raid_disks;
 	} while (first != 0);
 	return 1;
 }
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index adda94df5eb2..0689173fd9f5 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -393,6 +393,8 @@ static int calc_degraded(struct r5conf *conf)
 	degraded = 0;
 	for (i = 0; i < conf->previous_raid_disks; i++) {
 		struct md_rdev *rdev = rcu_dereference(conf->disks[i].rdev);
+		if (rdev && test_bit(Faulty, &rdev->flags))
+			rdev = rcu_dereference(conf->disks[i].replacement);
 		if (!rdev || test_bit(Faulty, &rdev->flags))
 			degraded++;
 		else if (test_bit(In_sync, &rdev->flags))
@@ -417,6 +419,8 @@ static int calc_degraded(struct r5conf *conf)
 	degraded2 = 0;
 	for (i = 0; i < conf->raid_disks; i++) {
 		struct md_rdev *rdev = rcu_dereference(conf->disks[i].rdev);
+		if (rdev && test_bit(Faulty, &rdev->flags))
+			rdev = rcu_dereference(conf->disks[i].replacement);
 		if (!rdev || test_bit(Faulty, &rdev->flags))
 			degraded2++;
 		else if (test_bit(In_sync, &rdev->flags))
@@ -1587,6 +1591,7 @@ static int resize_stripes(struct r5conf *conf, int newsize)
 		#ifdef CONFIG_MULTICORE_RAID456
 		init_waitqueue_head(&nsh->ops.wait_for_ops);
 		#endif
+		spin_lock_init(&nsh->stripe_lock);
 
 		list_add(&nsh->lru, &newstripes);
 	}
@@ -4192,7 +4197,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 			finish_wait(&conf->wait_for_overlap, &w);
 			set_bit(STRIPE_HANDLE, &sh->state);
 			clear_bit(STRIPE_DELAYED, &sh->state);
-			if ((bi->bi_rw & REQ_NOIDLE) &&
+			if ((bi->bi_rw & REQ_SYNC) &&
 			    !test_and_set_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
 				atomic_inc(&conf->preread_active_stripes);
 			release_stripe_plug(mddev, sh);
diff --git a/drivers/media/dvb/ngene/ngene-cards.c b/drivers/media/dvb/ngene/ngene-cards.c
index 72ee8de02260..0a497be97af8 100644
--- a/drivers/media/dvb/ngene/ngene-cards.c
+++ b/drivers/media/dvb/ngene/ngene-cards.c
@@ -524,7 +524,7 @@ static void ngene_resume(struct pci_dev *dev)
 	printk(KERN_INFO DEVICE_NAME ": resume\n");
 }
 
-static struct pci_error_handlers ngene_errors = {
+static const struct pci_error_handlers ngene_errors = {
 	.error_detected = ngene_error_detected,
 	.link_reset = ngene_link_reset,
 	.slot_reset = ngene_slot_reset,
diff --git a/drivers/memory/Makefile b/drivers/memory/Makefile
index 42b3ce9d80fc..9cce5d70ed52 100644
--- a/drivers/memory/Makefile
+++ b/drivers/memory/Makefile
@@ -2,6 +2,9 @@
 # Makefile for memory devices
 #
 
+ifeq ($(CONFIG_DDR),y)
+obj-$(CONFIG_OF)		+= of_memory.o
+endif
 obj-$(CONFIG_TI_EMIF)		+= emif.o
 obj-$(CONFIG_TEGRA20_MC)	+= tegra20-mc.o
 obj-$(CONFIG_TEGRA30_MC)	+= tegra30-mc.o
diff --git a/drivers/memory/emif.c b/drivers/memory/emif.c
index 33a4396b24cb..06d31c99e6ac 100644
--- a/drivers/memory/emif.c
+++ b/drivers/memory/emif.c
@@ -18,6 +18,7 @@
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/module.h>
@@ -25,6 +26,7 @@
 #include <linux/spinlock.h>
 #include <memory/jedec_ddr.h>
 #include "emif.h"
+#include "of_memory.h"
 
 /**
  * struct emif_data - Per device static data for driver's use
@@ -49,6 +51,7 @@
  *				frequency in effect at the moment)
  * @plat_data:			Pointer to saved platform data.
  * @debugfs_root:		dentry to the root folder for EMIF in debugfs
+ * @np_ddr:			Pointer to ddr device tree node
  */
 struct emif_data {
 	u8				duplicate;
@@ -63,6 +66,7 @@ struct emif_data {
 	struct emif_regs		*curr_regs;
 	struct emif_platform_data	*plat_data;
 	struct dentry			*debugfs_root;
+	struct device_node		*np_ddr;
 };
 
 static struct emif_data *emif1;
@@ -71,6 +75,7 @@ static unsigned long	irq_state;
 static u32		t_ck; /* DDR clock period in ps */
 static LIST_HEAD(device_list);
 
+#ifdef CONFIG_DEBUG_FS
 static void do_emif_regdump_show(struct seq_file *s, struct emif_data *emif,
 	struct emif_regs *regs)
 {
@@ -162,23 +167,23 @@ static int __init_or_module emif_debugfs_init(struct emif_data *emif)
 	int		ret;
 
 	dentry = debugfs_create_dir(dev_name(emif->dev), NULL);
-	if (IS_ERR(dentry)) {
-		ret = PTR_ERR(dentry);
+	if (!dentry) {
+		ret = -ENOMEM;
 		goto err0;
 	}
 	emif->debugfs_root = dentry;
 
 	dentry = debugfs_create_file("regcache_dump", S_IRUGO,
 			emif->debugfs_root, emif, &emif_regdump_fops);
-	if (IS_ERR(dentry)) {
-		ret = PTR_ERR(dentry);
+	if (!dentry) {
+		ret = -ENOMEM;
 		goto err1;
 	}
 
 	dentry = debugfs_create_file("mr4", S_IRUGO,
 			emif->debugfs_root, emif, &emif_mr4_fops);
-	if (IS_ERR(dentry)) {
-		ret = PTR_ERR(dentry);
+	if (!dentry) {
+		ret = -ENOMEM;
 		goto err1;
 	}
 
@@ -194,6 +199,16 @@ static void __exit emif_debugfs_exit(struct emif_data *emif)
 	debugfs_remove_recursive(emif->debugfs_root);
 	emif->debugfs_root = NULL;
 }
+#else
+static inline int __init_or_module emif_debugfs_init(struct emif_data *emif)
+{
+	return 0;
+}
+
+static inline void __exit emif_debugfs_exit(struct emif_data *emif)
+{
+}
+#endif
 
 /*
  * Calculate the period of DDR clock from frequency value
@@ -1148,6 +1163,168 @@ static int is_custom_config_valid(struct emif_custom_configs *cust_cfgs,
 	return valid;
 }
 
+#if defined(CONFIG_OF)
+static void __init_or_module of_get_custom_configs(struct device_node *np_emif,
+		struct emif_data *emif)
+{
+	struct emif_custom_configs	*cust_cfgs = NULL;
+	int				len;
+	const int			*lpmode, *poll_intvl;
+
+	lpmode = of_get_property(np_emif, "low-power-mode", &len);
+	poll_intvl = of_get_property(np_emif, "temp-alert-poll-interval", &len);
+
+	if (lpmode || poll_intvl)
+		cust_cfgs = devm_kzalloc(emif->dev, sizeof(*cust_cfgs),
+			GFP_KERNEL);
+
+	if (!cust_cfgs)
+		return;
+
+	if (lpmode) {
+		cust_cfgs->mask |= EMIF_CUSTOM_CONFIG_LPMODE;
+		cust_cfgs->lpmode = *lpmode;
+		of_property_read_u32(np_emif,
+				"low-power-mode-timeout-performance",
+				&cust_cfgs->lpmode_timeout_performance);
+		of_property_read_u32(np_emif,
+				"low-power-mode-timeout-power",
+				&cust_cfgs->lpmode_timeout_power);
+		of_property_read_u32(np_emif,
+				"low-power-mode-freq-threshold",
+				&cust_cfgs->lpmode_freq_threshold);
+	}
+
+	if (poll_intvl) {
+		cust_cfgs->mask |=
+				EMIF_CUSTOM_CONFIG_TEMP_ALERT_POLL_INTERVAL;
+		cust_cfgs->temp_alert_poll_interval_ms = *poll_intvl;
+	}
+
+	if (!is_custom_config_valid(cust_cfgs, emif->dev)) {
+		devm_kfree(emif->dev, cust_cfgs);
+		return;
+	}
+
+	emif->plat_data->custom_configs = cust_cfgs;
+}
+
+static void __init_or_module of_get_ddr_info(struct device_node *np_emif,
+		struct device_node *np_ddr,
+		struct ddr_device_info *dev_info)
+{
+	u32 density = 0, io_width = 0;
+	int len;
+
+	if (of_find_property(np_emif, "cs1-used", &len))
+		dev_info->cs1_used = true;
+
+	if (of_find_property(np_emif, "cal-resistor-per-cs", &len))
+		dev_info->cal_resistors_per_cs = true;
+
+	if (of_device_is_compatible(np_ddr , "jedec,lpddr2-s4"))
+		dev_info->type = DDR_TYPE_LPDDR2_S4;
+	else if (of_device_is_compatible(np_ddr , "jedec,lpddr2-s2"))
+		dev_info->type = DDR_TYPE_LPDDR2_S2;
+
+	of_property_read_u32(np_ddr, "density", &density);
+	of_property_read_u32(np_ddr, "io-width", &io_width);
+
+	/* Convert from density in Mb to the density encoding in jedc_ddr.h */
+	if (density & (density - 1))
+		dev_info->density = 0;
+	else
+		dev_info->density = __fls(density) - 5;
+
+	/* Convert from io_width in bits to io_width encoding in jedc_ddr.h */
+	if (io_width & (io_width - 1))
+		dev_info->io_width = 0;
+	else
+		dev_info->io_width = __fls(io_width) - 1;
+}
+
+static struct emif_data * __init_or_module of_get_memory_device_details(
+		struct device_node *np_emif, struct device *dev)
+{
+	struct emif_data		*emif = NULL;
+	struct ddr_device_info		*dev_info = NULL;
+	struct emif_platform_data	*pd = NULL;
+	struct device_node		*np_ddr;
+	int				len;
+
+	np_ddr = of_parse_phandle(np_emif, "device-handle", 0);
+	if (!np_ddr)
+		goto error;
+	emif	= devm_kzalloc(dev, sizeof(struct emif_data), GFP_KERNEL);
+	pd	= devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
+	dev_info = devm_kzalloc(dev, sizeof(*dev_info), GFP_KERNEL);
+
+	if (!emif || !pd || !dev_info) {
+		dev_err(dev, "%s: Out of memory!!\n",
+			__func__);
+		goto error;
+	}
+
+	emif->plat_data		= pd;
+	pd->device_info		= dev_info;
+	emif->dev		= dev;
+	emif->np_ddr		= np_ddr;
+	emif->temperature_level	= SDRAM_TEMP_NOMINAL;
+
+	if (of_device_is_compatible(np_emif, "ti,emif-4d"))
+		emif->plat_data->ip_rev = EMIF_4D;
+	else if (of_device_is_compatible(np_emif, "ti,emif-4d5"))
+		emif->plat_data->ip_rev = EMIF_4D5;
+
+	of_property_read_u32(np_emif, "phy-type", &pd->phy_type);
+
+	if (of_find_property(np_emif, "hw-caps-ll-interface", &len))
+		pd->hw_caps |= EMIF_HW_CAPS_LL_INTERFACE;
+
+	of_get_ddr_info(np_emif, np_ddr, dev_info);
+	if (!is_dev_data_valid(pd->device_info->type, pd->device_info->density,
+			pd->device_info->io_width, pd->phy_type, pd->ip_rev,
+			emif->dev)) {
+		dev_err(dev, "%s: invalid device data!!\n", __func__);
+		goto error;
+	}
+	/*
+	 * For EMIF instances other than EMIF1 see if the devices connected
+	 * are exactly same as on EMIF1(which is typically the case). If so,
+	 * mark it as a duplicate of EMIF1. This will save some memory and
+	 * computation.
+	 */
+	if (emif1 && emif1->np_ddr == np_ddr) {
+		emif->duplicate = true;
+		goto out;
+	} else if (emif1) {
+		dev_warn(emif->dev, "%s: Non-symmetric DDR geometry\n",
+			__func__);
+	}
+
+	of_get_custom_configs(np_emif, emif);
+	emif->plat_data->timings = of_get_ddr_timings(np_ddr, emif->dev,
+					emif->plat_data->device_info->type,
+					&emif->plat_data->timings_arr_size);
+
+	emif->plat_data->min_tck = of_get_min_tck(np_ddr, emif->dev);
+	goto out;
+
+error:
+	return NULL;
+out:
+	return emif;
+}
+
+#else
+
+static struct emif_data * __init_or_module of_get_memory_device_details(
+		struct device_node *np_emif, struct device *dev)
+{
+	return NULL;
+}
+#endif
+
 static struct emif_data *__init_or_module get_device_details(
 		struct platform_device *pdev)
 {
@@ -1267,7 +1444,11 @@ static int __init_or_module emif_probe(struct platform_device *pdev)
 	struct resource		*res;
 	int			irq;
 
-	emif = get_device_details(pdev);
+	if (pdev->dev.of_node)
+		emif = of_get_memory_device_details(pdev->dev.of_node, &pdev->dev);
+	else
+		emif = get_device_details(pdev);
+
 	if (!emif) {
 		pr_err("%s: error getting device data\n", __func__);
 		goto error;
@@ -1644,11 +1825,21 @@ static void __attribute__((unused)) freq_post_notify_handling(void)
 	spin_unlock_irqrestore(&emif_lock, irq_state);
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id emif_of_match[] = {
+		{ .compatible = "ti,emif-4d" },
+		{ .compatible = "ti,emif-4d5" },
+		{},
+};
+MODULE_DEVICE_TABLE(of, emif_of_match);
+#endif
+
 static struct platform_driver emif_driver = {
 	.remove		= __exit_p(emif_remove),
 	.shutdown	= emif_shutdown,
 	.driver = {
 		.name = "emif",
+		.of_match_table = of_match_ptr(emif_of_match),
 	},
 };
 
diff --git a/drivers/memory/of_memory.c b/drivers/memory/of_memory.c
new file mode 100644
index 000000000000..60074351f17e
--- /dev/null
+++ b/drivers/memory/of_memory.c
@@ -0,0 +1,153 @@
+/*
+ * OpenFirmware helpers for memory drivers
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 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/platform_device.h>
+#include <linux/list.h>
+#include <linux/of.h>
+#include <linux/gfp.h>
+#include <memory/jedec_ddr.h>
+#include <linux/export.h>
+
+/**
+ * of_get_min_tck() - extract min timing values for ddr
+ * @np: pointer to ddr device tree node
+ * @device: device requesting for min timing values
+ *
+ * Populates the lpddr2_min_tck structure by extracting data
+ * from device tree node. Returns a pointer to the populated
+ * structure. If any error in populating the structure, returns
+ * default min timings provided by JEDEC.
+ */
+const struct lpddr2_min_tck *of_get_min_tck(struct device_node *np,
+		struct device *dev)
+{
+	int			ret = 0;
+	struct lpddr2_min_tck	*min;
+
+	min = devm_kzalloc(dev, sizeof(*min), GFP_KERNEL);
+	if (!min)
+		goto default_min_tck;
+
+	ret |= of_property_read_u32(np, "tRPab-min-tck", &min->tRPab);
+	ret |= of_property_read_u32(np, "tRCD-min-tck", &min->tRCD);
+	ret |= of_property_read_u32(np, "tWR-min-tck", &min->tWR);
+	ret |= of_property_read_u32(np, "tRASmin-min-tck", &min->tRASmin);
+	ret |= of_property_read_u32(np, "tRRD-min-tck", &min->tRRD);
+	ret |= of_property_read_u32(np, "tWTR-min-tck", &min->tWTR);
+	ret |= of_property_read_u32(np, "tXP-min-tck", &min->tXP);
+	ret |= of_property_read_u32(np, "tRTP-min-tck", &min->tRTP);
+	ret |= of_property_read_u32(np, "tCKE-min-tck", &min->tCKE);
+	ret |= of_property_read_u32(np, "tCKESR-min-tck", &min->tCKESR);
+	ret |= of_property_read_u32(np, "tFAW-min-tck", &min->tFAW);
+
+	if (ret) {
+		devm_kfree(dev, min);
+		goto default_min_tck;
+	}
+
+	return min;
+
+default_min_tck:
+	dev_warn(dev, "%s: using default min-tck values\n", __func__);
+	return &lpddr2_jedec_min_tck;
+}
+EXPORT_SYMBOL(of_get_min_tck);
+
+static int of_do_get_timings(struct device_node *np,
+		struct lpddr2_timings *tim)
+{
+	int ret;
+
+	ret = of_property_read_u32(np, "max-freq", &tim->max_freq);
+	ret |= of_property_read_u32(np, "min-freq", &tim->min_freq);
+	ret |= of_property_read_u32(np, "tRPab", &tim->tRPab);
+	ret |= of_property_read_u32(np, "tRCD", &tim->tRCD);
+	ret |= of_property_read_u32(np, "tWR", &tim->tWR);
+	ret |= of_property_read_u32(np, "tRAS-min", &tim->tRAS_min);
+	ret |= of_property_read_u32(np, "tRRD", &tim->tRRD);
+	ret |= of_property_read_u32(np, "tWTR", &tim->tWTR);
+	ret |= of_property_read_u32(np, "tXP", &tim->tXP);
+	ret |= of_property_read_u32(np, "tRTP", &tim->tRTP);
+	ret |= of_property_read_u32(np, "tCKESR", &tim->tCKESR);
+	ret |= of_property_read_u32(np, "tDQSCK-max", &tim->tDQSCK_max);
+	ret |= of_property_read_u32(np, "tFAW", &tim->tFAW);
+	ret |= of_property_read_u32(np, "tZQCS", &tim->tZQCS);
+	ret |= of_property_read_u32(np, "tZQCL", &tim->tZQCL);
+	ret |= of_property_read_u32(np, "tZQinit", &tim->tZQinit);
+	ret |= of_property_read_u32(np, "tRAS-max-ns", &tim->tRAS_max_ns);
+	ret |= of_property_read_u32(np, "tDQSCK-max-derated",
+		&tim->tDQSCK_max_derated);
+
+	return ret;
+}
+
+/**
+ * of_get_ddr_timings() - extracts the ddr timings and updates no of
+ * frequencies available.
+ * @np_ddr: Pointer to ddr device tree node
+ * @dev: Device requesting for ddr timings
+ * @device_type: Type of ddr(LPDDR2 S2/S4)
+ * @nr_frequencies: No of frequencies available for ddr
+ * (updated by this function)
+ *
+ * Populates lpddr2_timings structure by extracting data from device
+ * tree node. Returns pointer to populated structure. If any error
+ * while populating, returns default timings provided by JEDEC.
+ */
+const struct lpddr2_timings *of_get_ddr_timings(struct device_node *np_ddr,
+		struct device *dev, u32 device_type, u32 *nr_frequencies)
+{
+	struct lpddr2_timings	*timings = NULL;
+	u32			arr_sz = 0, i = 0;
+	struct device_node	*np_tim;
+	char			*tim_compat;
+
+	switch (device_type) {
+	case DDR_TYPE_LPDDR2_S2:
+	case DDR_TYPE_LPDDR2_S4:
+		tim_compat = "jedec,lpddr2-timings";
+		break;
+	default:
+		dev_warn(dev, "%s: un-supported memory type\n", __func__);
+	}
+
+	for_each_child_of_node(np_ddr, np_tim)
+		if (of_device_is_compatible(np_tim, tim_compat))
+			arr_sz++;
+
+	if (arr_sz)
+		timings = devm_kzalloc(dev, sizeof(*timings) * arr_sz,
+			GFP_KERNEL);
+
+	if (!timings)
+		goto default_timings;
+
+	for_each_child_of_node(np_ddr, np_tim) {
+		if (of_device_is_compatible(np_tim, tim_compat)) {
+			if (of_do_get_timings(np_tim, &timings[i])) {
+				devm_kfree(dev, timings);
+				goto default_timings;
+			}
+			i++;
+		}
+	}
+
+	*nr_frequencies = arr_sz;
+
+	return timings;
+
+default_timings:
+	dev_warn(dev, "%s: using default timings\n", __func__);
+	*nr_frequencies = ARRAY_SIZE(lpddr2_jedec_timings);
+	return lpddr2_jedec_timings;
+}
+EXPORT_SYMBOL(of_get_ddr_timings);
diff --git a/drivers/memory/of_memory.h b/drivers/memory/of_memory.h
new file mode 100644
index 000000000000..ef2514f553d3
--- /dev/null
+++ b/drivers/memory/of_memory.h
@@ -0,0 +1,36 @@
+/*
+ * OpenFirmware helpers for memory drivers
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __LINUX_MEMORY_OF_REG_H
+#define __LINUX_MEMORY_OF_REG_H
+
+#if defined(CONFIG_OF) && defined(CONFIG_DDR)
+extern const struct lpddr2_min_tck *of_get_min_tck(struct device_node *np,
+		struct device *dev);
+extern const struct lpddr2_timings
+	*of_get_ddr_timings(struct device_node *np_ddr, struct device *dev,
+	u32 device_type, u32 *nr_frequencies);
+#else
+static inline const struct lpddr2_min_tck
+	*of_get_min_tck(struct device_node *np, struct device *dev)
+{
+	return NULL;
+}
+
+static inline const struct lpddr2_timings
+	*of_get_ddr_timings(struct device_node *np_ddr, struct device *dev,
+	u32 device_type, u32 *nr_frequencies)
+{
+	return NULL;
+}
+#endif /* CONFIG_OF && CONFIG_DDR */
+
+#endif /* __LINUX_MEMORY_OF_REG_ */
diff --git a/drivers/memory/tegra20-mc.c b/drivers/memory/tegra20-mc.c
index 3ed49c1c2b91..e6764bb41cb9 100644
--- a/drivers/memory/tegra20-mc.c
+++ b/drivers/memory/tegra20-mc.c
@@ -57,7 +57,7 @@ static inline u32 mc_readl(struct tegra20_mc *mc, u32 offs)
 
 	if (offs < 0x24)
 		val = readl(mc->regs[0] + offs);
-	if (offs < 0x400)
+	else if (offs < 0x400)
 		val = readl(mc->regs[1] + offs - 0x3c);
 
 	return val;
@@ -65,14 +65,10 @@ static inline u32 mc_readl(struct tegra20_mc *mc, u32 offs)
 
 static inline void mc_writel(struct tegra20_mc *mc, u32 val, u32 offs)
 {
-	if (offs < 0x24) {
+	if (offs < 0x24)
 		writel(val, mc->regs[0] + offs);
-		return;
-	}
-	if (offs < 0x400) {
+	else if (offs < 0x400)
 		writel(val, mc->regs[1] + offs - 0x3c);
-		return;
-	}
 }
 
 static const char * const tegra20_mc_client[] = {
diff --git a/drivers/memory/tegra30-mc.c b/drivers/memory/tegra30-mc.c
index e56ff04eb5cc..802b9ea431fa 100644
--- a/drivers/memory/tegra30-mc.c
+++ b/drivers/memory/tegra30-mc.c
@@ -95,11 +95,11 @@ static inline u32 mc_readl(struct tegra30_mc *mc, u32 offs)
 
 	if (offs < 0x10)
 		val = readl(mc->regs[0] + offs);
-	if (offs < 0x1f0)
+	else if (offs < 0x1f0)
 		val = readl(mc->regs[1] + offs - 0x3c);
-	if (offs < 0x228)
+	else if (offs < 0x228)
 		val = readl(mc->regs[2] + offs - 0x200);
-	if (offs < 0x400)
+	else if (offs < 0x400)
 		val = readl(mc->regs[3] + offs - 0x284);
 
 	return val;
@@ -107,22 +107,14 @@ static inline u32 mc_readl(struct tegra30_mc *mc, u32 offs)
 
 static inline void mc_writel(struct tegra30_mc *mc, u32 val, u32 offs)
 {
-	if (offs < 0x10) {
+	if (offs < 0x10)
 		writel(val, mc->regs[0] + offs);
-		return;
-	}
-	if (offs < 0x1f0) {
+	else if (offs < 0x1f0)
 		writel(val, mc->regs[1] + offs - 0x3c);
-		return;
-	}
-	if (offs < 0x228) {
+	else if (offs < 0x228)
 		writel(val, mc->regs[2] + offs - 0x200);
-		return;
-	}
-	if (offs < 0x400) {
+	else if (offs < 0x400)
 		writel(val, mc->regs[3] + offs - 0x284);
-		return;
-	}
 }
 
 static const char * const tegra30_mc_client[] = {
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index 866f95960b4b..29d72a259c85 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -342,7 +342,7 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 
 		 /*
 		  * Delay might be needed for ABB8500 cut 3.0, if not, remove
-		  * when hardware will be availible
+		  * when hardware will be available
 		  */
 			msleep(1);
 			break;
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 0e63cdd9b52a..6b67edbdbd01 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -418,6 +418,9 @@ static struct {
 
 static atomic_t ac_wake_req_state = ATOMIC_INIT(0);
 
+/* Functions definition */
+static void compute_armss_rate(void);
+
 /* Spinlocks */
 static DEFINE_SPINLOCK(prcmu_lock);
 static DEFINE_SPINLOCK(clkout_lock);
@@ -517,6 +520,7 @@ static struct dsiescclk dsiescclk[3] = {
 	}
 };
 
+
 /*
 * Used by MCDE to setup all necessary PRCMU registers
 */
@@ -1013,6 +1017,7 @@ int db8500_prcmu_set_arm_opp(u8 opp)
 		(mb1_transfer.ack.arm_opp != opp))
 		r = -EIO;
 
+	compute_armss_rate();
 	mutex_unlock(&mb1_transfer.lock);
 
 	return r;
@@ -1612,6 +1617,7 @@ static unsigned long pll_rate(void __iomem *reg, unsigned long src_rate,
 	if ((branch == PLL_FIX) || ((branch == PLL_DIV) &&
 		(val & PRCM_PLL_FREQ_DIV2EN) &&
 		((reg == PRCM_PLLSOC0_FREQ) ||
+		 (reg == PRCM_PLLARM_FREQ) ||
 		 (reg == PRCM_PLLDDR_FREQ))))
 		div *= 2;
 
@@ -1661,6 +1667,39 @@ static unsigned long clock_rate(u8 clock)
 	else
 		return 0;
 }
+static unsigned long latest_armss_rate;
+static unsigned long armss_rate(void)
+{
+	return latest_armss_rate;
+}
+
+static void compute_armss_rate(void)
+{
+	u32 r;
+	unsigned long rate;
+
+	r = readl(PRCM_ARM_CHGCLKREQ);
+
+	if (r & PRCM_ARM_CHGCLKREQ_PRCM_ARM_CHGCLKREQ) {
+		/* External ARMCLKFIX clock */
+
+		rate = pll_rate(PRCM_PLLDDR_FREQ, ROOT_CLOCK_RATE, PLL_FIX);
+
+		/* Check PRCM_ARM_CHGCLKREQ divider */
+		if (!(r & PRCM_ARM_CHGCLKREQ_PRCM_ARM_DIVSEL))
+			rate /= 2;
+
+		/* Check PRCM_ARMCLKFIX_MGT divider */
+		r = readl(PRCM_ARMCLKFIX_MGT);
+		r &= PRCM_CLK_MGT_CLKPLLDIV_MASK;
+		rate /= r;
+
+	} else {/* ARM PLL */
+		rate = pll_rate(PRCM_PLLARM_FREQ, ROOT_CLOCK_RATE, PLL_DIV);
+	}
+
+	latest_armss_rate = rate;
+}
 
 static unsigned long dsiclk_rate(u8 n)
 {
@@ -1707,6 +1746,8 @@ unsigned long prcmu_clock_rate(u8 clock)
 		return pll_rate(PRCM_PLLSOC0_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
 	else if (clock == PRCMU_PLLSOC1)
 		return pll_rate(PRCM_PLLSOC1_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
+	else if (clock == PRCMU_ARMSS)
+		return armss_rate();
 	else if (clock == PRCMU_PLLDDR)
 		return pll_rate(PRCM_PLLDDR_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
 	else if (clock == PRCMU_PLLDSI)
@@ -2693,6 +2734,7 @@ void __init db8500_prcmu_early_init(void)
 					 handle_simple_irq);
 		set_irq_flags(irq, IRQF_VALID);
 	}
+	compute_armss_rate();
 }
 
 static void __init init_prcm_registers(void)
diff --git a/drivers/mfd/dbx500-prcmu-regs.h b/drivers/mfd/dbx500-prcmu-regs.h
index 23108a6e3167..79c76ebdba52 100644
--- a/drivers/mfd/dbx500-prcmu-regs.h
+++ b/drivers/mfd/dbx500-prcmu-regs.h
@@ -61,7 +61,8 @@
 #define PRCM_PLLARM_LOCKP_PRCM_PLLARM_LOCKP3	0x2
 
 #define PRCM_ARM_CHGCLKREQ	(_PRCMU_BASE + 0x114)
-#define PRCM_ARM_CHGCLKREQ_PRCM_ARM_CHGCLKREQ	0x1
+#define PRCM_ARM_CHGCLKREQ_PRCM_ARM_CHGCLKREQ	BIT(0)
+#define PRCM_ARM_CHGCLKREQ_PRCM_ARM_DIVSEL	BIT(16)
 
 #define PRCM_PLLARM_ENABLE	(_PRCMU_BASE + 0x98)
 #define PRCM_PLLARM_ENABLE_PRCM_PLLARM_ENABLE	0x1
@@ -140,6 +141,7 @@
 /* PRCMU clock/PLL/reset registers */
 #define PRCM_PLLSOC0_FREQ	   (_PRCMU_BASE + 0x080)
 #define PRCM_PLLSOC1_FREQ	   (_PRCMU_BASE + 0x084)
+#define PRCM_PLLARM_FREQ	   (_PRCMU_BASE + 0x088)
 #define PRCM_PLLDDR_FREQ	   (_PRCMU_BASE + 0x08C)
 #define PRCM_PLL_FREQ_D_SHIFT	0
 #define PRCM_PLL_FREQ_D_MASK	BITS(0, 7)
diff --git a/drivers/mfd/rc5t583.c b/drivers/mfd/rc5t583.c
index 3a8fa88567b1..ff61efc76ce2 100644
--- a/drivers/mfd/rc5t583.c
+++ b/drivers/mfd/rc5t583.c
@@ -281,7 +281,7 @@ static int __devinit rc5t583_i2c_probe(struct i2c_client *i2c,
 
 	if (i2c->irq) {
 		ret = rc5t583_irq_init(rc5t583, i2c->irq, pdata->irq_base);
-		/* Still continue with waring if irq init fails */
+		/* Still continue with warning, if irq init fails */
 		if (ret)
 			dev_warn(&i2c->dev, "IRQ init failed: %d\n", ret);
 		else
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
index 0f70dce61160..fbabc3cbe350 100644
--- a/drivers/mfd/rdc321x-southbridge.c
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -1,5 +1,5 @@
 /*
- * RDC321x MFD southbrige driver
+ * RDC321x MFD southbridge driver
  *
  * Copyright (C) 2007-2010 Florian Fainelli <florian@openwrt.org>
  * Copyright (C) 2010 Bernhard Loos <bernhardloos@googlemail.com>
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index 5f58370ccf55..345960ca2fd8 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -25,6 +25,7 @@
 #include <linux/i2c.h>
 #include <linux/regmap.h>
 #include <linux/regulator/of_regulator.h>
+#include <linux/regulator/machine.h>
 
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps6586x.h>
@@ -346,6 +347,7 @@ failed:
 
 #ifdef CONFIG_OF
 static struct of_regulator_match tps6586x_matches[] = {
+	{ .name = "sys",     .driver_data = (void *)TPS6586X_ID_SYS     },
 	{ .name = "sm0",     .driver_data = (void *)TPS6586X_ID_SM_0    },
 	{ .name = "sm1",     .driver_data = (void *)TPS6586X_ID_SM_1    },
 	{ .name = "sm2",     .driver_data = (void *)TPS6586X_ID_SM_2    },
@@ -369,6 +371,7 @@ static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *clien
 	struct tps6586x_platform_data *pdata;
 	struct tps6586x_subdev_info *devs;
 	struct device_node *regs;
+	const char *sys_rail_name = NULL;
 	unsigned int count;
 	unsigned int i, j;
 	int err;
@@ -391,12 +394,22 @@ static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *clien
 		return NULL;
 
 	for (i = 0, j = 0; i < num && j < count; i++) {
+		struct regulator_init_data *reg_idata;
+
 		if (!tps6586x_matches[i].init_data)
 			continue;
 
+		reg_idata  = tps6586x_matches[i].init_data;
 		devs[j].name = "tps6586x-regulator";
 		devs[j].platform_data = tps6586x_matches[i].init_data;
 		devs[j].id = (int)tps6586x_matches[i].driver_data;
+		if (devs[j].id == TPS6586X_ID_SYS)
+			sys_rail_name = reg_idata->constraints.name;
+
+		if ((devs[j].id == TPS6586X_ID_LDO_5) ||
+			(devs[j].id == TPS6586X_ID_LDO_RTC))
+			reg_idata->supply_regulator = sys_rail_name;
+
 		devs[j].of_node = tps6586x_matches[i].of_node;
 		j++;
 	}
diff --git a/drivers/mfd/tps65911-comparator.c b/drivers/mfd/tps65911-comparator.c
index 5a62e6bf89ae..0b6e361432c4 100644
--- a/drivers/mfd/tps65911-comparator.c
+++ b/drivers/mfd/tps65911-comparator.c
@@ -136,7 +136,7 @@ static __devinit int tps65911_comparator_probe(struct platform_device *pdev)
 
 	ret = comp_threshold_set(tps65910, COMP2, pdata->vmbch2_threshold);
 	if (ret < 0) {
-		dev_err(&pdev->dev, "cannot set COMP2 theshold\n");
+		dev_err(&pdev->dev, "cannot set COMP2 threshold\n");
 		return ret;
 	}
 
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index 0aac4aff17a5..a050e56a9bbd 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -135,6 +135,7 @@ static struct regmap_irq_chip wm8994_irq_chip = {
 	.status_base = WM8994_INTERRUPT_STATUS_1,
 	.mask_base = WM8994_INTERRUPT_STATUS_1_MASK,
 	.ack_base = WM8994_INTERRUPT_STATUS_1,
+	.runtime_pm = true,
 };
 
 int wm8994_irq_init(struct wm8994 *wm8994)
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 98a442da892a..99c73352c430 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -105,7 +105,7 @@ config ATMEL_TCB_CLKSRC_BLOCK
 
 config IBM_ASM
 	tristate "Device driver for IBM RSA service processor"
-	depends on X86 && PCI && INPUT && EXPERIMENTAL
+	depends on X86 && PCI && INPUT
 	---help---
 	  This option enables device driver support for in-band access to the
 	  IBM RSA (Condor) service processor in eServer xSeries systems.
@@ -162,8 +162,8 @@ config SGI_IOC4
 	  Otherwise say N.
 
 config TIFM_CORE
-	tristate "TI Flash Media interface support (EXPERIMENTAL)"
-	depends on EXPERIMENTAL && PCI
+	tristate "TI Flash Media interface support"
+	depends on PCI
 	help
 	  If you want support for Texas Instruments(R) Flash Media adapters
 	  you should select this option and then also choose an appropriate
@@ -178,8 +178,8 @@ config TIFM_CORE
 	  be called tifm_core.
 
 config TIFM_7XX1
-	tristate "TI Flash Media PCI74xx/PCI76xx host adapter support (EXPERIMENTAL)"
-	depends on PCI && TIFM_CORE && EXPERIMENTAL
+	tristate "TI Flash Media PCI74xx/PCI76xx host adapter support"
+	depends on PCI && TIFM_CORE
 	default TIFM_CORE
 	help
 	  This option enables support for Texas Instruments(R) PCI74xx and
@@ -192,7 +192,7 @@ config TIFM_7XX1
 
 config ICS932S401
 	tristate "Integrated Circuits ICS932S401"
-	depends on I2C && EXPERIMENTAL
+	depends on I2C
 	help
 	  If you say yes here you get support for the Integrated Circuits
 	  ICS932S401 clock control chips.
@@ -398,7 +398,7 @@ config EP93XX_PWM
 
 config DS1682
 	tristate "Dallas DS1682 Total Elapsed Time Recorder with Alarm"
-	depends on I2C && EXPERIMENTAL
+	depends on I2C
 	help
 	  If you say yes here you get support for Dallas Semiconductor
 	  DS1682 Total Elapsed Time Recorder.
diff --git a/drivers/misc/bmp085-i2c.c b/drivers/misc/bmp085-i2c.c
index 9943971c13e3..a4f33c995ea1 100644
--- a/drivers/misc/bmp085-i2c.c
+++ b/drivers/misc/bmp085-i2c.c
@@ -57,12 +57,6 @@ static int bmp085_i2c_remove(struct i2c_client *client)
 	return bmp085_remove(&client->dev);
 }
 
-static const struct of_device_id bmp085_of_match[] = {
-	{ .compatible = "bosch,bmp085", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, bmp085_of_match);
-
 static const struct i2c_device_id bmp085_id[] = {
 	{ BMP085_NAME, 0 },
 	{ "bmp180", 0 },
@@ -74,7 +68,6 @@ static struct i2c_driver bmp085_i2c_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= BMP085_NAME,
-		.of_match_table = bmp085_of_match
 	},
 	.id_table	= bmp085_id,
 	.probe		= bmp085_i2c_probe,
diff --git a/drivers/misc/bmp085-spi.c b/drivers/misc/bmp085-spi.c
index 78aaff9b5231..5e982af99730 100644
--- a/drivers/misc/bmp085-spi.c
+++ b/drivers/misc/bmp085-spi.c
@@ -73,19 +73,8 @@ static struct spi_driver bmp085_spi_driver = {
 	.remove		= __devexit_p(bmp085_spi_remove)
 };
 
-static int __init bmp085_spi_init(void)
-{
-	return spi_register_driver(&bmp085_spi_driver);
-}
-
-static void __exit bmp085_spi_exit(void)
-{
-	spi_unregister_driver(&bmp085_spi_driver);
-}
+module_spi_driver(bmp085_spi_driver);
 
 MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
 MODULE_DESCRIPTION("BMP085 SPI bus driver");
 MODULE_LICENSE("GPL");
-
-module_init(bmp085_spi_init);
-module_exit(bmp085_spi_exit);
diff --git a/drivers/misc/c2port/Kconfig b/drivers/misc/c2port/Kconfig
index 33ee834e1b83..0dd690e61d3c 100644
--- a/drivers/misc/c2port/Kconfig
+++ b/drivers/misc/c2port/Kconfig
@@ -3,8 +3,7 @@
 #
 
 menuconfig C2PORT
-	tristate "Silicon Labs C2 port support (EXPERIMENTAL)"
-	depends on EXPERIMENTAL
+	tristate "Silicon Labs C2 port support"
 	default n
 	help
 	  This option enables support for Silicon Labs C2 port used to
@@ -22,7 +21,7 @@ menuconfig C2PORT
 if C2PORT
 
 config C2PORT_DURAMAR_2150
-	tristate "C2 port support for Eurotech's Duramar 2150 (EXPERIMENTAL)"
+	tristate "C2 port support for Eurotech's Duramar 2150"
 	depends on X86
 	default n
 	help
diff --git a/drivers/misc/carma/carma-fpga-program.c b/drivers/misc/carma/carma-fpga-program.c
index a2d25e4857e3..eaddfe9db149 100644
--- a/drivers/misc/carma/carma-fpga-program.c
+++ b/drivers/misc/carma/carma-fpga-program.c
@@ -978,7 +978,6 @@ static int fpga_of_probe(struct platform_device *op)
 	dev_set_drvdata(priv->dev, priv);
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_MEMCPY, mask);
-	dma_cap_set(DMA_INTERRUPT, mask);
 	dma_cap_set(DMA_SLAVE, mask);
 	dma_cap_set(DMA_SG, mask);
 
diff --git a/drivers/misc/carma/carma-fpga.c b/drivers/misc/carma/carma-fpga.c
index 8c279da07410..0c43297ed9ac 100644
--- a/drivers/misc/carma/carma-fpga.c
+++ b/drivers/misc/carma/carma-fpga.c
@@ -666,7 +666,7 @@ static int data_submit_dma(struct fpga_device *priv, struct data_buf *buf)
 	src = SYS_FPGA_BLOCK;
 	tx = chan->device->device_prep_dma_memcpy(chan, dst, src,
 						  REG_BLOCK_SIZE,
-						  DMA_PREP_INTERRUPT);
+						  0);
 	if (!tx) {
 		dev_err(priv->dev, "unable to prep SYS-FPGA DMA\n");
 		return -ENOMEM;
diff --git a/drivers/misc/eeprom/Kconfig b/drivers/misc/eeprom/Kconfig
index 701edf658970..c9e695ea7c9a 100644
--- a/drivers/misc/eeprom/Kconfig
+++ b/drivers/misc/eeprom/Kconfig
@@ -50,7 +50,7 @@ config EEPROM_LEGACY
 
 config EEPROM_MAX6875
 	tristate "Maxim MAX6874/5 power supply supervisor"
-	depends on I2C && EXPERIMENTAL
+	depends on I2C
 	help
 	  If you say yes here you get read-only support for the user EEPROM of
 	  the Maxim MAX6874/5 EEPROM-programmable, quad power-supply
diff --git a/drivers/misc/eeprom/at25.c b/drivers/misc/eeprom/at25.c
index 25003d6ceb56..4ed93dd54116 100644
--- a/drivers/misc/eeprom/at25.c
+++ b/drivers/misc/eeprom/at25.c
@@ -302,6 +302,61 @@ static ssize_t at25_mem_write(struct memory_accessor *mem, const char *buf,
 
 /*-------------------------------------------------------------------------*/
 
+static int at25_np_to_chip(struct device *dev,
+			   struct device_node *np,
+			   struct spi_eeprom *chip)
+{
+	u32 val;
+
+	memset(chip, 0, sizeof(*chip));
+	strncpy(chip->name, np->name, sizeof(chip->name));
+
+	if (of_property_read_u32(np, "size", &val) == 0 ||
+	    of_property_read_u32(np, "at25,byte-len", &val) == 0) {
+		chip->byte_len = val;
+	} else {
+		dev_err(dev, "Error: missing \"size\" property\n");
+		return -ENODEV;
+	}
+
+	if (of_property_read_u32(np, "pagesize", &val) == 0 ||
+	    of_property_read_u32(np, "at25,page-size", &val) == 0) {
+		chip->page_size = (u16)val;
+	} else {
+		dev_err(dev, "Error: missing \"pagesize\" property\n");
+		return -ENODEV;
+	}
+
+	if (of_property_read_u32(np, "at25,addr-mode", &val) == 0) {
+		chip->flags = (u16)val;
+	} else {
+		if (of_property_read_u32(np, "address-width", &val)) {
+			dev_err(dev,
+				"Error: missing \"address-width\" property\n");
+			return -ENODEV;
+		}
+		switch (val) {
+		case 8:
+			chip->flags |= EE_ADDR1;
+			break;
+		case 16:
+			chip->flags |= EE_ADDR2;
+			break;
+		case 24:
+			chip->flags |= EE_ADDR3;
+			break;
+		default:
+			dev_err(dev,
+				"Error: bad \"address-width\" property: %u\n",
+				val);
+			return -ENODEV;
+		}
+		if (of_find_property(np, "read-only", NULL))
+			chip->flags |= EE_READONLY;
+	}
+	return 0;
+}
+
 static int at25_probe(struct spi_device *spi)
 {
 	struct at25_data	*at25 = NULL;
@@ -314,33 +369,11 @@ static int at25_probe(struct spi_device *spi)
 	/* Chip description */
 	if (!spi->dev.platform_data) {
 		if (np) {
-			u32 val;
-
-			memset(&chip, 0, sizeof(chip));
-			strncpy(chip.name, np->name, 10);
-
-			err = of_property_read_u32(np, "at25,byte-len", &val);
-			if (err) {
-				dev_dbg(&spi->dev, "invalid chip dt description\n");
-				goto fail;
-			}
-			chip.byte_len = val;
-
-			err = of_property_read_u32(np, "at25,addr-mode", &val);
-			if (err) {
-				dev_dbg(&spi->dev, "invalid chip dt description\n");
-				goto fail;
-			}
-			chip.flags = (u16)val;
-
-			err = of_property_read_u32(np, "at25,page-size", &val);
-			if (err) {
-				dev_dbg(&spi->dev, "invalid chip dt description\n");
+			err = at25_np_to_chip(&spi->dev, np, &chip);
+			if (err)
 				goto fail;
-			}
-			chip.page_size = (u16)val;
 		} else {
-			dev_dbg(&spi->dev, "no chip description\n");
+			dev_err(&spi->dev, "Error: no chip description\n");
 			err = -ENODEV;
 			goto fail;
 		}
diff --git a/drivers/misc/hpilo.c b/drivers/misc/hpilo.c
index 6df0da4085e3..12ccdf94e4fa 100644
--- a/drivers/misc/hpilo.c
+++ b/drivers/misc/hpilo.c
@@ -736,7 +736,14 @@ static void ilo_remove(struct pci_dev *pdev)
 	free_irq(pdev->irq, ilo_hw);
 	ilo_unmap_device(pdev, ilo_hw);
 	pci_release_regions(pdev);
-	pci_disable_device(pdev);
+	/*
+	 * pci_disable_device(pdev) used to be here. But this PCI device has
+	 * two functions with interrupt lines connected to a single pin. The
+	 * other one is a USB host controller. So when we disable the PIN here
+	 * e.g. by rmmod hpilo, the controller stops working. It is because
+	 * the interrupt link is disabled in ACPI since it is not refcounted
+	 * yet. See acpi_pci_link_free_irq called from acpi_pci_irq_disable.
+	 */
 	kfree(ilo_hw);
 	ilo_hwdev[(minor / max_ccb)] = 0;
 }
@@ -826,7 +833,7 @@ unmap:
 free_regions:
 	pci_release_regions(pdev);
 disable:
-	pci_disable_device(pdev);
+/*	pci_disable_device(pdev);  see comment in ilo_remove */
 free:
 	kfree(ilo_hw);
 out:
diff --git a/drivers/misc/ibmasm/uart.c b/drivers/misc/ibmasm/uart.c
index 1dcb9ae1905a..01e2b0d7e590 100644
--- a/drivers/misc/ibmasm/uart.c
+++ b/drivers/misc/ibmasm/uart.c
@@ -33,7 +33,7 @@
 
 void ibmasm_register_uart(struct service_processor *sp)
 {
-	struct uart_port uport;
+	struct uart_8250_port uart;
 	void __iomem *iomem_base;
 
 	iomem_base = sp->base_address + SCOUT_COM_B_BASE;
@@ -47,14 +47,14 @@ void ibmasm_register_uart(struct service_processor *sp)
 		return;
 	}
 
-	memset(&uport, 0, sizeof(struct uart_port));
-	uport.irq	= sp->irq;
-	uport.uartclk	= 3686400;
-	uport.flags	= UPF_SHARE_IRQ;
-	uport.iotype	= UPIO_MEM;
-	uport.membase	= iomem_base;
+	memset(&uart, 0, sizeof(uart));
+	uart.port.irq		= sp->irq;
+	uart.port.uartclk	= 3686400;
+	uart.port.flags		= UPF_SHARE_IRQ;
+	uart.port.iotype	= UPIO_MEM;
+	uart.port.membase	= iomem_base;
 
-	sp->serial_line = serial8250_register_port(&uport);
+	sp->serial_line = serial8250_register_8250_port(&uart);
 	if (sp->serial_line < 0) {
 		dev_err(sp->dev, "Failed to register serial port\n");
 		return;
diff --git a/drivers/misc/lis3lv02d/lis3lv02d.c b/drivers/misc/lis3lv02d/lis3lv02d.c
index a981e2a42f92..4a87e5c0a320 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d.c
@@ -39,6 +39,7 @@
 #include <linux/miscdevice.h>
 #include <linux/pm_runtime.h>
 #include <linux/atomic.h>
+#include <linux/of_device.h>
 #include "lis3lv02d.h"
 
 #define DRIVER_NAME     "lis3lv02d"
@@ -80,6 +81,15 @@
 #define LIS3_SENSITIVITY_12B		((LIS3_ACCURACY * 1000) / 1024)
 #define LIS3_SENSITIVITY_8B		(18 * LIS3_ACCURACY)
 
+/*
+ * LIS331DLH spec says 1LSBs corresponds 4G/4096 -> 1LSB is 1000/1024 mG.
+ * Below macros defines sensitivity values for +/-2G. Dataout bits for
+ * +/-2G range is 12 bits so 4 bits adjustment must be done to get 12bit
+ * data from 16bit value. Currently this driver supports only 2G range.
+ */
+#define LIS3DLH_SENSITIVITY_2G		((LIS3_ACCURACY * 1000) / 1024)
+#define SHIFT_ADJ_2G			4
+
 #define LIS3_DEFAULT_FUZZ_12B		3
 #define LIS3_DEFAULT_FLAT_12B		3
 #define LIS3_DEFAULT_FUZZ_8B		1
@@ -135,6 +145,19 @@ static s16 lis3lv02d_read_12(struct lis3lv02d *lis3, int reg)
 	return (s16)((hi << 8) | lo);
 }
 
+/* 12bits for 2G range, 13 bits for 4G range and 14 bits for 8G range */
+static s16 lis331dlh_read_data(struct lis3lv02d *lis3, int reg)
+{
+	u8 lo, hi;
+	int v;
+
+	lis3->read(lis3, reg - 1, &lo);
+	lis3->read(lis3, reg, &hi);
+	v = (int) ((hi << 8) | lo);
+
+	return (s16) v >> lis3->shift_adj;
+}
+
 /**
  * lis3lv02d_get_axis - For the given axis, give the value converted
  * @axis:      1,2,3 - can also be negative
@@ -195,6 +218,7 @@ static void lis3lv02d_get_xyz(struct lis3lv02d *lis3, int *x, int *y, int *z)
 static int lis3_12_rates[4] = {40, 160, 640, 2560};
 static int lis3_8_rates[2] = {100, 400};
 static int lis3_3dc_rates[16] = {0, 1, 10, 25, 50, 100, 200, 400, 1600, 5000};
+static int lis3_3dlh_rates[4] = {50, 100, 400, 1000};
 
 /* ODR is Output Data Rate */
 static int lis3lv02d_get_odr(struct lis3lv02d *lis3)
@@ -267,7 +291,7 @@ static int lis3lv02d_selftest(struct lis3lv02d *lis3, s16 results[3])
 				(LIS3_IRQ1_DATA_READY | LIS3_IRQ2_DATA_READY));
 	}
 
-	if (lis3->whoami == WAI_3DC) {
+	if ((lis3->whoami == WAI_3DC) || (lis3->whoami == WAI_3DLH)) {
 		ctlreg = CTRL_REG4;
 		selftest = CTRL4_ST0;
 	} else {
@@ -398,9 +422,17 @@ int lis3lv02d_poweron(struct lis3lv02d *lis3)
 		lis3->read(lis3, CTRL_REG2, &reg);
 		if (lis3->whoami ==  WAI_12B)
 			reg |= CTRL2_BDU | CTRL2_BOOT;
+		else if (lis3->whoami ==  WAI_3DLH)
+			reg |= CTRL2_BOOT_3DLH;
 		else
 			reg |= CTRL2_BOOT_8B;
 		lis3->write(lis3, CTRL_REG2, reg);
+
+		if (lis3->whoami ==  WAI_3DLH) {
+			lis3->read(lis3, CTRL_REG4, &reg);
+			reg |= CTRL4_BDU;
+			lis3->write(lis3, CTRL_REG4, reg);
+		}
 	}
 
 	err = lis3lv02d_get_pwron_wait(lis3);
@@ -912,6 +944,154 @@ static void lis3lv02d_8b_configure(struct lis3lv02d *lis3,
 	}
 }
 
+#ifdef CONFIG_OF
+int lis3lv02d_init_dt(struct lis3lv02d *lis3)
+{
+	struct lis3lv02d_platform_data *pdata;
+	struct device_node *np = lis3->of_node;
+	u32 val;
+
+	if (!lis3->of_node)
+		return 0;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	if (of_get_property(np, "st,click-single-x", NULL))
+		pdata->click_flags |= LIS3_CLICK_SINGLE_X;
+	if (of_get_property(np, "st,click-double-x", NULL))
+		pdata->click_flags |= LIS3_CLICK_DOUBLE_X;
+
+	if (of_get_property(np, "st,click-single-y", NULL))
+		pdata->click_flags |= LIS3_CLICK_SINGLE_Y;
+	if (of_get_property(np, "st,click-double-y", NULL))
+		pdata->click_flags |= LIS3_CLICK_DOUBLE_Y;
+
+	if (of_get_property(np, "st,click-single-z", NULL))
+		pdata->click_flags |= LIS3_CLICK_SINGLE_Z;
+	if (of_get_property(np, "st,click-double-z", NULL))
+		pdata->click_flags |= LIS3_CLICK_DOUBLE_Z;
+
+	if (!of_property_read_u32(np, "st,click-threshold-x", &val))
+		pdata->click_thresh_x = val;
+	if (!of_property_read_u32(np, "st,click-threshold-y", &val))
+		pdata->click_thresh_y = val;
+	if (!of_property_read_u32(np, "st,click-threshold-z", &val))
+		pdata->click_thresh_z = val;
+
+	if (!of_property_read_u32(np, "st,click-time-limit", &val))
+		pdata->click_time_limit = val;
+	if (!of_property_read_u32(np, "st,click-latency", &val))
+		pdata->click_latency = val;
+	if (!of_property_read_u32(np, "st,click-window", &val))
+		pdata->click_window = val;
+
+	if (of_get_property(np, "st,irq1-disable", NULL))
+		pdata->irq_cfg |= LIS3_IRQ1_DISABLE;
+	if (of_get_property(np, "st,irq1-ff-wu-1", NULL))
+		pdata->irq_cfg |= LIS3_IRQ1_FF_WU_1;
+	if (of_get_property(np, "st,irq1-ff-wu-2", NULL))
+		pdata->irq_cfg |= LIS3_IRQ1_FF_WU_2;
+	if (of_get_property(np, "st,irq1-data-ready", NULL))
+		pdata->irq_cfg |= LIS3_IRQ1_DATA_READY;
+	if (of_get_property(np, "st,irq1-click", NULL))
+		pdata->irq_cfg |= LIS3_IRQ1_CLICK;
+
+	if (of_get_property(np, "st,irq2-disable", NULL))
+		pdata->irq_cfg |= LIS3_IRQ2_DISABLE;
+	if (of_get_property(np, "st,irq2-ff-wu-1", NULL))
+		pdata->irq_cfg |= LIS3_IRQ2_FF_WU_1;
+	if (of_get_property(np, "st,irq2-ff-wu-2", NULL))
+		pdata->irq_cfg |= LIS3_IRQ2_FF_WU_2;
+	if (of_get_property(np, "st,irq2-data-ready", NULL))
+		pdata->irq_cfg |= LIS3_IRQ2_DATA_READY;
+	if (of_get_property(np, "st,irq2-click", NULL))
+		pdata->irq_cfg |= LIS3_IRQ2_CLICK;
+
+	if (of_get_property(np, "st,irq-open-drain", NULL))
+		pdata->irq_cfg |= LIS3_IRQ_OPEN_DRAIN;
+	if (of_get_property(np, "st,irq-active-low", NULL))
+		pdata->irq_cfg |= LIS3_IRQ_ACTIVE_LOW;
+
+	if (!of_property_read_u32(np, "st,wu-duration-1", &val))
+		pdata->duration1 = val;
+	if (!of_property_read_u32(np, "st,wu-duration-2", &val))
+		pdata->duration2 = val;
+
+	if (of_get_property(np, "st,wakeup-x-lo", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_X_LO;
+	if (of_get_property(np, "st,wakeup-x-hi", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_X_HI;
+	if (of_get_property(np, "st,wakeup-y-lo", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_Y_LO;
+	if (of_get_property(np, "st,wakeup-y-hi", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_Y_HI;
+	if (of_get_property(np, "st,wakeup-z-lo", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_Z_LO;
+	if (of_get_property(np, "st,wakeup-z-hi", NULL))
+		pdata->wakeup_flags |= LIS3_WAKEUP_Z_HI;
+
+	if (!of_property_read_u32(np, "st,highpass-cutoff-hz", &val)) {
+		switch (val) {
+		case 1:
+			pdata->hipass_ctrl = LIS3_HIPASS_CUTFF_1HZ;
+			break;
+		case 2:
+			pdata->hipass_ctrl = LIS3_HIPASS_CUTFF_2HZ;
+			break;
+		case 4:
+			pdata->hipass_ctrl = LIS3_HIPASS_CUTFF_4HZ;
+			break;
+		case 8:
+			pdata->hipass_ctrl = LIS3_HIPASS_CUTFF_8HZ;
+			break;
+		}
+	}
+
+	if (of_get_property(np, "st,hipass1-disable", NULL))
+		pdata->hipass_ctrl |= LIS3_HIPASS1_DISABLE;
+	if (of_get_property(np, "st,hipass2-disable", NULL))
+		pdata->hipass_ctrl |= LIS3_HIPASS2_DISABLE;
+
+	if (of_get_property(np, "st,axis-x", &val))
+		pdata->axis_x = val;
+	if (of_get_property(np, "st,axis-y", &val))
+		pdata->axis_y = val;
+	if (of_get_property(np, "st,axis-z", &val))
+		pdata->axis_z = val;
+
+	if (of_get_property(np, "st,default-rate", NULL))
+		pdata->default_rate = val;
+
+	if (of_get_property(np, "st,min-limit-x", &val))
+		pdata->st_min_limits[0] = val;
+	if (of_get_property(np, "st,min-limit-y", &val))
+		pdata->st_min_limits[1] = val;
+	if (of_get_property(np, "st,min-limit-z", &val))
+		pdata->st_min_limits[2] = val;
+
+	if (of_get_property(np, "st,max-limit-x", &val))
+		pdata->st_max_limits[0] = val;
+	if (of_get_property(np, "st,max-limit-y", &val))
+		pdata->st_max_limits[1] = val;
+	if (of_get_property(np, "st,max-limit-z", &val))
+		pdata->st_max_limits[2] = val;
+
+
+	lis3->pdata = pdata;
+
+	return 0;
+}
+
+#else
+int lis3lv02d_init_dt(struct lis3lv02d *lis3)
+{
+	return 0;
+}
+#endif
+EXPORT_SYMBOL_GPL(lis3lv02d_init_dt);
+
 /*
  * Initialise the accelerometer and the various subsystems.
  * Should be rather independent of the bus system.
@@ -956,6 +1136,16 @@ int lis3lv02d_init_device(struct lis3lv02d *lis3)
 		lis3->odr_mask = CTRL1_ODR0|CTRL1_ODR1|CTRL1_ODR2|CTRL1_ODR3;
 		lis3->scale = LIS3_SENSITIVITY_8B;
 		break;
+	case WAI_3DLH:
+		pr_info("16 bits lis331dlh sensor found\n");
+		lis3->read_data = lis331dlh_read_data;
+		lis3->mdps_max_val = 2048; /* 12 bits for 2G */
+		lis3->shift_adj = SHIFT_ADJ_2G;
+		lis3->pwron_delay = LIS3_PWRON_DELAY_WAI_8B;
+		lis3->odrs = lis3_3dlh_rates;
+		lis3->odr_mask = CTRL1_DR0 | CTRL1_DR1;
+		lis3->scale = LIS3DLH_SENSITIVITY_2G;
+		break;
 	default:
 		pr_err("unknown sensor type 0x%X\n", lis3->whoami);
 		return -EINVAL;
diff --git a/drivers/misc/lis3lv02d/lis3lv02d.h b/drivers/misc/lis3lv02d/lis3lv02d.h
index 2b1482ad3f16..c439c827eea8 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d.h
+++ b/drivers/misc/lis3lv02d/lis3lv02d.h
@@ -26,12 +26,12 @@
 /*
  * This driver tries to support the "digital" accelerometer chips from
  * STMicroelectronics such as LIS3LV02DL, LIS302DL, LIS3L02DQ, LIS331DL,
- * LIS35DE, or LIS202DL. They are very similar in terms of programming, with
- * almost the same registers. In addition to differing on physical properties,
- * they differ on the number of axes (2/3), precision (8/12 bits), and special
- * features (freefall detection, click...). Unfortunately, not all the
- * differences can be probed via a register.
- * They can be connected either via I²C or SPI.
+ * LIS331DLH, LIS35DE, or LIS202DL. They are very similar in terms of
+ * programming, with almost the same registers. In addition to differing
+ * on physical properties, they differ on the number of axes (2/3),
+ * precision (8/12 bits), and special features (freefall detection,
+ * click...). Unfortunately, not all the differences can be probed via
+ * a register. They can be connected either via I²C or SPI.
  */
 
 #include <linux/lis3lv02d.h>
@@ -96,12 +96,22 @@ enum lis3lv02d_reg {
 };
 
 enum lis3_who_am_i {
+	WAI_3DLH	= 0x32,	/* 16 bits: LIS331DLH */
 	WAI_3DC		= 0x33,	/* 8 bits: LIS3DC, HP3DC */
 	WAI_12B		= 0x3A, /* 12 bits: LIS3LV02D[LQ]... */
 	WAI_8B		= 0x3B, /* 8 bits: LIS[23]02D[LQ]... */
 	WAI_6B		= 0x52, /* 6 bits: LIS331DLF - not supported */
 };
 
+enum lis3_type {
+	LIS3LV02D,
+	LIS3DC,
+	HP3DC,
+	LIS2302D,
+	LIS331DLF,
+	LIS331DLH,
+};
+
 enum lis3lv02d_ctrl1_12b {
 	CTRL1_Xen	= 0x01,
 	CTRL1_Yen	= 0x02,
@@ -129,6 +139,27 @@ enum lis3lv02d_ctrl1_3dc {
 	CTRL1_ODR3	= 0x80,
 };
 
+enum lis331dlh_ctrl1 {
+	CTRL1_DR0	= 0x08,
+	CTRL1_DR1	= 0x10,
+	CTRL1_PM0	= 0x20,
+	CTRL1_PM1	= 0x40,
+	CTRL1_PM2	= 0x80,
+};
+
+enum lis331dlh_ctrl2 {
+	CTRL2_HPEN1	= 0x04,
+	CTRL2_HPEN2	= 0x08,
+	CTRL2_FDS_3DLH	= 0x10,
+	CTRL2_BOOT_3DLH	= 0x80,
+};
+
+enum lis331dlh_ctrl4 {
+	CTRL4_STSIGN	= 0x08,
+	CTRL4_BLE	= 0x40,
+	CTRL4_BDU	= 0x80,
+};
+
 enum lis3lv02d_ctrl2 {
 	CTRL2_DAS	= 0x01,
 	CTRL2_SIM	= 0x02,
@@ -279,9 +310,14 @@ struct lis3lv02d {
 	int                     data_ready_count[2];
 	atomic_t		wake_thread;
 	unsigned char           irq_cfg;
+	unsigned int		shift_adj;
 
 	struct lis3lv02d_platform_data *pdata;	/* for passing board config */
 	struct mutex		mutex;     /* Serialize poll and selftest */
+
+#ifdef CONFIG_OF
+	struct device_node	*of_node;
+#endif
 };
 
 int lis3lv02d_init_device(struct lis3lv02d *lis3);
@@ -290,5 +326,6 @@ void lis3lv02d_joystick_disable(struct lis3lv02d *lis3);
 void lis3lv02d_poweroff(struct lis3lv02d *lis3);
 int lis3lv02d_poweron(struct lis3lv02d *lis3);
 int lis3lv02d_remove_fs(struct lis3lv02d *lis3);
+int lis3lv02d_init_dt(struct lis3lv02d *lis3);
 
 extern struct lis3lv02d lis3_dev;
diff --git a/drivers/misc/lis3lv02d/lis3lv02d_i2c.c b/drivers/misc/lis3lv02d/lis3lv02d_i2c.c
index e8c0019da97a..60ec8689d6e3 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d_i2c.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d_i2c.c
@@ -31,6 +31,10 @@
 #include <linux/i2c.h>
 #include <linux/pm_runtime.h>
 #include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+
 #include "lis3lv02d.h"
 
 #define DRV_NAME	"lis3lv02d_i2c"
@@ -90,7 +94,11 @@ static int lis3_i2c_init(struct lis3lv02d *lis3)
 	if (ret < 0)
 		return ret;
 
-	reg |= CTRL1_PD0 | CTRL1_Xen | CTRL1_Yen | CTRL1_Zen;
+	if (lis3->whoami == WAI_3DLH)
+		reg |= CTRL1_PM0 | CTRL1_Xen | CTRL1_Yen | CTRL1_Zen;
+	else
+		reg |= CTRL1_PD0 | CTRL1_Xen | CTRL1_Yen | CTRL1_Zen;
+
 	return lis3->write(lis3, CTRL_REG1, reg);
 }
 
@@ -98,12 +106,30 @@ static int lis3_i2c_init(struct lis3lv02d *lis3)
 static union axis_conversion lis3lv02d_axis_map =
 	{ .as_array = { LIS3_DEV_X, LIS3_DEV_Y, LIS3_DEV_Z } };
 
+#ifdef CONFIG_OF
+static struct of_device_id lis3lv02d_i2c_dt_ids[] = {
+	{ .compatible = "st,lis3lv02d" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, lis3lv02d_i2c_dt_ids);
+#endif
+
 static int __devinit lis3lv02d_i2c_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
 	int ret = 0;
 	struct lis3lv02d_platform_data *pdata = client->dev.platform_data;
 
+#ifdef CONFIG_OF
+	if (of_match_device(lis3lv02d_i2c_dt_ids, &client->dev)) {
+		lis3_dev.of_node = client->dev.of_node;
+		ret = lis3lv02d_init_dt(&lis3_dev);
+		if (ret)
+			return ret;
+		pdata = lis3_dev.pdata;
+	}
+#endif
+
 	if (pdata) {
 		if ((pdata->driver_features & LIS3_USE_BLOCK_READ) &&
 			(i2c_check_functionality(client->adapter,
@@ -231,7 +257,8 @@ static int lis3_i2c_runtime_resume(struct device *dev)
 #endif /* CONFIG_PM_RUNTIME */
 
 static const struct i2c_device_id lis3lv02d_id[] = {
-	{"lis3lv02d", 0 },
+	{"lis3lv02d", LIS3LV02D},
+	{"lis331dlh", LIS331DLH},
 	{}
 };
 
@@ -250,6 +277,7 @@ static struct i2c_driver lis3lv02d_i2c_driver = {
 		.name   = DRV_NAME,
 		.owner  = THIS_MODULE,
 		.pm     = &lis3_pm_ops,
+		.of_match_table = of_match_ptr(lis3lv02d_i2c_dt_ids),
 	},
 	.probe	= lis3lv02d_i2c_probe,
 	.remove	= __devexit_p(lis3lv02d_i2c_remove),
diff --git a/drivers/misc/lis3lv02d/lis3lv02d_spi.c b/drivers/misc/lis3lv02d/lis3lv02d_spi.c
index 80880e984b4f..ccb6475fa059 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d_spi.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d_spi.c
@@ -17,6 +17,9 @@
 #include <linux/workqueue.h>
 #include <linux/spi/spi.h>
 #include <linux/pm.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
 
 #include "lis3lv02d.h"
 
@@ -58,6 +61,14 @@ static int lis3_spi_init(struct lis3lv02d *lis3)
 static union axis_conversion lis3lv02d_axis_normal =
 	{ .as_array = { 1, 2, 3 } };
 
+#ifdef CONFIG_OF
+static struct of_device_id lis302dl_spi_dt_ids[] = {
+	{ .compatible = "st,lis302dl-spi" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, lis302dl_spi_dt_ids);
+#endif
+
 static int __devinit lis302dl_spi_probe(struct spi_device *spi)
 {
 	int ret;
@@ -75,6 +86,15 @@ static int __devinit lis302dl_spi_probe(struct spi_device *spi)
 	lis3_dev.irq		= spi->irq;
 	lis3_dev.ac		= lis3lv02d_axis_normal;
 	lis3_dev.pdata		= spi->dev.platform_data;
+
+#ifdef CONFIG_OF
+	if (of_match_device(lis302dl_spi_dt_ids, &spi->dev)) {
+		lis3_dev.of_node = spi->dev.of_node;
+		ret = lis3lv02d_init_dt(&lis3_dev);
+		if (ret)
+			return ret;
+	}
+#endif
 	spi_set_drvdata(spi, &lis3_dev);
 
 	return lis3lv02d_init_device(&lis3_dev);
@@ -121,6 +141,7 @@ static struct spi_driver lis302dl_spi_driver = {
 		.name   = DRV_NAME,
 		.owner  = THIS_MODULE,
 		.pm	= &lis3lv02d_spi_pm,
+		.of_match_table = of_match_ptr(lis302dl_spi_dt_ids),
 	},
 	.probe	= lis302dl_spi_probe,
 	.remove	= __devexit_p(lis302dl_spi_remove),
diff --git a/drivers/misc/mei/Kconfig b/drivers/misc/mei/Kconfig
index 47d78a72db2e..5a79ccde2fdf 100644
--- a/drivers/misc/mei/Kconfig
+++ b/drivers/misc/mei/Kconfig
@@ -1,6 +1,6 @@
 config INTEL_MEI
 	tristate "Intel Management Engine Interface (Intel MEI)"
-	depends on X86 && PCI && EXPERIMENTAL && WATCHDOG_CORE
+	depends on X86 && PCI && WATCHDOG_CORE
 	help
 	  The Intel Management Engine (Intel ME) provides Manageability,
 	  Security and Media services for system containing Intel chipsets.
diff --git a/drivers/misc/mei/hw.h b/drivers/misc/mei/hw.h
index 24c4c962819e..9700532f02f6 100644
--- a/drivers/misc/mei/hw.h
+++ b/drivers/misc/mei/hw.h
@@ -40,47 +40,48 @@
 /*
  * MEI device IDs
  */
-#define    MEI_DEV_ID_82946GZ	0x2974  /* 82946GZ/GL */
-#define    MEI_DEV_ID_82G35	0x2984  /* 82G35 Express */
-#define    MEI_DEV_ID_82Q965	0x2994  /* 82Q963/Q965 */
-#define    MEI_DEV_ID_82G965	0x29A4  /* 82P965/G965 */
-
-#define    MEI_DEV_ID_82GM965	0x2A04  /* Mobile PM965/GM965 */
-#define    MEI_DEV_ID_82GME965	0x2A14  /* Mobile GME965/GLE960 */
-
-#define    MEI_DEV_ID_ICH9_82Q35 0x29B4  /* 82Q35 Express */
-#define    MEI_DEV_ID_ICH9_82G33 0x29C4  /* 82G33/G31/P35/P31 Express */
-#define    MEI_DEV_ID_ICH9_82Q33 0x29D4  /* 82Q33 Express */
-#define    MEI_DEV_ID_ICH9_82X38 0x29E4  /* 82X38/X48 Express */
-#define    MEI_DEV_ID_ICH9_3200  0x29F4  /* 3200/3210 Server */
-
-#define    MEI_DEV_ID_ICH9_6	0x28B4  /* Bearlake */
-#define    MEI_DEV_ID_ICH9_7	0x28C4  /* Bearlake */
-#define    MEI_DEV_ID_ICH9_8	0x28D4  /* Bearlake */
-#define    MEI_DEV_ID_ICH9_9    0x28E4  /* Bearlake */
-#define    MEI_DEV_ID_ICH9_10	0x28F4  /* Bearlake */
-
-#define    MEI_DEV_ID_ICH9M_1	0x2A44  /* Cantiga */
-#define    MEI_DEV_ID_ICH9M_2	0x2A54  /* Cantiga */
-#define    MEI_DEV_ID_ICH9M_3	0x2A64  /* Cantiga */
-#define    MEI_DEV_ID_ICH9M_4	0x2A74  /* Cantiga */
-
-#define    MEI_DEV_ID_ICH10_1	0x2E04  /* Eaglelake */
-#define    MEI_DEV_ID_ICH10_2	0x2E14  /* Eaglelake */
-#define    MEI_DEV_ID_ICH10_3	0x2E24  /* Eaglelake */
-#define    MEI_DEV_ID_ICH10_4	0x2E34  /* Eaglelake */
-
-#define    MEI_DEV_ID_IBXPK_1	0x3B64  /* Calpella */
-#define    MEI_DEV_ID_IBXPK_2	0x3B65  /* Calpella */
-
-#define    MEI_DEV_ID_CPT_1	0x1C3A    /* Cougerpoint */
-#define    MEI_DEV_ID_PBG_1	0x1D3A    /* PBG */
-
-#define    MEI_DEV_ID_PPT_1	0x1E3A    /* Pantherpoint PPT */
-#define    MEI_DEV_ID_PPT_2	0x1CBA    /* Pantherpoint PPT */
-#define    MEI_DEV_ID_PPT_3	0x1DBA    /* Pantherpoint PPT */
-
-
+#define MEI_DEV_ID_82946GZ    0x2974  /* 82946GZ/GL */
+#define MEI_DEV_ID_82G35      0x2984  /* 82G35 Express */
+#define MEI_DEV_ID_82Q965     0x2994  /* 82Q963/Q965 */
+#define MEI_DEV_ID_82G965     0x29A4  /* 82P965/G965 */
+
+#define MEI_DEV_ID_82GM965    0x2A04  /* Mobile PM965/GM965 */
+#define MEI_DEV_ID_82GME965   0x2A14  /* Mobile GME965/GLE960 */
+
+#define MEI_DEV_ID_ICH9_82Q35 0x29B4  /* 82Q35 Express */
+#define MEI_DEV_ID_ICH9_82G33 0x29C4  /* 82G33/G31/P35/P31 Express */
+#define MEI_DEV_ID_ICH9_82Q33 0x29D4  /* 82Q33 Express */
+#define MEI_DEV_ID_ICH9_82X38 0x29E4  /* 82X38/X48 Express */
+#define MEI_DEV_ID_ICH9_3200  0x29F4  /* 3200/3210 Server */
+
+#define MEI_DEV_ID_ICH9_6     0x28B4  /* Bearlake */
+#define MEI_DEV_ID_ICH9_7     0x28C4  /* Bearlake */
+#define MEI_DEV_ID_ICH9_8     0x28D4  /* Bearlake */
+#define MEI_DEV_ID_ICH9_9     0x28E4  /* Bearlake */
+#define MEI_DEV_ID_ICH9_10    0x28F4  /* Bearlake */
+
+#define MEI_DEV_ID_ICH9M_1    0x2A44  /* Cantiga */
+#define MEI_DEV_ID_ICH9M_2    0x2A54  /* Cantiga */
+#define MEI_DEV_ID_ICH9M_3    0x2A64  /* Cantiga */
+#define MEI_DEV_ID_ICH9M_4    0x2A74  /* Cantiga */
+
+#define MEI_DEV_ID_ICH10_1    0x2E04  /* Eaglelake */
+#define MEI_DEV_ID_ICH10_2    0x2E14  /* Eaglelake */
+#define MEI_DEV_ID_ICH10_3    0x2E24  /* Eaglelake */
+#define MEI_DEV_ID_ICH10_4    0x2E34  /* Eaglelake */
+
+#define MEI_DEV_ID_IBXPK_1    0x3B64  /* Calpella */
+#define MEI_DEV_ID_IBXPK_2    0x3B65  /* Calpella */
+
+#define MEI_DEV_ID_CPT_1      0x1C3A  /* Couger Point */
+#define MEI_DEV_ID_PBG_1      0x1D3A  /* C600/X79 Patsburg */
+
+#define MEI_DEV_ID_PPT_1      0x1E3A  /* Panther Point */
+#define MEI_DEV_ID_PPT_2      0x1CBA  /* Panther Point */
+#define MEI_DEV_ID_PPT_3      0x1DBA  /* Panther Point */
+
+#define MEI_DEV_ID_LPT        0x8C3A  /* Lynx Point */
+#define MEI_DEV_ID_LPT_LP     0x9C3A  /* Lynx Point LP */
 /*
  * MEI HW Section
  */
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index e77f86e69fb5..98f1430e3e14 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -24,6 +24,25 @@
 #include "interface.h"
 #include <linux/mei.h>
 
+const char *mei_dev_state_str(int state)
+{
+#define MEI_DEV_STATE(state) case MEI_DEV_##state: return #state
+	switch (state) {
+	MEI_DEV_STATE(INITIALIZING);
+	MEI_DEV_STATE(INIT_CLIENTS);
+	MEI_DEV_STATE(ENABLED);
+	MEI_DEV_STATE(RESETING);
+	MEI_DEV_STATE(DISABLED);
+	MEI_DEV_STATE(RECOVERING_FROM_RESET);
+	MEI_DEV_STATE(POWER_DOWN);
+	MEI_DEV_STATE(POWER_UP);
+	default:
+		return "unkown";
+	}
+#undef MEI_DEV_STATE
+}
+
+
 const uuid_le mei_amthi_guid  = UUID_LE(0x12f80028, 0xb4b7, 0x4b2d, 0xac,
 						0xa8, 0x46, 0xe0, 0xff, 0x65,
 						0x81, 0x4c);
@@ -123,7 +142,7 @@ struct mei_device *mei_device_init(struct pci_dev *pdev)
 	mutex_init(&dev->device_lock);
 	init_waitqueue_head(&dev->wait_recvd_msg);
 	init_waitqueue_head(&dev->wait_stop_wd);
-	dev->mei_state = MEI_INITIALIZING;
+	dev->dev_state = MEI_DEV_INITIALIZING;
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 	dev->wd_interface_reg = false;
 
@@ -182,7 +201,7 @@ int mei_hw_init(struct mei_device *dev)
 	}
 
 	if (err <= 0 && !dev->recvd_msg) {
-		dev->mei_state = MEI_DISABLED;
+		dev->dev_state = MEI_DEV_DISABLED;
 		dev_dbg(&dev->pdev->dev,
 			"wait_event_interruptible_timeout failed"
 			"on wait for ME to turn on ME_RDY.\n");
@@ -192,7 +211,7 @@ int mei_hw_init(struct mei_device *dev)
 
 	if (!(((dev->host_hw_state & H_RDY) == H_RDY) &&
 	      ((dev->me_hw_state & ME_RDY_HRA) == ME_RDY_HRA))) {
-		dev->mei_state = MEI_DISABLED;
+		dev->dev_state = MEI_DEV_DISABLED;
 		dev_dbg(&dev->pdev->dev,
 			"host_hw_state = 0x%08x, me_hw_state = 0x%08x.\n",
 			dev->host_hw_state, dev->me_hw_state);
@@ -258,15 +277,15 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 	struct mei_cl_cb *cb_next = NULL;
 	bool unexpected;
 
-	if (dev->mei_state == MEI_RECOVERING_FROM_RESET) {
+	if (dev->dev_state == MEI_DEV_RECOVERING_FROM_RESET) {
 		dev->need_reset = true;
 		return;
 	}
 
-	unexpected = (dev->mei_state != MEI_INITIALIZING &&
-			dev->mei_state != MEI_DISABLED &&
-			dev->mei_state != MEI_POWER_DOWN &&
-			dev->mei_state != MEI_POWER_UP);
+	unexpected = (dev->dev_state != MEI_DEV_INITIALIZING &&
+			dev->dev_state != MEI_DEV_DISABLED &&
+			dev->dev_state != MEI_DEV_POWER_DOWN &&
+			dev->dev_state != MEI_DEV_POWER_UP);
 
 	dev->host_hw_state = mei_hcsr_read(dev);
 
@@ -285,10 +304,10 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 
 	dev->need_reset = false;
 
-	if (dev->mei_state != MEI_INITIALIZING) {
-		if (dev->mei_state != MEI_DISABLED &&
-		    dev->mei_state != MEI_POWER_DOWN)
-			dev->mei_state = MEI_RESETING;
+	if (dev->dev_state != MEI_DEV_INITIALIZING) {
+		if (dev->dev_state != MEI_DEV_DISABLED &&
+		    dev->dev_state != MEI_DEV_POWER_DOWN)
+			dev->dev_state = MEI_DEV_RESETING;
 
 		list_for_each_entry_safe(cl_pos,
 				cl_next, &dev->file_list, link) {
@@ -311,7 +330,6 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 
 	dev->me_clients_num = 0;
 	dev->rd_msg_hdr = 0;
-	dev->stop = false;
 	dev->wd_pending = false;
 
 	/* update the state of the registers after reset */
@@ -322,7 +340,8 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 	    dev->host_hw_state, dev->me_hw_state);
 
 	if (unexpected)
-		dev_warn(&dev->pdev->dev, "unexpected reset.\n");
+		dev_warn(&dev->pdev->dev, "unexpected reset: dev_state = %s\n",
+			 mei_dev_state_str(dev->dev_state));
 
 	/* Wake up all readings so they can be interrupted */
 	list_for_each_entry_safe(cl_pos, cl_next, &dev->file_list, link) {
@@ -371,7 +390,7 @@ void mei_host_start_message(struct mei_device *dev)
 	if (mei_write_message(dev, mei_hdr, (unsigned char *)host_start_req,
 				       mei_hdr->length)) {
 		dev_dbg(&dev->pdev->dev, "write send version message to FW fail.\n");
-		dev->mei_state = MEI_RESETING;
+		dev->dev_state = MEI_DEV_RESETING;
 		mei_reset(dev, 1);
 	}
 	dev->init_clients_state = MEI_START_MESSAGE;
@@ -403,7 +422,7 @@ void mei_host_enum_clients_message(struct mei_device *dev)
 	host_enum_req->hbm_cmd = HOST_ENUM_REQ_CMD;
 	if (mei_write_message(dev, mei_hdr, (unsigned char *)host_enum_req,
 				mei_hdr->length)) {
-		dev->mei_state = MEI_RESETING;
+		dev->dev_state = MEI_DEV_RESETING;
 		dev_dbg(&dev->pdev->dev, "write send enumeration request message to FW fail.\n");
 		mei_reset(dev, 1);
 	}
@@ -444,7 +463,7 @@ void mei_allocate_me_clients_storage(struct mei_device *dev)
 			sizeof(struct mei_me_client), GFP_KERNEL);
 	if (!clients) {
 		dev_dbg(&dev->pdev->dev, "memory allocation for ME clients failed.\n");
-		dev->mei_state = MEI_RESETING;
+		dev->dev_state = MEI_DEV_RESETING;
 		mei_reset(dev, 1);
 		return ;
 	}
@@ -490,7 +509,7 @@ int mei_host_client_properties(struct mei_device *dev)
 		if (mei_write_message(dev, mei_header,
 				(unsigned char *)host_cli_req,
 				mei_header->length)) {
-			dev->mei_state = MEI_RESETING;
+			dev->dev_state = MEI_DEV_RESETING;
 			dev_dbg(&dev->pdev->dev, "write send enumeration request message to FW fail.\n");
 			mei_reset(dev, 1);
 			return -EIO;
@@ -522,12 +541,12 @@ void mei_cl_init(struct mei_cl *priv, struct mei_device *dev)
 	priv->dev = dev;
 }
 
-int mei_find_me_client_index(const struct mei_device *dev, uuid_le cuuid)
+int mei_me_cl_by_uuid(const struct mei_device *dev, const uuid_le *cuuid)
 {
-	int i, res = -1;
+	int i, res = -ENOENT;
 
 	for (i = 0; i < dev->me_clients_num; ++i)
-		if (uuid_le_cmp(cuuid,
+		if (uuid_le_cmp(*cuuid,
 				dev->me_clients[i].props.protocol_name) == 0) {
 			res = i;
 			break;
@@ -538,35 +557,35 @@ int mei_find_me_client_index(const struct mei_device *dev, uuid_le cuuid)
 
 
 /**
- * mei_find_me_client_update_filext - searches for ME client guid
+ * mei_me_cl_update_filext - searches for ME client guid
  *                       sets client_id in mei_file_private if found
  * @dev: the device structure
- * @priv: private file structure to set client_id in
- * @cguid: searched guid of ME client
+ * @cl: private file structure to set client_id in
+ * @cuuid: searched uuid of ME client
  * @client_id: id of host client to be set in file private structure
  *
  * returns ME client index
  */
-u8 mei_find_me_client_update_filext(struct mei_device *dev, struct mei_cl *priv,
-				const uuid_le *cguid, u8 client_id)
+int mei_me_cl_update_filext(struct mei_device *dev, struct mei_cl *cl,
+				const uuid_le *cuuid, u8 host_cl_id)
 {
 	int i;
 
-	if (!dev || !priv || !cguid)
-		return 0;
+	if (!dev || !cl || !cuuid)
+		return -EINVAL;
 
 	/* check for valid client id */
-	i = mei_find_me_client_index(dev, *cguid);
+	i = mei_me_cl_by_uuid(dev, cuuid);
 	if (i >= 0) {
-		priv->me_client_id = dev->me_clients[i].client_id;
-		priv->state = MEI_FILE_CONNECTING;
-		priv->host_client_id = client_id;
+		cl->me_client_id = dev->me_clients[i].client_id;
+		cl->state = MEI_FILE_CONNECTING;
+		cl->host_client_id = host_cl_id;
 
-		list_add_tail(&priv->link, &dev->file_list);
+		list_add_tail(&cl->link, &dev->file_list);
 		return (u8)i;
 	}
 
-	return 0;
+	return -ENOENT;
 }
 
 /**
@@ -577,16 +596,16 @@ u8 mei_find_me_client_update_filext(struct mei_device *dev, struct mei_cl *priv,
  */
 void mei_host_init_iamthif(struct mei_device *dev)
 {
-	u8 i;
+	int i;
 	unsigned char *msg_buf;
 
 	mei_cl_init(&dev->iamthif_cl, dev);
 	dev->iamthif_cl.state = MEI_FILE_DISCONNECTED;
 
 	/* find ME amthi client */
-	i = mei_find_me_client_update_filext(dev, &dev->iamthif_cl,
+	i = mei_me_cl_update_filext(dev, &dev->iamthif_cl,
 			    &mei_amthi_guid, MEI_IAMTHIF_HOST_CLIENT_ID);
-	if (dev->iamthif_cl.state != MEI_FILE_CONNECTING) {
+	if (i < 0) {
 		dev_dbg(&dev->pdev->dev, "failed to find iamthif client.\n");
 		return;
 	}
diff --git a/drivers/misc/mei/interface.h b/drivers/misc/mei/interface.h
index fb5c7db4723b..ec6c785a3961 100644
--- a/drivers/misc/mei/interface.h
+++ b/drivers/misc/mei/interface.h
@@ -23,14 +23,6 @@
 #include "mei_dev.h"
 
 
-#define AMT_WD_DEFAULT_TIMEOUT 120	/* seconds */
-#define AMT_WD_MIN_TIMEOUT 120	/* seconds */
-#define AMT_WD_MAX_TIMEOUT 65535	/* seconds */
-
-#define MEI_WATCHDOG_DATA_SIZE         16
-#define MEI_START_WD_DATA_SIZE         20
-#define MEI_WD_PARAMS_SIZE             4
-
 
 void mei_read_slots(struct mei_device *dev,
 		     unsigned char *buffer,
@@ -64,7 +56,7 @@ int mei_flow_ctrl_creds(struct mei_device *dev, struct mei_cl *cl);
 
 
 int mei_wd_send(struct mei_device *dev);
-int mei_wd_stop(struct mei_device *dev, bool preserve);
+int mei_wd_stop(struct mei_device *dev);
 int mei_wd_host_init(struct mei_device *dev);
 /*
  * mei_watchdog_register  - Registering watchdog interface
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index d78c05e693f7..3533edde04a5 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -221,17 +221,10 @@ static int mei_irq_thread_read_client_message(struct mei_io_list *complete_list,
 				cl->status = 0;
 				list_del(&cb_pos->cb_list);
 				dev_dbg(&dev->pdev->dev,
-					"completed read host client = %d,"
-					"ME client = %d, "
-					"data length = %lu\n",
+					"completed read H cl = %d, ME cl = %d, length = %lu\n",
 					cl->host_client_id,
 					cl->me_client_id,
 					cb_pos->information);
-
-				*(cb_pos->response_buffer.data +
-					cb_pos->information) = '\0';
-				dev_dbg(&dev->pdev->dev, "cb_pos->res_buffer - %s\n",
-					cb_pos->response_buffer.data);
 				list_add_tail(&cb_pos->cb_list,
 					&complete_list->mei_cb.cb_list);
 			}
@@ -633,7 +626,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 		if (version_res->host_version_supported) {
 			dev->version.major_version = HBM_MAJOR_VERSION;
 			dev->version.minor_version = HBM_MINOR_VERSION;
-			if (dev->mei_state == MEI_INIT_CLIENTS &&
+			if (dev->dev_state == MEI_DEV_INIT_CLIENTS &&
 			    dev->init_clients_state == MEI_START_MESSAGE) {
 				dev->init_clients_timer = 0;
 				mei_host_enum_clients_message(dev);
@@ -707,7 +700,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 			dev->me_clients[dev->me_client_presentation_num].props
 						= props_res->client_properties;
 
-			if (dev->mei_state == MEI_INIT_CLIENTS &&
+			if (dev->dev_state == MEI_DEV_INIT_CLIENTS &&
 			    dev->init_clients_state ==
 					MEI_CLIENT_PROPERTIES_MESSAGE) {
 				dev->me_client_index++;
@@ -734,7 +727,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 					 * Client ID 2 - Reserved for AMTHI
 					 */
 					bitmap_set(dev->host_clients_map, 0, 3);
-					dev->mei_state = MEI_ENABLED;
+					dev->dev_state = MEI_DEV_ENABLED;
 
 					/* if wd initialization fails, initialization the AMTHI client,
 					 * otherwise the AMTHI client will be initialized after the WD client connect response
@@ -759,7 +752,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 	case HOST_ENUM_RES_CMD:
 		enum_res = (struct hbm_host_enum_response *) mei_msg;
 		memcpy(dev->me_clients_map, enum_res->valid_addresses, 32);
-		if (dev->mei_state == MEI_INIT_CLIENTS &&
+		if (dev->dev_state == MEI_DEV_INIT_CLIENTS &&
 		    dev->init_clients_state == MEI_ENUM_CLIENTS_MESSAGE) {
 				dev->init_clients_timer = 0;
 				dev->me_client_presentation_num = 0;
@@ -776,7 +769,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 		break;
 
 	case HOST_STOP_RES_CMD:
-		dev->mei_state = MEI_DISABLED;
+		dev->dev_state = MEI_DEV_DISABLED;
 		dev_dbg(&dev->pdev->dev, "resetting because of FW stop response.\n");
 		mei_reset(dev, 1);
 		break;
@@ -1224,10 +1217,9 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 		}
 	}
 
-	if (dev->stop && !dev->wd_pending) {
-		dev->wd_stopped = true;
+	if (dev->wd_state == MEI_WD_STOPPING) {
+		dev->wd_state = MEI_WD_IDLE;
 		wake_up_interruptible(&dev->wait_stop_wd);
-		return 0;
 	}
 
 	if (dev->extra_write_index) {
@@ -1240,7 +1232,7 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 		*slots -= dev->extra_write_index;
 		dev->extra_write_index = 0;
 	}
-	if (dev->mei_state == MEI_ENABLED) {
+	if (dev->dev_state == MEI_DEV_ENABLED) {
 		if (dev->wd_pending &&
 		    mei_flow_ctrl_creds(dev, &dev->wd_cl) > 0) {
 			if (mei_wd_send(dev))
@@ -1250,14 +1242,12 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 
 			dev->wd_pending = false;
 
-			if (dev->wd_timeout)
-				*slots -= mei_data2slots(MEI_START_WD_DATA_SIZE);
+			if (dev->wd_state == MEI_WD_RUNNING)
+				*slots -= mei_data2slots(MEI_WD_START_MSG_SIZE);
 			else
-				*slots -= mei_data2slots(MEI_WD_PARAMS_SIZE);
+				*slots -= mei_data2slots(MEI_WD_STOP_MSG_SIZE);
 		}
 	}
-	if (dev->stop)
-		return -ENODEV;
 
 	/* complete control write list CB */
 	dev_dbg(&dev->pdev->dev, "complete control write list cb.\n");
@@ -1361,8 +1351,8 @@ void mei_timer(struct work_struct *work)
 
 
 	mutex_lock(&dev->device_lock);
-	if (dev->mei_state != MEI_ENABLED) {
-		if (dev->mei_state == MEI_INIT_CLIENTS) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
+		if (dev->dev_state == MEI_DEV_INIT_CLIENTS) {
 			if (dev->init_clients_timer) {
 				if (--dev->init_clients_timer == 0) {
 					dev_dbg(&dev->pdev->dev, "IMEI reset due to init clients timeout ,init clients state = %d.\n",
@@ -1484,8 +1474,8 @@ irqreturn_t mei_interrupt_thread_handler(int irq, void *dev_id)
 
 	/* check if ME wants a reset */
 	if ((dev->me_hw_state & ME_RDY_HRA) == 0 &&
-	    dev->mei_state != MEI_RESETING &&
-	    dev->mei_state != MEI_INITIALIZING) {
+	    dev->dev_state != MEI_DEV_RESETING &&
+	    dev->dev_state != MEI_DEV_INITIALIZING) {
 		dev_dbg(&dev->pdev->dev, "FW not ready.\n");
 		mei_reset(dev, 1);
 		mutex_unlock(&dev->device_lock);
@@ -1498,7 +1488,7 @@ irqreturn_t mei_interrupt_thread_handler(int irq, void *dev_id)
 			dev_dbg(&dev->pdev->dev, "we need to start the dev.\n");
 			dev->host_hw_state |= (H_IE | H_IG | H_RDY);
 			mei_hcsr_set(dev);
-			dev->mei_state = MEI_INIT_CLIENTS;
+			dev->dev_state = MEI_DEV_INIT_CLIENTS;
 			dev_dbg(&dev->pdev->dev, "link is established start sending messages.\n");
 			/* link is established
 			 * start sending messages.
diff --git a/drivers/misc/mei/iorw.c b/drivers/misc/mei/iorw.c
index 50f52e21f587..fcba98eb892e 100644
--- a/drivers/misc/mei/iorw.c
+++ b/drivers/misc/mei/iorw.c
@@ -38,7 +38,31 @@
 #include <linux/mei.h>
 #include "interface.h"
 
+/**
+ * mei_me_cl_by_id return index to me_clients for client_id
+ *
+ * @dev: the device structure
+ * @client_id: me client id
+ *
+ * Locking: called under "dev->device_lock" lock
+ *
+ * returns index on success, -ENOENT on failure.
+ */
 
+int mei_me_cl_by_id(struct mei_device *dev, u8 client_id)
+{
+	int i;
+	for (i = 0; i < dev->me_clients_num; i++)
+		if (dev->me_clients[i].client_id == client_id)
+			break;
+	if (WARN_ON(dev->me_clients[i].client_id != client_id))
+		return -ENOENT;
+
+	if (i == dev->me_clients_num)
+		return -ENOENT;
+
+	return i;
+}
 
 /**
  * mei_ioctl_connect_client - the connect to fw client IOCTL function
@@ -84,7 +108,7 @@ int mei_ioctl_connect_client(struct file *file,
 
 	cb->major_file_operations = MEI_IOCTL;
 
-	if (dev->mei_state != MEI_ENABLED) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
 		rets = -ENODEV;
 		goto end;
 	}
@@ -95,7 +119,7 @@ int mei_ioctl_connect_client(struct file *file,
 	}
 
 	/* find ME client we're trying to connect to */
-	i = mei_find_me_client_index(dev, data->in_client_uuid);
+	i = mei_me_cl_by_uuid(dev, &data->in_client_uuid);
 	if (i >= 0 && !dev->me_clients[i].props.fixed_address) {
 		cl->me_client_id = dev->me_clients[i].client_id;
 		cl->state = MEI_FILE_CONNECTING;
@@ -273,19 +297,12 @@ int amthi_read(struct mei_device *dev, struct file *file,
 		return -ETIMEDOUT;
 	}
 
-	for (i = 0; i < dev->me_clients_num; i++) {
-		if (dev->me_clients[i].client_id ==
-		    dev->iamthif_cl.me_client_id)
-			break;
-	}
+	i = mei_me_cl_by_id(dev, dev->iamthif_cl.me_client_id);
 
-	if (i == dev->me_clients_num) {
+	if (i < 0) {
 		dev_dbg(&dev->pdev->dev, "amthi client not found.\n");
 		return -ENODEV;
 	}
-	if (WARN_ON(dev->me_clients[i].client_id != cl->me_client_id))
-		return -ENODEV;
-
 	dev_dbg(&dev->pdev->dev, "checking amthi data\n");
 	cb = find_amthi_read_list_entry(dev, file);
 
@@ -316,8 +333,7 @@ int amthi_read(struct mei_device *dev, struct file *file,
 	dev->iamthif_timer = 0;
 
 	if (cb) {
-		timeout = cb->read_time +
-					msecs_to_jiffies(IAMTHIF_READ_TIMER);
+		timeout = cb->read_time + msecs_to_jiffies(IAMTHIF_READ_TIMER);
 		dev_dbg(&dev->pdev->dev, "amthi timeout = %lud\n",
 				timeout);
 
@@ -386,7 +402,7 @@ int mei_start_read(struct mei_device *dev, struct mei_cl *cl)
 	if (cl->state != MEI_FILE_CONNECTED)
 		return -ENODEV;
 
-	if (dev->mei_state != MEI_ENABLED)
+	if (dev->dev_state != MEI_DEV_ENABLED)
 		return -ENODEV;
 
 	dev_dbg(&dev->pdev->dev, "check if read is pending.\n");
@@ -401,19 +417,8 @@ int mei_start_read(struct mei_device *dev, struct mei_cl *cl)
 
 	dev_dbg(&dev->pdev->dev, "allocation call back successful. host client = %d, ME client = %d\n",
 		cl->host_client_id, cl->me_client_id);
-
-	for (i = 0; i < dev->me_clients_num; i++) {
-		if (dev->me_clients[i].client_id == cl->me_client_id)
-			break;
-
-	}
-
-	if (WARN_ON(dev->me_clients[i].client_id != cl->me_client_id)) {
-		rets = -ENODEV;
-		goto unlock;
-	}
-
-	if (i == dev->me_clients_num) {
+	i = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (i < 0) {
 		rets = -ENODEV;
 		goto unlock;
 	}
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 7422c7652845..e8b0858132c1 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -41,11 +41,8 @@
 #include <linux/mei.h>
 #include "interface.h"
 
-static const char mei_driver_name[] = "mei";
-
-/* The device pointer */
-/* Currently this driver works as long as there is only a single AMT device. */
-struct pci_dev *mei_device;
+/* AMT device is a singleton on the platform */
+static struct pci_dev *mei_pdev;
 
 /* mei_pci_tbl - PCI Device ID Table */
 static DEFINE_PCI_DEVICE_TABLE(mei_pci_tbl) = {
@@ -80,6 +77,8 @@ static DEFINE_PCI_DEVICE_TABLE(mei_pci_tbl) = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_1)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_2)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_3)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_LP)},
 
 	/* required last entry */
 	{0, }
@@ -220,10 +219,10 @@ static int mei_open(struct inode *inode, struct file *file)
 	int err;
 
 	err = -ENODEV;
-	if (!mei_device)
+	if (!mei_pdev)
 		goto out;
 
-	dev = pci_get_drvdata(mei_device);
+	dev = pci_get_drvdata(mei_pdev);
 	if (!dev)
 		goto out;
 
@@ -234,18 +233,24 @@ static int mei_open(struct inode *inode, struct file *file)
 		goto out_unlock;
 
 	err = -ENODEV;
-	if (dev->mei_state != MEI_ENABLED) {
-		dev_dbg(&dev->pdev->dev, "mei_state != MEI_ENABLED  mei_state= %d\n",
-		    dev->mei_state);
+	if (dev->dev_state != MEI_DEV_ENABLED) {
+		dev_dbg(&dev->pdev->dev, "dev_state != MEI_ENABLED  dev_state = %s\n",
+		    mei_dev_state_str(dev->dev_state));
 		goto out_unlock;
 	}
 	err = -EMFILE;
-	if (dev->open_handle_count >= MEI_MAX_OPEN_HANDLE_COUNT)
+	if (dev->open_handle_count >= MEI_MAX_OPEN_HANDLE_COUNT) {
+		dev_err(&dev->pdev->dev, "open_handle_count exceded %d",
+			MEI_MAX_OPEN_HANDLE_COUNT);
 		goto out_unlock;
+	}
 
 	cl_id = find_first_zero_bit(dev->host_clients_map, MEI_CLIENTS_MAX);
-	if (cl_id >= MEI_CLIENTS_MAX)
+	if (cl_id >= MEI_CLIENTS_MAX) {
+		dev_err(&dev->pdev->dev, "client_id exceded %d",
+				MEI_CLIENTS_MAX) ;
 		goto out_unlock;
+	}
 
 	cl->host_client_id  = cl_id;
 
@@ -386,17 +391,16 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 	dev = cl->dev;
 
 	mutex_lock(&dev->device_lock);
-	if (dev->mei_state != MEI_ENABLED) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
 		rets = -ENODEV;
 		goto out;
 	}
 
 	if ((cl->sm_state & MEI_WD_STATE_INDEPENDENCE_MSG_SENT) == 0) {
 		/* Do not allow to read watchdog client */
-		i = mei_find_me_client_index(dev, mei_wd_guid);
+		i = mei_me_cl_by_uuid(dev, &mei_wd_guid);
 		if (i >= 0) {
 			struct mei_me_client *me_client = &dev->me_clients[i];
-
 			if (cl->me_client_id == me_client->client_id) {
 				rets = -EBADF;
 				goto out;
@@ -541,7 +545,7 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 
 	mutex_lock(&dev->device_lock);
 
-	if (dev->mei_state != MEI_ENABLED) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
 		mutex_unlock(&dev->device_lock);
 		return -ENODEV;
 	}
@@ -616,26 +620,16 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 			rets = -ENOMEM;
 			goto unlock_dev;
 		}
-		if (dev->mei_state != MEI_ENABLED) {
+		if (dev->dev_state != MEI_DEV_ENABLED) {
 			rets = -ENODEV;
 			goto unlock_dev;
 		}
-		for (i = 0; i < dev->me_clients_num; i++) {
-			if (dev->me_clients[i].client_id ==
-				dev->iamthif_cl.me_client_id)
-				break;
-		}
-
-		if (WARN_ON(dev->me_clients[i].client_id != cl->me_client_id)) {
+		i = mei_me_cl_by_id(dev, dev->iamthif_cl.me_client_id);
+		if (i < 0) {
 			rets = -ENODEV;
 			goto unlock_dev;
 		}
-		if (i == dev->me_clients_num ||
-		    (dev->me_clients[i].client_id !=
-		      dev->iamthif_cl.me_client_id)) {
-			rets = -ENODEV;
-			goto unlock_dev;
-		} else if (length > dev->me_clients[i].props.max_msg_length ||
+		if (length > dev->me_clients[i].props.max_msg_length ||
 			   length <= 0) {
 			rets = -EMSGSIZE;
 			goto unlock_dev;
@@ -688,16 +682,8 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		    cl->me_client_id);
 		goto unlock_dev;
 	}
-	for (i = 0; i < dev->me_clients_num; i++) {
-		if (dev->me_clients[i].client_id ==
-		    cl->me_client_id)
-			break;
-	}
-	if (WARN_ON(dev->me_clients[i].client_id != cl->me_client_id)) {
-		rets = -ENODEV;
-		goto unlock_dev;
-	}
-	if (i == dev->me_clients_num) {
+	i = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (i < 0) {
 		rets = -ENODEV;
 		goto unlock_dev;
 	}
@@ -790,7 +776,7 @@ static long mei_ioctl(struct file *file, unsigned int cmd, unsigned long data)
 	dev_dbg(&dev->pdev->dev, "IOCTL cmd = 0x%x", cmd);
 
 	mutex_lock(&dev->device_lock);
-	if (dev->mei_state != MEI_ENABLED) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
 		rets = -ENODEV;
 		goto out;
 	}
@@ -869,7 +855,7 @@ static unsigned int mei_poll(struct file *file, poll_table *wait)
 
 	mutex_lock(&dev->device_lock);
 
-	if (dev->mei_state != MEI_ENABLED)
+	if (dev->dev_state != MEI_DEV_ENABLED)
 		goto out;
 
 
@@ -966,7 +952,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 		goto end;
 	}
 
-	if (mei_device) {
+	if (mei_pdev) {
 		err = -EEXIST;
 		goto end;
 	}
@@ -979,7 +965,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 	/* set PCI host mastering  */
 	pci_set_master(pdev);
 	/* pci request regions for mei driver */
-	err = pci_request_regions(pdev, mei_driver_name);
+	err = pci_request_regions(pdev, KBUILD_MODNAME);
 	if (err) {
 		dev_err(&pdev->dev, "failed to get pci regions.\n");
 		goto disable_device;
@@ -1004,12 +990,12 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 		err = request_threaded_irq(pdev->irq,
 			NULL,
 			mei_interrupt_thread_handler,
-			IRQF_ONESHOT, mei_driver_name, dev);
+			IRQF_ONESHOT, KBUILD_MODNAME, dev);
 	else
 		err = request_threaded_irq(pdev->irq,
 			mei_interrupt_quick_handler,
 			mei_interrupt_thread_handler,
-			IRQF_SHARED, mei_driver_name, dev);
+			IRQF_SHARED, KBUILD_MODNAME, dev);
 
 	if (err) {
 		dev_err(&pdev->dev, "request_threaded_irq failure. irq = %d\n",
@@ -1027,7 +1013,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 	if (err)
 		goto release_irq;
 
-	mei_device = pdev;
+	mei_pdev = pdev;
 	pci_set_drvdata(pdev, dev);
 
 
@@ -1072,7 +1058,7 @@ static void __devexit mei_remove(struct pci_dev *pdev)
 {
 	struct mei_device *dev;
 
-	if (mei_device != pdev)
+	if (mei_pdev != pdev)
 		return;
 
 	dev = pci_get_drvdata(pdev);
@@ -1081,9 +1067,11 @@ static void __devexit mei_remove(struct pci_dev *pdev)
 
 	mutex_lock(&dev->device_lock);
 
-	mei_wd_stop(dev, false);
+	cancel_delayed_work(&dev->timer_work);
 
-	mei_device = NULL;
+	mei_wd_stop(dev);
+
+	mei_pdev = NULL;
 
 	if (dev->iamthif_cl.state == MEI_FILE_CONNECTED) {
 		dev->iamthif_cl.state = MEI_FILE_DISCONNECTING;
@@ -1136,12 +1124,15 @@ static int mei_pci_suspend(struct device *device)
 	if (!dev)
 		return -ENODEV;
 	mutex_lock(&dev->device_lock);
+
+	cancel_delayed_work(&dev->timer_work);
+
 	/* Stop watchdog if exists */
-	err = mei_wd_stop(dev, true);
+	err = mei_wd_stop(dev);
 	/* Set new mei state */
-	if (dev->mei_state == MEI_ENABLED ||
-	    dev->mei_state == MEI_RECOVERING_FROM_RESET) {
-		dev->mei_state = MEI_POWER_DOWN;
+	if (dev->dev_state == MEI_DEV_ENABLED ||
+	    dev->dev_state == MEI_DEV_RECOVERING_FROM_RESET) {
+		dev->dev_state = MEI_DEV_POWER_DOWN;
 		mei_reset(dev, 0);
 	}
 	mutex_unlock(&dev->device_lock);
@@ -1169,12 +1160,12 @@ static int mei_pci_resume(struct device *device)
 		err = request_threaded_irq(pdev->irq,
 			NULL,
 			mei_interrupt_thread_handler,
-			IRQF_ONESHOT, mei_driver_name, dev);
+			IRQF_ONESHOT, KBUILD_MODNAME, dev);
 	else
 		err = request_threaded_irq(pdev->irq,
 			mei_interrupt_quick_handler,
 			mei_interrupt_thread_handler,
-			IRQF_SHARED, mei_driver_name, dev);
+			IRQF_SHARED, KBUILD_MODNAME, dev);
 
 	if (err) {
 		dev_err(&pdev->dev, "request_threaded_irq failed: irq = %d.\n",
@@ -1183,7 +1174,7 @@ static int mei_pci_resume(struct device *device)
 	}
 
 	mutex_lock(&dev->device_lock);
-	dev->mei_state = MEI_POWER_UP;
+	dev->dev_state = MEI_DEV_POWER_UP;
 	mei_reset(dev, 1);
 	mutex_unlock(&dev->device_lock);
 
@@ -1201,7 +1192,7 @@ static SIMPLE_DEV_PM_OPS(mei_pm_ops, mei_pci_suspend, mei_pci_resume);
  *  PCI driver structure
  */
 static struct pci_driver mei_driver = {
-	.name = mei_driver_name,
+	.name = KBUILD_MODNAME,
 	.id_table = mei_pci_tbl,
 	.probe = mei_probe,
 	.remove = __devexit_p(mei_remove),
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index d61c4ddfc80c..adb35fb9281c 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -25,18 +25,20 @@
 /*
  * watch dog definition
  */
-#define MEI_WATCHDOG_DATA_SIZE         16
-#define MEI_START_WD_DATA_SIZE         20
-#define MEI_WD_PARAMS_SIZE             4
+#define MEI_WD_HDR_SIZE       4
+#define MEI_WD_STOP_MSG_SIZE  MEI_WD_HDR_SIZE
+#define MEI_WD_START_MSG_SIZE (MEI_WD_HDR_SIZE + 16)
+
+#define MEI_WD_DEFAULT_TIMEOUT   120  /* seconds */
+#define MEI_WD_MIN_TIMEOUT       120  /* seconds */
+#define MEI_WD_MAX_TIMEOUT     65535  /* seconds */
+
+#define MEI_WD_STOP_TIMEOUT      10 /* msecs */
+
 #define MEI_WD_STATE_INDEPENDENCE_MSG_SENT       (1 << 0)
 
 #define MEI_RD_MSG_BUF_SIZE           (128 * sizeof(u32))
 
-/*
- * MEI PCI Device object
- */
-extern struct pci_dev *mei_device;
-
 
 /*
  * AMTHI Client UUID
@@ -54,19 +56,21 @@ extern const uuid_le mei_wd_guid;
 extern const u8 mei_wd_state_independence_msg[3][4];
 
 /*
+ * Number of Maximum MEI Clients
+ */
+#define MEI_CLIENTS_MAX 256
+
+/*
  * Number of File descriptors/handles
  * that can be opened to the driver.
  *
- * Limit to 253: 255 Total Clients
+ * Limit to 253: 256 Total Clients
+ * minus internal client for MEI Bus Messags
  * minus internal client for AMTHI
  * minus internal client for Watchdog
  */
-#define  MEI_MAX_OPEN_HANDLE_COUNT	253
+#define  MEI_MAX_OPEN_HANDLE_COUNT (MEI_CLIENTS_MAX - 3)
 
-/*
- * Number of Maximum MEI Clients
- */
-#define MEI_CLIENTS_MAX 255
 
 /* File state */
 enum file_state {
@@ -78,17 +82,19 @@ enum file_state {
 };
 
 /* MEI device states */
-enum mei_states {
-	MEI_INITIALIZING = 0,
-	MEI_INIT_CLIENTS,
-	MEI_ENABLED,
-	MEI_RESETING,
-	MEI_DISABLED,
-	MEI_RECOVERING_FROM_RESET,
-	MEI_POWER_DOWN,
-	MEI_POWER_UP
+enum mei_dev_state {
+	MEI_DEV_INITIALIZING = 0,
+	MEI_DEV_INIT_CLIENTS,
+	MEI_DEV_ENABLED,
+	MEI_DEV_RESETING,
+	MEI_DEV_DISABLED,
+	MEI_DEV_RECOVERING_FROM_RESET,
+	MEI_DEV_POWER_DOWN,
+	MEI_DEV_POWER_UP
 };
 
+const char *mei_dev_state_str(int state);
+
 /* init clients states*/
 enum mei_init_clients_states {
 	MEI_START_MESSAGE = 0,
@@ -113,6 +119,12 @@ enum mei_file_transaction_states {
 	MEI_READ_COMPLETE
 };
 
+enum mei_wd_states {
+	MEI_WD_IDLE,
+	MEI_WD_RUNNING,
+	MEI_WD_STOPPING,
+};
+
 /* MEI CB */
 enum mei_cb_major_types {
 	MEI_READ = 0,
@@ -128,7 +140,7 @@ enum mei_cb_major_types {
 struct mei_message_data {
 	u32 size;
 	unsigned char *data;
-} __packed;
+};
 
 
 struct mei_cl_cb {
@@ -218,10 +230,9 @@ struct mei_device {
 	/*
 	 * mei device  states
 	 */
-	enum mei_states mei_state;
+	enum mei_dev_state dev_state;
 	enum mei_init_clients_states init_clients_state;
 	u16 init_clients_timer;
-	bool stop;
 	bool need_reset;
 
 	u32 extra_write_index;
@@ -241,12 +252,11 @@ struct mei_device {
 	bool mei_host_buffer_is_empty;
 
 	struct mei_cl wd_cl;
+	enum mei_wd_states wd_state;
 	bool wd_interface_reg;
 	bool wd_pending;
-	bool wd_stopped;
-	bool wd_bypass;	/* if false, don't refresh watchdog ME client */
-	u16 wd_timeout;	/* seconds ((wd_data[1] << 8) + wd_data[0]) */
-	unsigned char wd_data[MEI_START_WD_DATA_SIZE];
+	u16 wd_timeout;
+	unsigned char wd_data[MEI_WD_START_MSG_SIZE];
 
 
 	struct file *iamthif_file_object;
@@ -279,9 +289,10 @@ void mei_host_init_iamthif(struct mei_device *dev);
 void mei_allocate_me_clients_storage(struct mei_device *dev);
 
 
-u8 mei_find_me_client_update_filext(struct mei_device *dev,
-				struct mei_cl *priv,
-				const uuid_le *cguid, u8 client_id);
+int mei_me_cl_update_filext(struct mei_device *dev, struct mei_cl *cl,
+			const uuid_le *cguid, u8 host_client_id);
+int mei_me_cl_by_uuid(const struct mei_device *dev, const uuid_le *cuuid);
+int mei_me_cl_by_id(struct mei_device *dev, u8 client_id);
 
 /*
  * MEI IO List Functions
@@ -348,7 +359,6 @@ void mei_run_next_iamthif_cmd(struct mei_device *dev);
 
 void mei_free_cb_private(struct mei_cl_cb *priv_cb);
 
-int mei_find_me_client_index(const struct mei_device *dev, uuid_le cuuid);
 
 /*
  * Register Access Function
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index 5133fd77b91c..d96c537f046f 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -48,8 +48,8 @@ const uuid_le mei_wd_guid = UUID_LE(0x05B79A6F, 0x4628, 0x4D7F, 0x89,
 static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
 {
 	dev_dbg(&dev->pdev->dev, "wd: set timeout=%d.\n", timeout);
-	memcpy(dev->wd_data, mei_start_wd_params, MEI_WD_PARAMS_SIZE);
-	memcpy(dev->wd_data + MEI_WD_PARAMS_SIZE, &timeout, sizeof(u16));
+	memcpy(dev->wd_data, mei_start_wd_params, MEI_WD_HDR_SIZE);
+	memcpy(dev->wd_data + MEI_WD_HDR_SIZE, &timeout, sizeof(u16));
 }
 
 /**
@@ -66,10 +66,11 @@ int mei_wd_host_init(struct mei_device *dev)
 
 	/* look for WD client and connect to it */
 	dev->wd_cl.state = MEI_FILE_DISCONNECTED;
-	dev->wd_timeout = AMT_WD_DEFAULT_TIMEOUT;
+	dev->wd_timeout = MEI_WD_DEFAULT_TIMEOUT;
+	dev->wd_state = MEI_WD_IDLE;
 
 	/* find ME WD client */
-	mei_find_me_client_update_filext(dev, &dev->wd_cl,
+	mei_me_cl_update_filext(dev, &dev->wd_cl,
 				&mei_wd_guid, MEI_WD_HOST_CLIENT_ID);
 
 	dev_dbg(&dev->pdev->dev, "wd: check client\n");
@@ -108,10 +109,10 @@ int mei_wd_send(struct mei_device *dev)
 	mei_hdr->msg_complete = 1;
 	mei_hdr->reserved = 0;
 
-	if (!memcmp(dev->wd_data, mei_start_wd_params, MEI_WD_PARAMS_SIZE))
-		mei_hdr->length = MEI_START_WD_DATA_SIZE;
-	else if (!memcmp(dev->wd_data, mei_stop_wd_params, MEI_WD_PARAMS_SIZE))
-		mei_hdr->length = MEI_WD_PARAMS_SIZE;
+	if (!memcmp(dev->wd_data, mei_start_wd_params, MEI_WD_HDR_SIZE))
+		mei_hdr->length = MEI_WD_START_MSG_SIZE;
+	else if (!memcmp(dev->wd_data, mei_stop_wd_params, MEI_WD_HDR_SIZE))
+		mei_hdr->length = MEI_WD_STOP_MSG_SIZE;
 	else
 		return -EINVAL;
 
@@ -128,18 +129,17 @@ int mei_wd_send(struct mei_device *dev)
  *	-EIO when message send fails
  *	-EINVAL when invalid message is to be sent
  */
-int mei_wd_stop(struct mei_device *dev, bool preserve)
+int mei_wd_stop(struct mei_device *dev)
 {
 	int ret;
-	u16 wd_timeout = dev->wd_timeout;
 
-	cancel_delayed_work(&dev->timer_work);
-	if (dev->wd_cl.state != MEI_FILE_CONNECTED || !dev->wd_timeout)
+	if (dev->wd_cl.state != MEI_FILE_CONNECTED ||
+	    dev->wd_state != MEI_WD_RUNNING)
 		return 0;
 
-	dev->wd_timeout = 0;
-	memcpy(dev->wd_data, mei_stop_wd_params, MEI_WD_PARAMS_SIZE);
-	dev->stop = true;
+	memcpy(dev->wd_data, mei_stop_wd_params, MEI_WD_STOP_MSG_SIZE);
+
+	dev->wd_state = MEI_WD_STOPPING;
 
 	ret = mei_flow_ctrl_creds(dev, &dev->wd_cl);
 	if (ret < 0)
@@ -161,13 +161,14 @@ int mei_wd_stop(struct mei_device *dev, bool preserve)
 	} else {
 		dev->wd_pending = true;
 	}
-	dev->wd_stopped = false;
+
 	mutex_unlock(&dev->device_lock);
 
 	ret = wait_event_interruptible_timeout(dev->wait_stop_wd,
-					dev->wd_stopped, 10 * HZ);
+					dev->wd_state == MEI_WD_IDLE,
+					msecs_to_jiffies(MEI_WD_STOP_TIMEOUT));
 	mutex_lock(&dev->device_lock);
-	if (dev->wd_stopped) {
+	if (dev->wd_state == MEI_WD_IDLE) {
 		dev_dbg(&dev->pdev->dev, "wd: stop completed ret=%d.\n", ret);
 		ret = 0;
 	} else {
@@ -177,9 +178,6 @@ int mei_wd_stop(struct mei_device *dev, bool preserve)
 			"wd: stop failed to complete ret=%d.\n", ret);
 	}
 
-	if (preserve)
-		dev->wd_timeout = wd_timeout;
-
 out:
 	return ret;
 }
@@ -196,16 +194,16 @@ static int mei_wd_ops_start(struct watchdog_device *wd_dev)
 	int err = -ENODEV;
 	struct mei_device *dev;
 
-	dev = pci_get_drvdata(mei_device);
+	dev = watchdog_get_drvdata(wd_dev);
 	if (!dev)
 		return -ENODEV;
 
 	mutex_lock(&dev->device_lock);
 
-	if (dev->mei_state != MEI_ENABLED) {
+	if (dev->dev_state != MEI_DEV_ENABLED) {
 		dev_dbg(&dev->pdev->dev,
-			"wd: mei_state != MEI_ENABLED  mei_state = %d\n",
-			dev->mei_state);
+			"wd: dev_state != MEI_DEV_ENABLED  dev_state = %s\n",
+			mei_dev_state_str(dev->dev_state));
 		goto end_unlock;
 	}
 
@@ -233,13 +231,13 @@ end_unlock:
 static int mei_wd_ops_stop(struct watchdog_device *wd_dev)
 {
 	struct mei_device *dev;
-	dev = pci_get_drvdata(mei_device);
 
+	dev = watchdog_get_drvdata(wd_dev);
 	if (!dev)
 		return -ENODEV;
 
 	mutex_lock(&dev->device_lock);
-	mei_wd_stop(dev, false);
+	mei_wd_stop(dev);
 	mutex_unlock(&dev->device_lock);
 
 	return 0;
@@ -256,8 +254,8 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 {
 	int ret = 0;
 	struct mei_device *dev;
-	dev = pci_get_drvdata(mei_device);
 
+	dev = watchdog_get_drvdata(wd_dev);
 	if (!dev)
 		return -ENODEV;
 
@@ -269,6 +267,8 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 		goto end;
 	}
 
+	dev->wd_state = MEI_WD_RUNNING;
+
 	/* Check if we can send the ping to HW*/
 	if (dev->mei_host_buffer_is_empty &&
 		mei_flow_ctrl_creds(dev, &dev->wd_cl) > 0) {
@@ -309,13 +309,13 @@ end:
 static int mei_wd_ops_set_timeout(struct watchdog_device *wd_dev, unsigned int timeout)
 {
 	struct mei_device *dev;
-	dev = pci_get_drvdata(mei_device);
 
+	dev = watchdog_get_drvdata(wd_dev);
 	if (!dev)
 		return -ENODEV;
 
 	/* Check Timeout value */
-	if (timeout < AMT_WD_MIN_TIMEOUT || timeout > AMT_WD_MAX_TIMEOUT)
+	if (timeout < MEI_WD_MIN_TIMEOUT || timeout > MEI_WD_MAX_TIMEOUT)
 		return -EINVAL;
 
 	mutex_lock(&dev->device_lock);
@@ -341,37 +341,42 @@ static const struct watchdog_ops wd_ops = {
 };
 static const struct watchdog_info wd_info = {
 		.identity = INTEL_AMT_WATCHDOG_ID,
-		.options = WDIOF_KEEPALIVEPING | WDIOF_ALARMONLY,
+		.options = WDIOF_KEEPALIVEPING |
+			   WDIOF_SETTIMEOUT |
+			   WDIOF_ALARMONLY,
 };
 
 static struct watchdog_device amt_wd_dev = {
 		.info = &wd_info,
 		.ops = &wd_ops,
-		.timeout = AMT_WD_DEFAULT_TIMEOUT,
-		.min_timeout = AMT_WD_MIN_TIMEOUT,
-		.max_timeout = AMT_WD_MAX_TIMEOUT,
+		.timeout = MEI_WD_DEFAULT_TIMEOUT,
+		.min_timeout = MEI_WD_MIN_TIMEOUT,
+		.max_timeout = MEI_WD_MAX_TIMEOUT,
 };
 
 
-void  mei_watchdog_register(struct mei_device *dev)
+void mei_watchdog_register(struct mei_device *dev)
 {
-	dev_dbg(&dev->pdev->dev, "dev->wd_timeout =%d.\n", dev->wd_timeout);
-
 	if (watchdog_register_device(&amt_wd_dev)) {
 		dev_err(&dev->pdev->dev,
 			"wd: unable to register watchdog device.\n");
 		dev->wd_interface_reg = false;
-	} else {
-		dev_dbg(&dev->pdev->dev,
-			"wd: successfully register watchdog interface.\n");
-		dev->wd_interface_reg = true;
+		return;
 	}
+
+	dev_dbg(&dev->pdev->dev,
+		"wd: successfully register watchdog interface.\n");
+	dev->wd_interface_reg = true;
+	watchdog_set_drvdata(&amt_wd_dev, dev);
 }
 
 void mei_watchdog_unregister(struct mei_device *dev)
 {
-	if (dev->wd_interface_reg)
-		watchdog_unregister_device(&amt_wd_dev);
+	if (!dev->wd_interface_reg)
+		return;
+
+	watchdog_set_drvdata(&amt_wd_dev, NULL);
+	watchdog_unregister_device(&amt_wd_dev);
 	dev->wd_interface_reg = false;
 }
 
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 9fbcacd703d5..c9f20dae1855 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -699,7 +699,7 @@ static int __devinit pch_phub_probe(struct pci_dev *pdev,
 	chip->pch_phub_base_address = pci_iomap(pdev, 1, 0);
 
 
-	if (chip->pch_phub_base_address == 0) {
+	if (chip->pch_phub_base_address == NULL) {
 		dev_err(&pdev->dev, "%s : pci_iomap FAILED", __func__);
 		ret = -ENOMEM;
 		goto err_pci_iomap;
@@ -893,18 +893,7 @@ static struct pci_driver pch_phub_driver = {
 	.resume = pch_phub_resume
 };
 
-static int __init pch_phub_pci_init(void)
-{
-	return pci_register_driver(&pch_phub_driver);
-}
-
-static void __exit pch_phub_pci_exit(void)
-{
-	pci_unregister_driver(&pch_phub_driver);
-}
-
-module_init(pch_phub_pci_init);
-module_exit(pch_phub_pci_exit);
+module_pci_driver(pch_phub_driver);
 
 MODULE_DESCRIPTION("Intel EG20T PCH/LAPIS Semiconductor IOH(ML7213/ML7223) PHUB");
 MODULE_LICENSE("GPL");
diff --git a/drivers/misc/pti.c b/drivers/misc/pti.c
index b7eb545394b1..4999b34b7a60 100644
--- a/drivers/misc/pti.c
+++ b/drivers/misc/pti.c
@@ -60,7 +60,7 @@ struct pti_tty {
 };
 
 struct pti_dev {
-	struct tty_port port;
+	struct tty_port port[PTITTY_MINOR_NUM];
 	unsigned long pti_addr;
 	unsigned long aperture_base;
 	void __iomem *pti_ioaddr;
@@ -76,7 +76,7 @@ struct pti_dev {
  */
 static DEFINE_MUTEX(alloclock);
 
-static struct pci_device_id pci_ids[] __devinitconst = {
+static const struct pci_device_id pci_ids[] __devinitconst = {
 		{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x82B)},
 		{0}
 };
@@ -393,25 +393,6 @@ void pti_writedata(struct pti_masterchannel *mc, u8 *buf, int count)
 }
 EXPORT_SYMBOL_GPL(pti_writedata);
 
-/**
- * pti_pci_remove()- Driver exit method to remove PTI from
- *		   PCI bus.
- * @pdev: variable containing pci info of PTI.
- */
-static void __devexit pti_pci_remove(struct pci_dev *pdev)
-{
-	struct pti_dev *drv_data;
-
-	drv_data = pci_get_drvdata(pdev);
-	if (drv_data != NULL) {
-		pci_iounmap(pdev, drv_data->pti_ioaddr);
-		pci_set_drvdata(pdev, NULL);
-		kfree(drv_data);
-		pci_release_region(pdev, 1);
-		pci_disable_device(pdev);
-	}
-}
-
 /*
  * for the tty_driver_*() basic function descriptions, see tty_driver.h.
  * Specific header comments made for PTI-related specifics.
@@ -446,7 +427,7 @@ static int pti_tty_driver_open(struct tty_struct *tty, struct file *filp)
 	 * also removes a locking requirement for the actual write
 	 * procedure.
 	 */
-	return tty_port_open(&drv_data->port, tty, filp);
+	return tty_port_open(tty->port, tty, filp);
 }
 
 /**
@@ -462,7 +443,7 @@ static int pti_tty_driver_open(struct tty_struct *tty, struct file *filp)
  */
 static void pti_tty_driver_close(struct tty_struct *tty, struct file *filp)
 {
-	tty_port_close(&drv_data->port, tty, filp);
+	tty_port_close(tty->port, tty, filp);
 }
 
 /**
@@ -818,6 +799,7 @@ static const struct tty_port_operations tty_port_ops = {
 static int __devinit pti_pci_probe(struct pci_dev *pdev,
 		const struct pci_device_id *ent)
 {
+	unsigned int a;
 	int retval = -EINVAL;
 	int pci_bar = 1;
 
@@ -830,7 +812,7 @@ static int __devinit pti_pci_probe(struct pci_dev *pdev,
 			__func__, __LINE__);
 		pr_err("%s(%d): Error value returned: %d\n",
 			__func__, __LINE__, retval);
-		return retval;
+		goto err;
 	}
 
 	retval = pci_enable_device(pdev);
@@ -838,17 +820,16 @@ static int __devinit pti_pci_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev,
 			"%s: pci_enable_device() returned error %d\n",
 			__func__, retval);
-		return retval;
+		goto err_unreg_misc;
 	}
 
 	drv_data = kzalloc(sizeof(*drv_data), GFP_KERNEL);
-
 	if (drv_data == NULL) {
 		retval = -ENOMEM;
 		dev_err(&pdev->dev,
 			"%s(%d): kmalloc() returned NULL memory.\n",
 			__func__, __LINE__);
-		return retval;
+		goto err_disable_pci;
 	}
 	drv_data->pti_addr = pci_resource_start(pdev, pci_bar);
 
@@ -857,33 +838,65 @@ static int __devinit pti_pci_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev,
 			"%s(%d): pci_request_region() returned error %d\n",
 			__func__, __LINE__, retval);
-		kfree(drv_data);
-		return retval;
+		goto err_free_dd;
 	}
 	drv_data->aperture_base = drv_data->pti_addr+APERTURE_14;
 	drv_data->pti_ioaddr =
 		ioremap_nocache((u32)drv_data->aperture_base,
 		APERTURE_LEN);
 	if (!drv_data->pti_ioaddr) {
-		pci_release_region(pdev, pci_bar);
 		retval = -ENOMEM;
-		kfree(drv_data);
-		return retval;
+		goto err_rel_reg;
 	}
 
 	pci_set_drvdata(pdev, drv_data);
 
-	tty_port_init(&drv_data->port);
-	drv_data->port.ops = &tty_port_ops;
+	for (a = 0; a < PTITTY_MINOR_NUM; a++) {
+		struct tty_port *port = &drv_data->port[a];
+		tty_port_init(port);
+		port->ops = &tty_port_ops;
 
-	tty_register_device(pti_tty_driver, 0, &pdev->dev);
-	tty_register_device(pti_tty_driver, 1, &pdev->dev);
+		tty_port_register_device(port, pti_tty_driver, a, &pdev->dev);
+	}
 
 	register_console(&pti_console);
 
+	return 0;
+err_rel_reg:
+	pci_release_region(pdev, pci_bar);
+err_free_dd:
+	kfree(drv_data);
+err_disable_pci:
+	pci_disable_device(pdev);
+err_unreg_misc:
+	misc_deregister(&pti_char_driver);
+err:
 	return retval;
 }
 
+/**
+ * pti_pci_remove()- Driver exit method to remove PTI from
+ *		   PCI bus.
+ * @pdev: variable containing pci info of PTI.
+ */
+static void __devexit pti_pci_remove(struct pci_dev *pdev)
+{
+	struct pti_dev *drv_data = pci_get_drvdata(pdev);
+
+	unregister_console(&pti_console);
+
+	tty_unregister_device(pti_tty_driver, 0);
+	tty_unregister_device(pti_tty_driver, 1);
+
+	iounmap(drv_data->pti_ioaddr);
+	pci_set_drvdata(pdev, NULL);
+	kfree(drv_data);
+	pci_release_region(pdev, 1);
+	pci_disable_device(pdev);
+
+	misc_deregister(&pti_char_driver);
+}
+
 static struct pci_driver pti_pci_driver = {
 	.name		= PCINAME,
 	.id_table	= pci_ids,
@@ -933,25 +946,24 @@ static int __init pti_init(void)
 		pr_err("%s(%d): Error value returned: %d\n",
 			__func__, __LINE__, retval);
 
-		pti_tty_driver = NULL;
-		return retval;
+		goto put_tty;
 	}
 
 	retval = pci_register_driver(&pti_pci_driver);
-
 	if (retval) {
 		pr_err("%s(%d): PCI registration failed of pti driver\n",
 			__func__, __LINE__);
 		pr_err("%s(%d): Error value returned: %d\n",
 			__func__, __LINE__, retval);
-
-		tty_unregister_driver(pti_tty_driver);
-		pr_err("%s(%d): Unregistering TTY part of pti driver\n",
-			__func__, __LINE__);
-		pti_tty_driver = NULL;
-		return retval;
+		goto unreg_tty;
 	}
 
+	return 0;
+unreg_tty:
+	tty_unregister_driver(pti_tty_driver);
+put_tty:
+	put_tty_driver(pti_tty_driver);
+	pti_tty_driver = NULL;
 	return retval;
 }
 
@@ -960,31 +972,9 @@ static int __init pti_init(void)
  */
 static void __exit pti_exit(void)
 {
-	int retval;
-
-	tty_unregister_device(pti_tty_driver, 0);
-	tty_unregister_device(pti_tty_driver, 1);
-
-	retval = tty_unregister_driver(pti_tty_driver);
-	if (retval) {
-		pr_err("%s(%d): TTY unregistration failed of pti driver\n",
-			__func__, __LINE__);
-		pr_err("%s(%d): Error value returned: %d\n",
-			__func__, __LINE__, retval);
-	}
-
+	tty_unregister_driver(pti_tty_driver);
 	pci_unregister_driver(&pti_pci_driver);
-
-	retval = misc_deregister(&pti_char_driver);
-	if (retval) {
-		pr_err("%s(%d): CHAR unregistration failed of pti driver\n",
-			__func__, __LINE__);
-		pr_err("%s(%d): Error value returned: %d\n",
-			__func__, __LINE__, retval);
-	}
-
-	unregister_console(&pti_console);
-	return;
+	put_tty_driver(pti_tty_driver);
 }
 
 module_init(pti_init);
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index acfaeeb9e01a..46937b107261 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -30,11 +30,13 @@
 
 #include <linux/ti_wilink_st.h>
 
+extern void st_kim_recv(void *, const unsigned char *, long);
+void st_int_recv(void *, const unsigned char *, long);
 /* function pointer pointing to either,
  * st_kim_recv during registration to receive fw download responses
  * st_int_recv after registration to receive proto stack responses
  */
-void (*st_recv) (void*, const unsigned char*, long);
+static void (*st_recv) (void *, const unsigned char *, long);
 
 /********************************************************************/
 static void add_channel_to_table(struct st_data_s *st_gdata,
@@ -100,7 +102,7 @@ int st_int_write(struct st_data_s *st_gdata,
  * push the skb received to relevant
  * protocol stacks
  */
-void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
+static void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
 {
 	pr_debug(" %s(prot:%d) ", __func__, chnl_id);
 
@@ -140,7 +142,7 @@ void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
  * This function is being called with spin lock held, protocol drivers are
  * only expected to complete their waits and do nothing more than that.
  */
-void st_reg_complete(struct st_data_s *st_gdata, char err)
+static void st_reg_complete(struct st_data_s *st_gdata, char err)
 {
 	unsigned char i = 0;
 	pr_info(" %s ", __func__);
@@ -379,7 +381,7 @@ done:
  *	completely, return that skb which has the pending data.
  *	In normal cases, return top of txq.
  */
-struct sk_buff *st_int_dequeue(struct st_data_s *st_gdata)
+static struct sk_buff *st_int_dequeue(struct st_data_s *st_gdata)
 {
 	struct sk_buff *returning_skb;
 
@@ -401,7 +403,7 @@ struct sk_buff *st_int_dequeue(struct st_data_s *st_gdata)
  *	txq and waitq needs protection since the other contexts
  *	may be sending data, waking up chip.
  */
-void st_int_enqueue(struct st_data_s *st_gdata, struct sk_buff *skb)
+static void st_int_enqueue(struct st_data_s *st_gdata, struct sk_buff *skb)
 {
 	unsigned long flags = 0;
 
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 7c14f8fd98db..04a819944f6b 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -63,10 +63,27 @@ static struct platform_device *st_get_plat_device(int id)
  *	in case of error don't complete so that waiting for proper
  *	response times out
  */
-void validate_firmware_response(struct kim_data_s *kim_gdata)
+static void validate_firmware_response(struct kim_data_s *kim_gdata)
 {
 	struct sk_buff *skb = kim_gdata->rx_skb;
-	if (unlikely(skb->data[5] != 0)) {
+	if (!skb)
+		return;
+
+	/* these magic numbers are the position in the response buffer which
+	 * allows us to distinguish whether the response is for the read
+	 * version info. command
+	 */
+	if (skb->data[2] == 0x01 && skb->data[3] == 0x01 &&
+			skb->data[4] == 0x10 && skb->data[5] == 0x00) {
+		/* fw version response */
+		memcpy(kim_gdata->resp_buffer,
+				kim_gdata->rx_skb->data,
+				kim_gdata->rx_skb->len);
+		complete_all(&kim_gdata->kim_rcvd);
+		kim_gdata->rx_state = ST_W4_PACKET_TYPE;
+		kim_gdata->rx_skb = NULL;
+		kim_gdata->rx_count = 0;
+	} else if (unlikely(skb->data[5] != 0)) {
 		pr_err("no proper response during fw download");
 		pr_err("data6 %x", skb->data[5]);
 		kfree_skb(skb);
@@ -119,7 +136,7 @@ static inline int kim_check_data_len(struct kim_data_s *kim_gdata, int len)
  *	have been observed to come in bursts of different
  *	tty_receive and hence the logic
  */
-void kim_int_recv(struct kim_data_s *kim_gdata,
+static void kim_int_recv(struct kim_data_s *kim_gdata,
 	const unsigned char *data, long count)
 {
 	const unsigned char *ptr;
@@ -207,16 +224,19 @@ static long read_local_version(struct kim_data_s *kim_gdata, char *bts_scr_name)
 		return -EIO;
 	}
 
-	if (!wait_for_completion_timeout
-	    (&kim_gdata->kim_rcvd, msecs_to_jiffies(CMD_RESP_TIME))) {
+	if (!wait_for_completion_interruptible_timeout(
+		&kim_gdata->kim_rcvd, msecs_to_jiffies(CMD_RESP_TIME))) {
 		pr_err(" waiting for ver info- timed out ");
 		return -ETIMEDOUT;
 	}
 	INIT_COMPLETION(kim_gdata->kim_rcvd);
+	/* the positions 12 & 13 in the response buffer provide with the
+	 * chip, major & minor numbers
+	 */
 
 	version =
-		MAKEWORD(kim_gdata->resp_buffer[13],
-				kim_gdata->resp_buffer[14]);
+		MAKEWORD(kim_gdata->resp_buffer[12],
+				kim_gdata->resp_buffer[13]);
 	chip = (version & 0x7C00) >> 10;
 	min_ver = (version & 0x007F);
 	maj_ver = (version & 0x0380) >> 7;
@@ -236,7 +256,7 @@ static long read_local_version(struct kim_data_s *kim_gdata, char *bts_scr_name)
 	return 0;
 }
 
-void skip_change_remote_baud(unsigned char **ptr, long *len)
+static void skip_change_remote_baud(unsigned char **ptr, long *len)
 {
 	unsigned char *nxt_action, *cur_action;
 	cur_action = *ptr;
@@ -370,9 +390,9 @@ static long download_firmware(struct kim_data_s *kim_gdata)
 			break;
 		case ACTION_WAIT_EVENT:  /* wait */
 			pr_debug("W");
-			if (!wait_for_completion_timeout
-					(&kim_gdata->kim_rcvd,
-					 msecs_to_jiffies(CMD_RESP_TIME))) {
+			if (!wait_for_completion_interruptible_timeout(
+					&kim_gdata->kim_rcvd,
+					msecs_to_jiffies(CMD_RESP_TIME))) {
 				pr_err("response timeout during fw download ");
 				/* timed out */
 				release_firmware(kim_gdata->fw_entry);
@@ -410,16 +430,10 @@ void st_kim_recv(void *disc_data, const unsigned char *data, long count)
 	struct st_data_s	*st_gdata = (struct st_data_s *)disc_data;
 	struct kim_data_s	*kim_gdata = st_gdata->kim_data;
 
-	/* copy to local buffer */
-	if (unlikely(data[4] == 0x01 && data[5] == 0x10 && data[0] == 0x04)) {
-		/* must be the read_ver_cmd */
-		memcpy(kim_gdata->resp_buffer, data, count);
-		complete_all(&kim_gdata->kim_rcvd);
-		return;
-	} else {
-		kim_int_recv(kim_gdata, data, count);
-		/* either completes or times out */
-	}
+	/* proceed to gather all data and distinguish read fw version response
+	 * from other fw responses when data gathering is complete
+	 */
+	kim_int_recv(kim_gdata, data, count);
 	return;
 }
 
@@ -454,11 +468,6 @@ long st_kim_start(void *kim_data)
 		if (pdata->chip_enable)
 			pdata->chip_enable(kim_gdata);
 
-		/* Configure BT nShutdown to HIGH state */
-		gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
-		mdelay(5);	/* FIXME: a proper toggle */
-		gpio_set_value(kim_gdata->nshutdown, GPIO_HIGH);
-		mdelay(100);
 		/* re-initialize the completion */
 		INIT_COMPLETION(kim_gdata->ldisc_installed);
 		/* send notification to UIM */
@@ -467,8 +476,8 @@ long st_kim_start(void *kim_data)
 		sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
 				NULL, "install");
 		/* wait for ldisc to be installed */
-		err = wait_for_completion_timeout(&kim_gdata->ldisc_installed,
-				msecs_to_jiffies(LDISC_TIME));
+		err = wait_for_completion_interruptible_timeout(
+			&kim_gdata->ldisc_installed, msecs_to_jiffies(LDISC_TIME));
 		if (!err) {
 			/* ldisc installation timeout,
 			 * flush uart, power cycle BT_EN */
@@ -500,8 +509,7 @@ long st_kim_start(void *kim_data)
  *	(b) upon failure to either install ldisc or download firmware.
  *	The function is responsible to (a) notify UIM about un-installation,
  *	(b) flush UART if the ldisc was installed.
- *	(c) reset BT_EN - pull down nshutdown at the end.
- *	(d) invoke platform's chip disabling routine.
+ *	(c) invoke platform's chip disabling routine.
  */
 long st_kim_stop(void *kim_data)
 {
@@ -526,20 +534,13 @@ long st_kim_stop(void *kim_data)
 	sysfs_notify(&kim_gdata->kim_pdev->dev.kobj, NULL, "install");
 
 	/* wait for ldisc to be un-installed */
-	err = wait_for_completion_timeout(&kim_gdata->ldisc_installed,
-			msecs_to_jiffies(LDISC_TIME));
+	err = wait_for_completion_interruptible_timeout(
+		&kim_gdata->ldisc_installed, msecs_to_jiffies(LDISC_TIME));
 	if (!err) {		/* timeout */
 		pr_err(" timed out waiting for ldisc to be un-installed");
-		return -ETIMEDOUT;
+		err = -ETIMEDOUT;
 	}
 
-	/* By default configure BT nShutdown to LOW state */
-	gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
-	mdelay(1);
-	gpio_set_value(kim_gdata->nshutdown, GPIO_HIGH);
-	mdelay(1);
-	gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
-
 	/* platform specific disable */
 	if (pdata->chip_disable)
 		pdata->chip_disable(kim_gdata);
@@ -701,7 +702,7 @@ static const struct file_operations list_debugfs_fops = {
  * board-*.c file
  */
 
-struct dentry *kim_debugfs_dir;
+static struct dentry *kim_debugfs_dir;
 static int kim_probe(struct platform_device *pdev)
 {
 	long status;
@@ -731,20 +732,6 @@ static int kim_probe(struct platform_device *pdev)
 	/* refer to itself */
 	kim_gdata->core_data->kim_data = kim_gdata;
 
-	/* Claim the chip enable nShutdown gpio from the system */
-	kim_gdata->nshutdown = pdata->nshutdown_gpio;
-	status = gpio_request(kim_gdata->nshutdown, "kim");
-	if (unlikely(status)) {
-		pr_err(" gpio %ld request failed ", kim_gdata->nshutdown);
-		return status;
-	}
-
-	/* Configure nShutdown GPIO as output=0 */
-	status = gpio_direction_output(kim_gdata->nshutdown, 0);
-	if (unlikely(status)) {
-		pr_err(" unable to configure gpio %ld", kim_gdata->nshutdown);
-		return status;
-	}
 	/* get reference of pdev for request_firmware
 	 */
 	kim_gdata->kim_pdev = pdev;
@@ -780,18 +767,10 @@ static int kim_probe(struct platform_device *pdev)
 
 static int kim_remove(struct platform_device *pdev)
 {
-	/* free the GPIOs requested */
-	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
 	struct kim_data_s	*kim_gdata;
 
 	kim_gdata = dev_get_drvdata(&pdev->dev);
 
-	/* Free the Bluetooth/FM/GPIO
-	 * nShutdown gpio from the system
-	 */
-	gpio_free(pdata->nshutdown_gpio);
-	pr_info("nshutdown GPIO Freed");
-
 	debugfs_remove_recursive(kim_debugfs_dir);
 	sysfs_remove_group(&pdev->dev.kobj, &uim_attr_grp);
 	pr_info("sysfs entries removed");
@@ -804,7 +783,7 @@ static int kim_remove(struct platform_device *pdev)
 	return 0;
 }
 
-int kim_suspend(struct platform_device *pdev, pm_message_t state)
+static int kim_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
 
@@ -814,7 +793,7 @@ int kim_suspend(struct platform_device *pdev, pm_message_t state)
 	return -EOPNOTSUPP;
 }
 
-int kim_resume(struct platform_device *pdev)
+static int kim_resume(struct platform_device *pdev)
 {
 	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
 
diff --git a/drivers/misc/tifm_7xx1.c b/drivers/misc/tifm_7xx1.c
index ba2479022670..f8d6654391e5 100644
--- a/drivers/misc/tifm_7xx1.c
+++ b/drivers/misc/tifm_7xx1.c
@@ -434,21 +434,9 @@ static struct pci_driver tifm_7xx1_driver = {
 	.resume = tifm_7xx1_resume,
 };
 
-static int __init tifm_7xx1_init(void)
-{
-	return pci_register_driver(&tifm_7xx1_driver);
-}
-
-static void __exit tifm_7xx1_exit(void)
-{
-	pci_unregister_driver(&tifm_7xx1_driver);
-}
-
+module_pci_driver(tifm_7xx1_driver);
 MODULE_AUTHOR("Alex Dubov");
 MODULE_DESCRIPTION("TI FlashMedia host driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, tifm_7xx1_pci_tbl);
 MODULE_VERSION(DRIVER_VERSION);
-
-module_init(tifm_7xx1_init);
-module_exit(tifm_7xx1_exit);
diff --git a/drivers/mmc/card/sdio_uart.c b/drivers/mmc/card/sdio_uart.c
index 5a2cbfac66d2..d2339ea37815 100644
--- a/drivers/mmc/card/sdio_uart.c
+++ b/drivers/mmc/card/sdio_uart.c
@@ -518,7 +518,7 @@ static void sdio_uart_check_modem_status(struct sdio_uart_port *port)
 	if (status & UART_MSR_DCTS) {
 		port->icount.cts++;
 		tty = tty_port_tty_get(&port->port);
-		if (tty && (tty->termios->c_cflag & CRTSCTS)) {
+		if (tty && (tty->termios.c_cflag & CRTSCTS)) {
 			int cts = (status & UART_MSR_CTS);
 			if (tty->hw_stopped) {
 				if (cts) {
@@ -671,12 +671,12 @@ static int sdio_uart_activate(struct tty_port *tport, struct tty_struct *tty)
 	port->ier = UART_IER_RLSI|UART_IER_RDI|UART_IER_RTOIE|UART_IER_UUE;
 	port->mctrl = TIOCM_OUT2;
 
-	sdio_uart_change_speed(port, tty->termios, NULL);
+	sdio_uart_change_speed(port, &tty->termios, NULL);
 
-	if (tty->termios->c_cflag & CBAUD)
+	if (tty->termios.c_cflag & CBAUD)
 		sdio_uart_set_mctrl(port, TIOCM_RTS | TIOCM_DTR);
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		if (!(sdio_uart_get_mctrl(port) & TIOCM_CTS))
 			tty->hw_stopped = 1;
 
@@ -850,7 +850,7 @@ static void sdio_uart_throttle(struct tty_struct *tty)
 {
 	struct sdio_uart_port *port = tty->driver_data;
 
-	if (!I_IXOFF(tty) && !(tty->termios->c_cflag & CRTSCTS))
+	if (!I_IXOFF(tty) && !(tty->termios.c_cflag & CRTSCTS))
 		return;
 
 	if (sdio_uart_claim_func(port) != 0)
@@ -861,7 +861,7 @@ static void sdio_uart_throttle(struct tty_struct *tty)
 		sdio_uart_start_tx(port);
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		sdio_uart_clear_mctrl(port, TIOCM_RTS);
 
 	sdio_uart_irq(port->func);
@@ -872,7 +872,7 @@ static void sdio_uart_unthrottle(struct tty_struct *tty)
 {
 	struct sdio_uart_port *port = tty->driver_data;
 
-	if (!I_IXOFF(tty) && !(tty->termios->c_cflag & CRTSCTS))
+	if (!I_IXOFF(tty) && !(tty->termios.c_cflag & CRTSCTS))
 		return;
 
 	if (sdio_uart_claim_func(port) != 0)
@@ -887,7 +887,7 @@ static void sdio_uart_unthrottle(struct tty_struct *tty)
 		}
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		sdio_uart_set_mctrl(port, TIOCM_RTS);
 
 	sdio_uart_irq(port->func);
@@ -898,12 +898,12 @@ static void sdio_uart_set_termios(struct tty_struct *tty,
 						struct ktermios *old_termios)
 {
 	struct sdio_uart_port *port = tty->driver_data;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 
 	if (sdio_uart_claim_func(port) != 0)
 		return;
 
-	sdio_uart_change_speed(port, tty->termios, old_termios);
+	sdio_uart_change_speed(port, &tty->termios, old_termios);
 
 	/* Handle transition to B0 status */
 	if ((old_termios->c_cflag & CBAUD) && !(cflag & CBAUD))
@@ -1132,8 +1132,8 @@ static int sdio_uart_probe(struct sdio_func *func,
 		kfree(port);
 	} else {
 		struct device *dev;
-		dev = tty_register_device(sdio_uart_tty_driver,
-						port->index, &func->dev);
+		dev = tty_port_register_device(&port->port,
+				sdio_uart_tty_driver, port->index, &func->dev);
 		if (IS_ERR(dev)) {
 			sdio_uart_port_remove(port);
 			ret = PTR_ERR(dev);
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index d4619e2ec030..2273ce6b6c1a 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -641,7 +641,7 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 	/*
 	 * If the host and card support UHS-I mode request the card
 	 * to switch to 1.8V signaling level.  No 1.8v signalling if
-	 * UHS mode is not enabled to maintain compatibilty and some
+	 * UHS mode is not enabled to maintain compatibility and some
 	 * systems that claim 1.8v signalling in fact do not support
 	 * it.
 	 */
diff --git a/drivers/mmc/host/at91_mci.c b/drivers/mmc/host/at91_mci.c
index efdb81d21c44..74bed0fc23e7 100644
--- a/drivers/mmc/host/at91_mci.c
+++ b/drivers/mmc/host/at91_mci.c
@@ -356,7 +356,7 @@ static void at91_mci_handle_transmitted(struct at91mci_host *host)
 }
 
 /*
- * Update bytes tranfered count during a write operation
+ * Update bytes transfered count during a write operation
  */
 static void at91_mci_update_bytes_xfered(struct at91mci_host *host)
 {
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index a53c7c478e05..852d5fbda630 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -1022,7 +1022,7 @@ static void atmci_stop_transfer(struct atmel_mci *host)
 }
 
 /*
- * Stop data transfer because error(s) occured.
+ * Stop data transfer because error(s) occurred.
  */
 static void atmci_stop_transfer_pdc(struct atmel_mci *host)
 {
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 3a09f93cc3b6..686e256764c8 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -447,7 +447,7 @@ static void omap_hsmmc_stop_clock(struct omap_hsmmc_host *host)
 	OMAP_HSMMC_WRITE(host->base, SYSCTL,
 		OMAP_HSMMC_READ(host->base, SYSCTL) & ~CEN);
 	if ((OMAP_HSMMC_READ(host->base, SYSCTL) & CEN) != 0x0)
-		dev_dbg(mmc_dev(host->mmc), "MMC Clock is not stoped\n");
+		dev_dbg(mmc_dev(host->mmc), "MMC Clock is not stopped\n");
 }
 
 static void omap_hsmmc_enable_irq(struct omap_hsmmc_host *host,
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index e23f8134591c..32f4a070551f 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -315,7 +315,7 @@ static void esdhc_writeb_le(struct sdhci_host *host, u8 val, int reg)
 		new_val = val & (SDHCI_CTRL_LED | \
 				SDHCI_CTRL_4BITBUS | \
 				SDHCI_CTRL_D3CD);
-		/* ensure the endianess */
+		/* ensure the endianness */
 		new_val |= ESDHC_HOST_CONTROL_LE;
 		/* bits 8&9 are reserved on mx25 */
 		if (!is_imx25_esdhc(imx_data)) {
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index 3135a1a5d75d..58eab9ac1d01 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -806,7 +806,7 @@ static void command_res_completed(struct urb *urb)
 		 * we suspect a buggy USB host controller
 		 */
 	} else if (!vub300->data) {
-		/* this means that the command (typically CMD52) suceeded */
+		/* this means that the command (typically CMD52) succeeded */
 	} else if (vub300->resp.common.header_type != 0x02) {
 		/*
 		 * this is an error response from the VUB300 chip
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index f2f482bec573..a6e74514e662 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -1123,6 +1123,33 @@ static unsigned long mtdchar_get_unmapped_area(struct file *file,
 }
 #endif
 
+static inline unsigned long get_vm_size(struct vm_area_struct *vma)
+{
+	return vma->vm_end - vma->vm_start;
+}
+
+static inline resource_size_t get_vm_offset(struct vm_area_struct *vma)
+{
+	return (resource_size_t) vma->vm_pgoff << PAGE_SHIFT;
+}
+
+/*
+ * Set a new vm offset.
+ *
+ * Verify that the incoming offset really works as a page offset,
+ * and that the offset and size fit in a resource_size_t.
+ */
+static inline int set_vm_offset(struct vm_area_struct *vma, resource_size_t off)
+{
+	pgoff_t pgoff = off >> PAGE_SHIFT;
+	if (off != (resource_size_t) pgoff << PAGE_SHIFT)
+		return -EINVAL;
+	if (off + get_vm_size(vma) - 1 < off)
+		return -EINVAL;
+	vma->vm_pgoff = pgoff;
+	return 0;
+}
+
 /*
  * set up a mapping for shared memory segments
  */
@@ -1132,20 +1159,29 @@ static int mtdchar_mmap(struct file *file, struct vm_area_struct *vma)
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
 	struct map_info *map = mtd->priv;
-	unsigned long start;
-	unsigned long off;
-	u32 len;
+	resource_size_t start, off;
+	unsigned long len, vma_len;
 
 	if (mtd->type == MTD_RAM || mtd->type == MTD_ROM) {
-		off = vma->vm_pgoff << PAGE_SHIFT;
+		off = get_vm_offset(vma);
 		start = map->phys;
 		len = PAGE_ALIGN((start & ~PAGE_MASK) + map->size);
 		start &= PAGE_MASK;
-		if ((vma->vm_end - vma->vm_start + off) > len)
+		vma_len = get_vm_size(vma);
+
+		/* Overflow in off+len? */
+		if (vma_len + off < off)
+			return -EINVAL;
+		/* Does it fit in the mapping? */
+		if (vma_len + off > len)
 			return -EINVAL;
 
 		off += start;
-		vma->vm_pgoff = off >> PAGE_SHIFT;
+		/* Did that overflow? */
+		if (off < start)
+			return -EINVAL;
+		if (set_vm_offset(vma, off) < 0)
+			return -EINVAL;
 		vma->vm_flags |= VM_IO | VM_RESERVED;
 
 #ifdef pgprot_noncached
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 98ee43819911..7edadee487ba 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -1391,7 +1391,6 @@ static irqreturn_t ican3_irq(int irq, void *dev_id)
  */
 static int ican3_reset_module(struct ican3_dev *mod)
 {
-	u8 val = 1 << mod->num;
 	unsigned long start;
 	u8 runold, runnew;
 
@@ -1405,8 +1404,7 @@ static int ican3_reset_module(struct ican3_dev *mod)
 	runold = ioread8(mod->dpm + TARGET_RUNNING);
 
 	/* reset the module */
-	iowrite8(val, &mod->ctrl->reset_assert);
-	iowrite8(val, &mod->ctrl->reset_deassert);
+	iowrite8(0x00, &mod->dpmctrl->hwreset);
 
 	/* wait until the module has finished resetting and is running */
 	start = jiffies;
diff --git a/drivers/net/can/ti_hecc.c b/drivers/net/can/ti_hecc.c
index 527dbcf95335..9ded21e79db5 100644
--- a/drivers/net/can/ti_hecc.c
+++ b/drivers/net/can/ti_hecc.c
@@ -984,12 +984,12 @@ static int __devexit ti_hecc_remove(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct ti_hecc_priv *priv = netdev_priv(ndev);
 
+	unregister_candev(ndev);
 	clk_disable(priv->clk);
 	clk_put(priv->clk);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	iounmap(priv->base);
 	release_mem_region(res->start, resource_size(res));
-	unregister_candev(ndev);
 	free_candev(ndev);
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/net/ethernet/3com/typhoon.c b/drivers/net/ethernet/3com/typhoon.c
index b15366635147..bb9670f29b59 100644
--- a/drivers/net/ethernet/3com/typhoon.c
+++ b/drivers/net/ethernet/3com/typhoon.c
@@ -364,7 +364,7 @@ typhoon_inc_rxfree_index(u32 *index, const int count)
 static inline void
 typhoon_inc_tx_index(u32 *index, const int count)
 {
-	/* if we start using the Hi Tx ring, this needs updateing */
+	/* if we start using the Hi Tx ring, this needs updating */
 	typhoon_inc_index(index, count, TXLO_ENTRIES);
 }
 
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 1bf5bbfe778e..55a2e3795055 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -149,7 +149,7 @@ static void atl1c_reset_pcie(struct atl1c_hw *hw, u32 flag)
 	data &= ~(PCI_ERR_UNC_DLP | PCI_ERR_UNC_FCP);
 	pci_write_config_dword(pdev, pos + PCI_ERR_UNCOR_SEVER, data);
 	/* clear error status */
-	pci_write_config_word(pdev, pci_pcie_cap(pdev) + PCI_EXP_DEVSTA,
+	pcie_capability_write_word(pdev, PCI_EXP_DEVSTA,
 			PCI_EXP_DEVSTA_NFED |
 			PCI_EXP_DEVSTA_FED |
 			PCI_EXP_DEVSTA_CED |
@@ -2685,7 +2685,7 @@ static void atl1c_io_resume(struct pci_dev *pdev)
 	netif_device_attach(netdev);
 }
 
-static struct pci_error_handlers atl1c_err_handler = {
+static const struct pci_error_handlers atl1c_err_handler = {
 	.error_detected = atl1c_io_error_detected,
 	.slot_reset = atl1c_io_slot_reset,
 	.resume = atl1c_io_resume,
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index a98acc8a956f..e213da29e73d 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -2489,7 +2489,7 @@ static void atl1e_io_resume(struct pci_dev *pdev)
 	netif_device_attach(netdev);
 }
 
-static struct pci_error_handlers atl1e_err_handler = {
+static const struct pci_error_handlers atl1e_err_handler = {
 	.error_detected = atl1e_io_error_detected,
 	.slot_reset = atl1e_io_slot_reset,
 	.resume = atl1e_io_resume,
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 79cebd8525ce..d4310700c7a7 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -8564,7 +8564,7 @@ bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	return 0;
 
 error:
-	iounmap(bp->regview);
+	pci_iounmap(pdev, bp->regview);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
@@ -8742,7 +8742,7 @@ static void bnx2_io_resume(struct pci_dev *pdev)
 	rtnl_unlock();
 }
 
-static struct pci_error_handlers bnx2_err_handler = {
+static const struct pci_error_handlers bnx2_err_handler = {
 	.error_detected	= bnx2_io_error_detected,
 	.slot_reset	= bnx2_io_slot_reset,
 	.resume		= bnx2_io_resume,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index 6d1a24acb77e..eac25236856c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -1458,7 +1458,7 @@ struct bnx2x {
 	int				fw_stats_req_sz;
 
 	/*
-	 * FW statistics data shortcut (points at the begining of
+	 * FW statistics data shortcut (points at the beginning of
 	 * fw_stats buffer + fw_stats_req_sz).
 	 */
 	struct bnx2x_fw_stats_data	*fw_stats_data;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index af20c6ee2cd9..e8e97a7d1d06 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -662,14 +662,16 @@ void bnx2x_csum_validate(struct sk_buff *skb, union eth_rx_cqe *cqe,
 				 struct bnx2x_fastpath *fp,
 				 struct bnx2x_eth_q_stats *qstats)
 {
-	/* Do nothing if no IP/L4 csum validation was done */
-
+	/* Do nothing if no L4 csum validation was done.
+	 * We do not check whether IP csum was validated. For IPv4 we assume
+	 * that if the card got as far as validating the L4 csum, it also
+	 * validated the IP csum. IPv6 has no IP csum.
+	 */
 	if (cqe->fast_path_cqe.status_flags &
-	    (ETH_FAST_PATH_RX_CQE_IP_XSUM_NO_VALIDATION_FLG |
-	     ETH_FAST_PATH_RX_CQE_L4_XSUM_NO_VALIDATION_FLG))
+	    ETH_FAST_PATH_RX_CQE_L4_XSUM_NO_VALIDATION_FLG)
 		return;
 
-	/* If both IP/L4 validation were done, check if an error was found. */
+	/* If L4 validation was done, check if an error was found. */
 
 	if (cqe->fast_path_cqe.type_error_flags &
 	    (ETH_FAST_PATH_RX_CQE_IP_BAD_XSUM_FLG |
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index 211753e01f81..e11485ca037d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -1162,14 +1162,9 @@ static int bnx2x_send_final_clnup(struct bnx2x *bp, u8 clnup_func,
 
 static u8 bnx2x_is_pcie_pending(struct pci_dev *dev)
 {
-	int pos;
 	u16 status;
 
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return false;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVSTA, &status);
+	pcie_capability_read_word(dev, PCI_EXP_DEVSTA, &status);
 	return status & PCI_EXP_DEVSTA_TRPND;
 }
 
@@ -6135,8 +6130,7 @@ static void bnx2x_init_pxp(struct bnx2x *bp)
 	u16 devctl;
 	int r_order, w_order;
 
-	pci_read_config_word(bp->pdev,
-			     pci_pcie_cap(bp->pdev) + PCI_EXP_DEVCTL, &devctl);
+	pcie_capability_read_word(bp->pdev, PCI_EXP_DEVCTL, &devctl);
 	DP(NETIF_MSG_HW, "read 0x%x from devctl\n", devctl);
 	w_order = ((devctl & PCI_EXP_DEVCTL_PAYLOAD) >> 5);
 	if (bp->mrrs == -1)
@@ -9380,7 +9374,7 @@ static int __devinit bnx2x_prev_mark_path(struct bnx2x *bp)
 
 static int __devinit bnx2x_do_flr(struct bnx2x *bp)
 {
-	int i, pos;
+	int i;
 	u16 status;
 	struct pci_dev *dev = bp->pdev;
 
@@ -9397,16 +9391,12 @@ static int __devinit bnx2x_do_flr(struct bnx2x *bp)
 		return -EINVAL;
 	}
 
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return -ENOTTY;
-
 	/* Wait for Transaction Pending bit clean */
 	for (i = 0; i < 4; i++) {
 		if (i)
 			msleep((1 << (i - 1)) * 100);
 
-		pci_read_config_word(dev, pos + PCI_EXP_DEVSTA, &status);
+		pcie_capability_read_word(dev, PCI_EXP_DEVSTA, &status);
 		if (!(status & PCI_EXP_DEVSTA_TRPND))
 			goto clear;
 	}
@@ -9831,12 +9821,13 @@ static void __devinit bnx2x_get_igu_cam_info(struct bnx2x *bp)
 	}
 
 #ifdef CONFIG_PCI_MSI
-	/*
-	 * It's expected that number of CAM entries for this functions is equal
-	 * to the number evaluated based on the MSI-X table size. We want a
-	 * harsh warning if these values are different!
+	/* Due to new PF resource allocation by MFW T7.4 and above, it's
+	 * optional that number of CAM entries will not be equal to the value
+	 * advertised in PCI.
+	 * Driver should use the minimal value of both as the actual status
+	 * block count
 	 */
-	WARN_ON(bp->igu_sb_cnt != igu_sb_cnt);
+	bp->igu_sb_cnt = min_t(int, bp->igu_sb_cnt, igu_sb_cnt);
 #endif
 
 	if (igu_sb_cnt == 0)
@@ -12166,7 +12157,7 @@ static void bnx2x_io_resume(struct pci_dev *pdev)
 	rtnl_unlock();
 }
 
-static struct pci_error_handlers bnx2x_err_handler = {
+static const struct pci_error_handlers bnx2x_err_handler = {
 	.error_detected = bnx2x_io_error_detected,
 	.slot_reset     = bnx2x_io_slot_reset,
 	.resume         = bnx2x_io_resume,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index f83e033da6da..acf2fe4ca608 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -1321,7 +1321,7 @@ void bnx2x_init_mcast_obj(struct bnx2x *bp,
  * the current command will be enqueued to the tail of the
  * pending commands list.
  *
- * Return: 0 is operation was sucessfull and there are no pending completions,
+ * Return: 0 is operation was successfull and there are no pending completions,
  *         negative if there were errors, positive if there are pending
  *         completions.
  */
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index bf906c51d82a..388d32213937 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -3653,17 +3653,9 @@ static int tg3_power_down_prepare(struct tg3 *tp)
 	tg3_enable_register_access(tp);
 
 	/* Restore the CLKREQ setting. */
-	if (tg3_flag(tp, CLKREQ_BUG)) {
-		u16 lnkctl;
-
-		pci_read_config_word(tp->pdev,
-				     pci_pcie_cap(tp->pdev) + PCI_EXP_LNKCTL,
-				     &lnkctl);
-		lnkctl |= PCI_EXP_LNKCTL_CLKREQ_EN;
-		pci_write_config_word(tp->pdev,
-				      pci_pcie_cap(tp->pdev) + PCI_EXP_LNKCTL,
-				      lnkctl);
-	}
+	if (tg3_flag(tp, CLKREQ_BUG))
+		pcie_capability_set_word(tp->pdev, PCI_EXP_LNKCTL,
+					 PCI_EXP_LNKCTL_CLKREQ_EN);
 
 	misc_host_ctrl = tr32(TG3PCI_MISC_HOST_CTRL);
 	tw32(TG3PCI_MISC_HOST_CTRL,
@@ -4434,20 +4426,13 @@ relink:
 
 	/* Prevent send BD corruption. */
 	if (tg3_flag(tp, CLKREQ_BUG)) {
-		u16 oldlnkctl, newlnkctl;
-
-		pci_read_config_word(tp->pdev,
-				     pci_pcie_cap(tp->pdev) + PCI_EXP_LNKCTL,
-				     &oldlnkctl);
 		if (tp->link_config.active_speed == SPEED_100 ||
 		    tp->link_config.active_speed == SPEED_10)
-			newlnkctl = oldlnkctl & ~PCI_EXP_LNKCTL_CLKREQ_EN;
+			pcie_capability_clear_word(tp->pdev, PCI_EXP_LNKCTL,
+						   PCI_EXP_LNKCTL_CLKREQ_EN);
 		else
-			newlnkctl = oldlnkctl | PCI_EXP_LNKCTL_CLKREQ_EN;
-		if (newlnkctl != oldlnkctl)
-			pci_write_config_word(tp->pdev,
-					      pci_pcie_cap(tp->pdev) +
-					      PCI_EXP_LNKCTL, newlnkctl);
+			pcie_capability_set_word(tp->pdev, PCI_EXP_LNKCTL,
+						 PCI_EXP_LNKCTL_CLKREQ_EN);
 	}
 
 	if (current_link_up != netif_carrier_ok(tp->dev)) {
@@ -8054,7 +8039,7 @@ static int tg3_chip_reset(struct tg3 *tp)
 
 	udelay(120);
 
-	if (tg3_flag(tp, PCI_EXPRESS) && pci_pcie_cap(tp->pdev)) {
+	if (tg3_flag(tp, PCI_EXPRESS) && pci_is_pcie(tp->pdev)) {
 		u16 val16;
 
 		if (tp->pci_chip_rev_id == CHIPREV_ID_5750_A0) {
@@ -8071,24 +8056,17 @@ static int tg3_chip_reset(struct tg3 *tp)
 		}
 
 		/* Clear the "no snoop" and "relaxed ordering" bits. */
-		pci_read_config_word(tp->pdev,
-				     pci_pcie_cap(tp->pdev) + PCI_EXP_DEVCTL,
-				     &val16);
-		val16 &= ~(PCI_EXP_DEVCTL_RELAX_EN |
-			   PCI_EXP_DEVCTL_NOSNOOP_EN);
+		val16 = PCI_EXP_DEVCTL_RELAX_EN | PCI_EXP_DEVCTL_NOSNOOP_EN;
 		/*
 		 * Older PCIe devices only support the 128 byte
 		 * MPS setting.  Enforce the restriction.
 		 */
 		if (!tg3_flag(tp, CPMU_PRESENT))
-			val16 &= ~PCI_EXP_DEVCTL_PAYLOAD;
-		pci_write_config_word(tp->pdev,
-				      pci_pcie_cap(tp->pdev) + PCI_EXP_DEVCTL,
-				      val16);
+			val16 |= PCI_EXP_DEVCTL_PAYLOAD;
+		pcie_capability_clear_word(tp->pdev, PCI_EXP_DEVCTL, val16);
 
 		/* Clear error status */
-		pci_write_config_word(tp->pdev,
-				      pci_pcie_cap(tp->pdev) + PCI_EXP_DEVSTA,
+		pcie_capability_write_word(tp->pdev, PCI_EXP_DEVSTA,
 				      PCI_EXP_DEVSTA_CED |
 				      PCI_EXP_DEVSTA_NFED |
 				      PCI_EXP_DEVSTA_FED |
@@ -14565,9 +14543,7 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 
 		tg3_flag_set(tp, PCI_EXPRESS);
 
-		pci_read_config_word(tp->pdev,
-				     pci_pcie_cap(tp->pdev) + PCI_EXP_LNKCTL,
-				     &lnkctl);
+		pcie_capability_read_word(tp->pdev, PCI_EXP_LNKCTL, &lnkctl);
 		if (lnkctl & PCI_EXP_LNKCTL_CLKREQ_EN) {
 			if (GET_ASIC_REV(tp->pci_chip_rev_id) ==
 			    ASIC_REV_5906) {
@@ -16397,7 +16373,7 @@ done:
 	rtnl_unlock();
 }
 
-static struct pci_error_handlers tg3_err_handler = {
+static const struct pci_error_handlers tg3_err_handler = {
 	.error_detected	= tg3_io_error_detected,
 	.slot_reset	= tg3_io_slot_reset,
 	.resume		= tg3_io_resume
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 77884191a8c6..4e980a7886fb 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -1086,7 +1086,7 @@ static int __init at91ether_probe(struct platform_device *pdev)
 	/* Clock */
 	lp->ether_clk = clk_get(&pdev->dev, "ether_clk");
 	if (IS_ERR(lp->ether_clk)) {
-		res = -ENODEV;
+		res = PTR_ERR(lp->ether_clk);
 		goto err_ioumap;
 	}
 	clk_enable(lp->ether_clk);
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index 6505070abcfa..875bbb999aa2 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -3036,7 +3036,7 @@ static void t3_io_resume(struct pci_dev *pdev)
 	t3_resume_ports(adapter);
 }
 
-static struct pci_error_handlers t3_err_handler = {
+static const struct pci_error_handlers t3_err_handler = {
 	.error_detected = t3_io_error_detected,
 	.slot_reset = t3_io_slot_reset,
 	.resume = t3_io_resume,
diff --git a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
index bff8a3cdd3df..aef45d3113ba 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
@@ -3289,22 +3289,18 @@ static void config_pcie(struct adapter *adap)
 	unsigned int log2_width, pldsize;
 	unsigned int fst_trn_rx, fst_trn_tx, acklat, rpllmt;
 
-	pci_read_config_word(adap->pdev,
-			     adap->pdev->pcie_cap + PCI_EXP_DEVCTL,
-			     &val);
+	pcie_capability_read_word(adap->pdev, PCI_EXP_DEVCTL, &val);
 	pldsize = (val & PCI_EXP_DEVCTL_PAYLOAD) >> 5;
 
 	pci_read_config_word(adap->pdev, 0x2, &devid);
 	if (devid == 0x37) {
-		pci_write_config_word(adap->pdev,
-				      adap->pdev->pcie_cap + PCI_EXP_DEVCTL,
-				      val & ~PCI_EXP_DEVCTL_READRQ &
-				      ~PCI_EXP_DEVCTL_PAYLOAD);
+		pcie_capability_write_word(adap->pdev, PCI_EXP_DEVCTL,
+					   val & ~PCI_EXP_DEVCTL_READRQ &
+					   ~PCI_EXP_DEVCTL_PAYLOAD);
 		pldsize = 0;
 	}
 
-	pci_read_config_word(adap->pdev, adap->pdev->pcie_cap + PCI_EXP_LNKCTL,
-			     &val);
+	pcie_capability_read_word(adap->pdev, PCI_EXP_LNKCTL, &val);
 
 	fst_trn_tx = G_NUMFSTTRNSEQ(t3_read_reg(adap, A_PCIE_PEX_CTRL0));
 	fst_trn_rx = adap->params.rev == 0 ? fst_trn_tx :
@@ -3425,15 +3421,13 @@ out_err:
 static void get_pci_mode(struct adapter *adapter, struct pci_params *p)
 {
 	static unsigned short speed_map[] = { 33, 66, 100, 133 };
-	u32 pci_mode, pcie_cap;
+	u32 pci_mode;
 
-	pcie_cap = pci_pcie_cap(adapter->pdev);
-	if (pcie_cap) {
+	if (pci_is_pcie(adapter->pdev)) {
 		u16 val;
 
 		p->variant = PCI_VARIANT_PCIE;
-		pci_read_config_word(adapter->pdev, pcie_cap + PCI_EXP_LNKSTA,
-					&val);
+		pcie_capability_read_word(adapter->pdev, PCI_EXP_LNKSTA, &val);
 		p->width = (val >> 4) & 0x3f;
 		return;
 	}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 5ed49af23d6a..933985420acb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -3453,7 +3453,7 @@ static void eeh_resume(struct pci_dev *pdev)
 	rtnl_unlock();
 }
 
-static struct pci_error_handlers cxgb4_eeh = {
+static const struct pci_error_handlers cxgb4_eeh = {
 	.error_detected = eeh_err_detected,
 	.slot_reset     = eeh_slot_reset,
 	.resume         = eeh_resume,
@@ -3694,15 +3694,7 @@ static void __devinit print_port_info(const struct net_device *dev)
 
 static void __devinit enable_pcie_relaxed_ordering(struct pci_dev *dev)
 {
-	u16 v;
-	int pos;
-
-	pos = pci_pcie_cap(dev);
-	if (pos > 0) {
-		pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &v);
-		v |= PCI_EXP_DEVCTL_RELAX_EN;
-		pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, v);
-	}
+	pcie_capability_set_word(dev, PCI_EXP_DEVCTL, PCI_EXP_DEVCTL_RELAX_EN);
 }
 
 /*
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index fa947dfa4c30..af1601323173 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -2741,11 +2741,9 @@ static void __devinit get_pci_mode(struct adapter *adapter,
 				   struct pci_params *p)
 {
 	u16 val;
-	u32 pcie_cap = pci_pcie_cap(adapter->pdev);
 
-	if (pcie_cap) {
-		pci_read_config_word(adapter->pdev, pcie_cap + PCI_EXP_LNKSTA,
-				     &val);
+	if (pci_is_pcie(adapter->pdev)) {
+		pcie_capability_read_word(adapter->pdev, PCI_EXP_LNKSTA, &val);
 		p->speed = val & PCI_EXP_LNKSTA_CLS;
 		p->width = (val & PCI_EXP_LNKSTA_NLW) >> 4;
 	}
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 78b8aa8069f0..95d10472f236 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -4106,7 +4106,7 @@ err:
 	dev_err(&adapter->pdev->dev, "EEH resume failed\n");
 }
 
-static struct pci_error_handlers be_eeh_handlers = {
+static const struct pci_error_handlers be_eeh_handlers = {
 	.error_detected = be_eeh_err_detected,
 	.slot_reset = be_eeh_reset,
 	.resume = be_eeh_resume,
diff --git a/drivers/net/ethernet/freescale/gianfar_ethtool.c b/drivers/net/ethernet/freescale/gianfar_ethtool.c
index 8971921cc1c8..ab6762caa957 100644
--- a/drivers/net/ethernet/freescale/gianfar_ethtool.c
+++ b/drivers/net/ethernet/freescale/gianfar_ethtool.c
@@ -1773,6 +1773,7 @@ static int gfar_get_nfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 }
 
 int gfar_phc_index = -1;
+EXPORT_SYMBOL(gfar_phc_index);
 
 static int gfar_get_ts_info(struct net_device *dev,
 			    struct ethtool_ts_info *info)
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index c08e5d40fecb..0daa66b8eca0 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -515,7 +515,7 @@ static int gianfar_ptp_probe(struct platform_device *dev)
 		err = PTR_ERR(etsects->clock);
 		goto no_clock;
 	}
-	gfar_phc_clock = ptp_clock_index(etsects->clock);
+	gfar_phc_index = ptp_clock_index(etsects->clock);
 
 	dev_set_drvdata(&dev->dev, etsects);
 
@@ -539,7 +539,7 @@ static int gianfar_ptp_remove(struct platform_device *dev)
 	gfar_write(&etsects->regs->tmr_temask, 0);
 	gfar_write(&etsects->regs->tmr_ctrl,   0);
 
-	gfar_phc_clock = -1;
+	gfar_phc_index = -1;
 	ptp_clock_unregister(etsects->clock);
 	iounmap(etsects->regs);
 	release_resource(etsects->rsrc);
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index 535f94fac4a1..29ce9bd27f94 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -3157,7 +3157,7 @@ static void e100_io_resume(struct pci_dev *pdev)
 	}
 }
 
-static struct pci_error_handlers e100_err_handler = {
+static const struct pci_error_handlers e100_err_handler = {
 	.error_detected = e100_io_error_detected,
 	.slot_reset = e100_io_slot_reset,
 	.resume = e100_io_resume,
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 3bfbb8df8989..f3f9aeb7d1e1 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -192,7 +192,7 @@ static pci_ers_result_t e1000_io_error_detected(struct pci_dev *pdev,
 static pci_ers_result_t e1000_io_slot_reset(struct pci_dev *pdev);
 static void e1000_io_resume(struct pci_dev *pdev);
 
-static struct pci_error_handlers e1000_err_handler = {
+static const struct pci_error_handlers e1000_err_handler = {
 	.error_detected = e1000_io_error_detected,
 	.slot_reset = e1000_io_slot_reset,
 	.resume = e1000_io_resume,
@@ -3149,6 +3149,17 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
+	/* On PCI/PCI-X HW, if packet size is less than ETH_ZLEN,
+	 * packets may get corrupted during padding by HW.
+	 * To WA this issue, pad all small packets manually.
+	 */
+	if (skb->len < ETH_ZLEN) {
+		if (skb_pad(skb, ETH_ZLEN - skb->len))
+			return NETDEV_TX_OK;
+		skb->len = ETH_ZLEN;
+		skb_set_tail_pointer(skb, ETH_ZLEN);
+	}
+
 	mss = skb_shinfo(skb)->gso_size;
 	/* The controller does a simple calculation to
 	 * make sure there is enough room in the FIFO before
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index d01a099475a1..3f0223ac4c7c 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -5584,16 +5584,15 @@ static void e1000_complete_shutdown(struct pci_dev *pdev, bool sleep,
 	 */
 	if (adapter->flags & FLAG_IS_QUAD_PORT) {
 		struct pci_dev *us_dev = pdev->bus->self;
-		int pos = pci_pcie_cap(us_dev);
 		u16 devctl;
 
-		pci_read_config_word(us_dev, pos + PCI_EXP_DEVCTL, &devctl);
-		pci_write_config_word(us_dev, pos + PCI_EXP_DEVCTL,
-		                      (devctl & ~PCI_EXP_DEVCTL_CERE));
+		pcie_capability_read_word(us_dev, PCI_EXP_DEVCTL, &devctl);
+		pcie_capability_write_word(us_dev, PCI_EXP_DEVCTL,
+					   (devctl & ~PCI_EXP_DEVCTL_CERE));
 
 		e1000_power_off(pdev, sleep, wake);
 
-		pci_write_config_word(us_dev, pos + PCI_EXP_DEVCTL, devctl);
+		pcie_capability_write_word(us_dev, PCI_EXP_DEVCTL, devctl);
 	} else {
 		e1000_power_off(pdev, sleep, wake);
 	}
@@ -5607,25 +5606,15 @@ static void __e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
 #else
 static void __e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
 {
-	int pos;
-	u16 reg16;
-
 	/*
 	 * Both device and parent should have the same ASPM setting.
 	 * Disable ASPM in downstream component first and then upstream.
 	 */
-	pos = pci_pcie_cap(pdev);
-	pci_read_config_word(pdev, pos + PCI_EXP_LNKCTL, &reg16);
-	reg16 &= ~state;
-	pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
-
-	if (!pdev->bus->self)
-		return;
+	pcie_capability_clear_word(pdev, PCI_EXP_LNKCTL, state);
 
-	pos = pci_pcie_cap(pdev->bus->self);
-	pci_read_config_word(pdev->bus->self, pos + PCI_EXP_LNKCTL, &reg16);
-	reg16 &= ~state;
-	pci_write_config_word(pdev->bus->self, pos + PCI_EXP_LNKCTL, reg16);
+	if (pdev->bus->self)
+		pcie_capability_clear_word(pdev->bus->self, PCI_EXP_LNKCTL,
+					   state);
 }
 #endif
 static void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
@@ -6486,7 +6475,7 @@ static void __devexit e1000_remove(struct pci_dev *pdev)
 }
 
 /* PCI Error Recovery (ERS) */
-static struct pci_error_handlers e1000_err_handler = {
+static const struct pci_error_handlers e1000_err_handler = {
 	.error_detected = e1000_io_error_detected,
 	.slot_reset = e1000_io_slot_reset,
 	.resume = e1000_io_resume,
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 48cc4fb1a307..f88c822e57a6 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -217,7 +217,7 @@ static pci_ers_result_t igb_io_error_detected(struct pci_dev *,
 static pci_ers_result_t igb_io_slot_reset(struct pci_dev *);
 static void igb_io_resume(struct pci_dev *);
 
-static struct pci_error_handlers igb_err_handler = {
+static const struct pci_error_handlers igb_err_handler = {
 	.error_detected = igb_io_error_detected,
 	.slot_reset = igb_io_slot_reset,
 	.resume = igb_io_resume,
@@ -6538,28 +6538,20 @@ static int igb_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 s32 igb_read_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
 {
 	struct igb_adapter *adapter = hw->back;
-	u16 cap_offset;
 
-	cap_offset = adapter->pdev->pcie_cap;
-	if (!cap_offset)
+	if (pcie_capability_read_word(adapter->pdev, reg, value))
 		return -E1000_ERR_CONFIG;
 
-	pci_read_config_word(adapter->pdev, cap_offset + reg, value);
-
 	return 0;
 }
 
 s32 igb_write_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
 {
 	struct igb_adapter *adapter = hw->back;
-	u16 cap_offset;
 
-	cap_offset = adapter->pdev->pcie_cap;
-	if (!cap_offset)
+	if (pcie_capability_write_word(adapter->pdev, reg, *value))
 		return -E1000_ERR_CONFIG;
 
-	pci_write_config_word(adapter->pdev, cap_offset + reg, *value);
-
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/intel/igbvf/netdev.c b/drivers/net/ethernet/intel/igbvf/netdev.c
index 0696abfe9944..0ac11f527a84 100644
--- a/drivers/net/ethernet/intel/igbvf/netdev.c
+++ b/drivers/net/ethernet/intel/igbvf/netdev.c
@@ -2833,7 +2833,7 @@ static void __devexit igbvf_remove(struct pci_dev *pdev)
 }
 
 /* PCI Error Recovery (ERS) */
-static struct pci_error_handlers igbvf_err_handler = {
+static const struct pci_error_handlers igbvf_err_handler = {
 	.error_detected = igbvf_io_error_detected,
 	.slot_reset = igbvf_io_slot_reset,
 	.resume = igbvf_io_resume,
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_main.c b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
index d05fc95befc5..d99a2d51b948 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_main.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
@@ -115,7 +115,7 @@ static pci_ers_result_t ixgb_io_error_detected (struct pci_dev *pdev,
 static pci_ers_result_t ixgb_io_slot_reset (struct pci_dev *pdev);
 static void ixgb_io_resume (struct pci_dev *pdev);
 
-static struct pci_error_handlers ixgb_err_handler = {
+static const struct pci_error_handlers ixgb_err_handler = {
 	.error_detected = ixgb_io_error_detected,
 	.slot_reset = ixgb_io_slot_reset,
 	.resume = ixgb_io_resume,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 4326f74f7137..ee61819d6088 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -7527,7 +7527,7 @@ static pci_ers_result_t ixgbe_io_error_detected(struct pci_dev *pdev,
 		goto skip_bad_vf_detection;
 
 	bdev = pdev->bus->self;
-	while (bdev && (bdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT))
+	while (bdev && (pci_pcie_type(bdev) != PCI_EXP_TYPE_ROOT_PORT))
 		bdev = bdev->bus->self;
 
 	if (!bdev)
@@ -7677,7 +7677,7 @@ static void ixgbe_io_resume(struct pci_dev *pdev)
 	netif_device_attach(netdev);
 }
 
-static struct pci_error_handlers ixgbe_err_handler = {
+static const struct pci_error_handlers ixgbe_err_handler = {
 	.error_detected = ixgbe_io_error_detected,
 	.slot_reset = ixgbe_io_slot_reset,
 	.resume = ixgbe_io_resume,
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 60ef64587412..6647383c4ddc 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -3256,7 +3256,7 @@ static void ixgbevf_io_resume(struct pci_dev *pdev)
 }
 
 /* PCI Error Recovery (ERS) */
-static struct pci_error_handlers ixgbevf_err_handler = {
+static const struct pci_error_handlers ixgbevf_err_handler = {
 	.error_detected = ixgbevf_io_error_detected,
 	.slot_reset = ixgbevf_io_slot_reset,
 	.resume = ixgbevf_io_resume,
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.c b/drivers/net/ethernet/mellanox/mlx4/icm.c
index daf417923661..31d02649be41 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.c
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.c
@@ -227,9 +227,10 @@ int mlx4_UNMAP_ICM_AUX(struct mlx4_dev *dev)
 			MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
 }
 
-int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj)
+int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj)
 {
-	int i = (obj & (table->num_obj - 1)) / (MLX4_TABLE_CHUNK_SIZE / table->obj_size);
+	u32 i = (obj & (table->num_obj - 1)) /
+			(MLX4_TABLE_CHUNK_SIZE / table->obj_size);
 	int ret = 0;
 
 	mutex_lock(&table->mutex);
@@ -262,16 +263,18 @@ out:
 	return ret;
 }
 
-void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj)
+void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj)
 {
-	int i;
+	u32 i;
+	u64 offset;
 
 	i = (obj & (table->num_obj - 1)) / (MLX4_TABLE_CHUNK_SIZE / table->obj_size);
 
 	mutex_lock(&table->mutex);
 
 	if (--table->icm[i]->refcount == 0) {
-		mlx4_UNMAP_ICM(dev, table->virt + i * MLX4_TABLE_CHUNK_SIZE,
+		offset = (u64) i * MLX4_TABLE_CHUNK_SIZE;
+		mlx4_UNMAP_ICM(dev, table->virt + offset,
 			       MLX4_TABLE_CHUNK_SIZE / MLX4_ICM_PAGE_SIZE);
 		mlx4_free_icm(dev, table->icm[i], table->coherent);
 		table->icm[i] = NULL;
@@ -280,9 +283,11 @@ void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj)
 	mutex_unlock(&table->mutex);
 }
 
-void *mlx4_table_find(struct mlx4_icm_table *table, int obj, dma_addr_t *dma_handle)
+void *mlx4_table_find(struct mlx4_icm_table *table, u32 obj,
+			dma_addr_t *dma_handle)
 {
-	int idx, offset, dma_offset, i;
+	int offset, dma_offset, i;
+	u64 idx;
 	struct mlx4_icm_chunk *chunk;
 	struct mlx4_icm *icm;
 	struct page *page = NULL;
@@ -292,7 +297,7 @@ void *mlx4_table_find(struct mlx4_icm_table *table, int obj, dma_addr_t *dma_han
 
 	mutex_lock(&table->mutex);
 
-	idx = (obj & (table->num_obj - 1)) * table->obj_size;
+	idx = (u64) (obj & (table->num_obj - 1)) * table->obj_size;
 	icm = table->icm[idx / MLX4_TABLE_CHUNK_SIZE];
 	dma_offset = offset = idx % MLX4_TABLE_CHUNK_SIZE;
 
@@ -326,10 +331,11 @@ out:
 }
 
 int mlx4_table_get_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			 int start, int end)
+			 u32 start, u32 end)
 {
 	int inc = MLX4_TABLE_CHUNK_SIZE / table->obj_size;
-	int i, err;
+	int err;
+	u32 i;
 
 	for (i = start; i <= end; i += inc) {
 		err = mlx4_table_get(dev, table, i);
@@ -349,9 +355,9 @@ fail:
 }
 
 void mlx4_table_put_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			  int start, int end)
+			  u32 start, u32 end)
 {
-	int i;
+	u32 i;
 
 	for (i = start; i <= end; i += MLX4_TABLE_CHUNK_SIZE / table->obj_size)
 		mlx4_table_put(dev, table, i);
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.h b/drivers/net/ethernet/mellanox/mlx4/icm.h
index a67744f53506..dee67fa39107 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.h
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.h
@@ -71,17 +71,17 @@ struct mlx4_icm *mlx4_alloc_icm(struct mlx4_dev *dev, int npages,
 				gfp_t gfp_mask, int coherent);
 void mlx4_free_icm(struct mlx4_dev *dev, struct mlx4_icm *icm, int coherent);
 
-int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj);
-void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj);
+int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj);
+void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj);
 int mlx4_table_get_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			 int start, int end);
+			 u32 start, u32 end);
 void mlx4_table_put_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			  int start, int end);
+			  u32 start, u32 end);
 int mlx4_init_icm_table(struct mlx4_dev *dev, struct mlx4_icm_table *table,
 			u64 virt, int obj_size,	u32 nobj, int reserved,
 			int use_lowmem, int use_coherent);
 void mlx4_cleanup_icm_table(struct mlx4_dev *dev, struct mlx4_icm_table *table);
-void *mlx4_table_find(struct mlx4_icm_table *table, int obj, dma_addr_t *dma_handle);
+void *mlx4_table_find(struct mlx4_icm_table *table, u32 obj, dma_addr_t *dma_handle);
 
 static inline void mlx4_icm_first(struct mlx4_icm *icm,
 				  struct mlx4_icm_iter *iter)
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 2f816c6aed72..dd6ea942625c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -2300,7 +2300,7 @@ static pci_ers_result_t mlx4_pci_slot_reset(struct pci_dev *pdev)
 	return ret ? PCI_ERS_RESULT_DISCONNECT : PCI_ERS_RESULT_RECOVERED;
 }
 
-static struct pci_error_handlers mlx4_err_handler = {
+static const struct pci_error_handlers mlx4_err_handler = {
 	.error_detected = mlx4_pci_err_detected,
 	.slot_reset     = mlx4_pci_slot_reset,
 };
diff --git a/drivers/net/ethernet/mellanox/mlx4/reset.c b/drivers/net/ethernet/mellanox/mlx4/reset.c
index 11e7c1cb99bf..dd1b5093d8b1 100644
--- a/drivers/net/ethernet/mellanox/mlx4/reset.c
+++ b/drivers/net/ethernet/mellanox/mlx4/reset.c
@@ -141,16 +141,16 @@ int mlx4_reset(struct mlx4_dev *dev)
 	/* Now restore the PCI headers */
 	if (pcie_cap) {
 		devctl = hca_header[(pcie_cap + PCI_EXP_DEVCTL) / 4];
-		if (pci_write_config_word(dev->pdev, pcie_cap + PCI_EXP_DEVCTL,
-					   devctl)) {
+		if (pcie_capability_write_word(dev->pdev, PCI_EXP_DEVCTL,
+					       devctl)) {
 			err = -ENODEV;
 			mlx4_err(dev, "Couldn't restore HCA PCI Express "
 				 "Device Control register, aborting.\n");
 			goto out;
 		}
 		linkctl = hca_header[(pcie_cap + PCI_EXP_LNKCTL) / 4];
-		if (pci_write_config_word(dev->pdev, pcie_cap + PCI_EXP_LNKCTL,
-					   linkctl)) {
+		if (pcie_capability_write_word(dev->pdev, PCI_EXP_LNKCTL,
+					       linkctl)) {
 			err = -ENODEV;
 			mlx4_err(dev, "Couldn't restore HCA PCI Express "
 				 "Link control register, aborting.\n");
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index fa85cf1353fd..83516e3369c9 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -1078,22 +1078,16 @@ static int myri10ge_reset(struct myri10ge_priv *mgp)
 #ifdef CONFIG_MYRI10GE_DCA
 static int myri10ge_toggle_relaxed(struct pci_dev *pdev, int on)
 {
-	int ret, cap, err;
+	int ret;
 	u16 ctl;
 
-	cap = pci_pcie_cap(pdev);
-	if (!cap)
-		return 0;
-
-	err = pci_read_config_word(pdev, cap + PCI_EXP_DEVCTL, &ctl);
-	if (err)
-		return 0;
+	pcie_capability_read_word(pdev, PCI_EXP_DEVCTL, &ctl);
 
 	ret = (ctl & PCI_EXP_DEVCTL_RELAX_EN) >> 4;
 	if (ret != on) {
 		ctl &= ~PCI_EXP_DEVCTL_RELAX_EN;
 		ctl |= (on << 4);
-		pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL, ctl);
+		pcie_capability_write_word(pdev, PCI_EXP_DEVCTL, ctl);
 	}
 	return ret;
 }
@@ -3192,18 +3186,13 @@ static void myri10ge_enable_ecrc(struct myri10ge_priv *mgp)
 	struct device *dev = &mgp->pdev->dev;
 	int cap;
 	unsigned err_cap;
-	u16 val;
-	u8 ext_type;
 	int ret;
 
 	if (!myri10ge_ecrc_enable || !bridge)
 		return;
 
 	/* check that the bridge is a root port */
-	cap = pci_pcie_cap(bridge);
-	pci_read_config_word(bridge, cap + PCI_CAP_FLAGS, &val);
-	ext_type = (val & PCI_EXP_FLAGS_TYPE) >> 4;
-	if (ext_type != PCI_EXP_TYPE_ROOT_PORT) {
+	if (pci_pcie_type(bridge) != PCI_EXP_TYPE_ROOT_PORT) {
 		if (myri10ge_ecrc_enable > 1) {
 			struct pci_dev *prev_bridge, *old_bridge = bridge;
 
@@ -3218,11 +3207,8 @@ static void myri10ge_enable_ecrc(struct myri10ge_priv *mgp)
 						" to force ECRC\n");
 					return;
 				}
-				cap = pci_pcie_cap(bridge);
-				pci_read_config_word(bridge,
-						     cap + PCI_CAP_FLAGS, &val);
-				ext_type = (val & PCI_EXP_FLAGS_TYPE) >> 4;
-			} while (ext_type != PCI_EXP_TYPE_ROOT_PORT);
+			} while (pci_pcie_type(bridge) !=
+				 PCI_EXP_TYPE_ROOT_PORT);
 
 			dev_info(dev,
 				 "Forcing ECRC on non-root port %s"
@@ -3335,11 +3321,10 @@ static void myri10ge_select_firmware(struct myri10ge_priv *mgp)
 	int overridden = 0;
 
 	if (myri10ge_force_firmware == 0) {
-		int link_width, exp_cap;
+		int link_width;
 		u16 lnk;
 
-		exp_cap = pci_pcie_cap(mgp->pdev);
-		pci_read_config_word(mgp->pdev, exp_cap + PCI_EXP_LNKSTA, &lnk);
+		pcie_capability_read_word(mgp->pdev, PCI_EXP_LNKSTA, &lnk);
 		link_width = (lnk >> 4) & 0x3f;
 
 		/* Check to see if Link is less than 8 or if the
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index d958c2299372..de50547c187d 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -484,7 +484,7 @@ static DEFINE_PCI_DEVICE_TABLE(s2io_tbl) = {
 
 MODULE_DEVICE_TABLE(pci, s2io_tbl);
 
-static struct pci_error_handlers s2io_err_handler = {
+static const struct pci_error_handlers s2io_err_handler = {
 	.error_detected = s2io_io_error_detected,
 	.slot_reset = s2io_io_slot_reset,
 	.resume = s2io_io_resume,
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-config.c b/drivers/net/ethernet/neterion/vxge/vxge-config.c
index 32d06824fe3e..c2e420a84d22 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-config.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-config.c
@@ -757,7 +757,7 @@ __vxge_hw_verify_pci_e_info(struct __vxge_hw_device *hldev)
 	u16 lnk;
 
 	/* Get the negotiated link width and speed from PCI config space */
-	pci_read_config_word(dev, dev->pcie_cap + PCI_EXP_LNKSTA, &lnk);
+	pcie_capability_read_word(dev, PCI_EXP_LNKSTA, &lnk);
 
 	if ((lnk & PCI_EXP_LNKSTA_CLS) != 1)
 		return VXGE_HW_ERR_INVALID_PCI_INFO;
@@ -1982,7 +1982,7 @@ u16 vxge_hw_device_link_width_get(struct __vxge_hw_device *hldev)
 	struct pci_dev *dev = hldev->pdev;
 	u16 lnk;
 
-	pci_read_config_word(dev, dev->pcie_cap + PCI_EXP_LNKSTA, &lnk);
+	pcie_capability_read_word(dev, PCI_EXP_LNKSTA, &lnk);
 	return (lnk & VXGE_HW_PCI_EXP_LNKCAP_LNK_WIDTH) >> 4;
 }
 
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index de2190443510..cfa71a30dc8d 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -4799,7 +4799,7 @@ static void __devexit vxge_remove(struct pci_dev *pdev)
 			     __LINE__);
 }
 
-static struct pci_error_handlers vxge_err_handler = {
+static const struct pci_error_handlers vxge_err_handler = {
 	.error_detected = vxge_io_error_detected,
 	.slot_reset = vxge_io_slot_reset,
 	.resume = vxge_io_resume,
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index c42bbb16cdae..a688a2ddcfd6 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -722,10 +722,8 @@ static int octeon_mgmt_init_phy(struct net_device *netdev)
 				   octeon_mgmt_adjust_link, 0,
 				   PHY_INTERFACE_MODE_MII);
 
-	if (IS_ERR(p->phydev)) {
-		p->phydev = NULL;
+	if (!p->phydev)
 		return -1;
-	}
 
 	phy_start_aneg(p->phydev);
 
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
index feb85d56c750..b2a94d02a521 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
@@ -2795,7 +2795,7 @@ static const struct dev_pm_ops pch_gbe_pm_ops = {
 };
 #endif
 
-static struct pci_error_handlers pch_gbe_err_handler = {
+static const struct pci_error_handlers pch_gbe_err_handler = {
 	.error_detected = pch_gbe_io_error_detected,
 	.slot_reset = pch_gbe_io_slot_reset,
 	.resume = pch_gbe_io_resume
diff --git a/drivers/net/ethernet/pasemi/pasemi_mac.c b/drivers/net/ethernet/pasemi/pasemi_mac.c
index e559dfa06d6a..6fa74d530e44 100644
--- a/drivers/net/ethernet/pasemi/pasemi_mac.c
+++ b/drivers/net/ethernet/pasemi/pasemi_mac.c
@@ -1101,9 +1101,9 @@ static int pasemi_mac_phy_init(struct net_device *dev)
 	phydev = of_phy_connect(dev, phy_dn, &pasemi_adjust_link, 0,
 				PHY_INTERFACE_MODE_SGMII);
 
-	if (IS_ERR(phydev)) {
+	if (!phydev) {
 		printk(KERN_ERR "%s: Could not attach to phy\n", dev->name);
-		return PTR_ERR(phydev);
+		return -ENODEV;
 	}
 
 	mac->phydev = phydev;
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
index 342b3a79bd0f..df450616ab37 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
@@ -1378,11 +1378,15 @@ static void netxen_mask_aer_correctable(struct netxen_adapter *adapter)
 	struct pci_dev *root = pdev->bus->self;
 	u32 aer_pos;
 
+	/* root bus? */
+	if (!root)
+		return;
+
 	if (adapter->ahw.board_type != NETXEN_BRDTYPE_P3_4_GB_MM &&
 		adapter->ahw.board_type != NETXEN_BRDTYPE_P3_10G_TP)
 		return;
 
-	if (root->pcie_type != PCI_EXP_TYPE_ROOT_PORT)
+	if (pci_pcie_type(root) != PCI_EXP_TYPE_ROOT_PORT)
 		return;
 
 	aer_pos = pci_find_ext_capability(root, PCI_EXT_CAP_ID_ERR);
@@ -3336,7 +3340,7 @@ netxen_free_vlan_ip_list(struct netxen_adapter *adapter)
 { }
 #endif
 
-static struct pci_error_handlers netxen_err_handler = {
+static const struct pci_error_handlers netxen_err_handler = {
 	.error_detected = netxen_io_error_detected,
 	.slot_reset = netxen_io_slot_reset,
 	.resume = netxen_io_resume,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
index b8ead696141e..2a179d087207 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
@@ -15,7 +15,7 @@ qlcnic_poll_rsp(struct qlcnic_adapter *adapter)
 
 	do {
 		/* give atleast 1ms for firmware to respond */
-		msleep(1);
+		mdelay(1);
 
 		if (++timeout > QLCNIC_OS_CRB_RETRY_COUNT)
 			return QLCNIC_CDRP_RSP_TIMEOUT;
@@ -601,7 +601,7 @@ void qlcnic_fw_destroy_ctx(struct qlcnic_adapter *adapter)
 		qlcnic_fw_cmd_destroy_tx_ctx(adapter);
 
 		/* Allow dma queues to drain after context reset */
-		msleep(20);
+		mdelay(20);
 	}
 }
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 212c12193275..473ce134ca63 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -4522,7 +4522,7 @@ static void
 qlcnic_restore_indev_addr(struct net_device *dev, unsigned long event)
 { }
 #endif
-static struct pci_error_handlers qlcnic_err_handler = {
+static const struct pci_error_handlers qlcnic_err_handler = {
 	.error_detected = qlcnic_io_error_detected,
 	.slot_reset = qlcnic_io_slot_reset,
 	.resume = qlcnic_io_resume,
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index b53a3b60b648..b262d6156816 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -4847,7 +4847,7 @@ static void qlge_io_resume(struct pci_dev *pdev)
 	netif_device_attach(ndev);
 }
 
-static struct pci_error_handlers qlge_err_handler = {
+static const struct pci_error_handlers qlge_err_handler = {
 	.error_detected = qlge_io_error_detected,
 	.slot_reset = qlge_io_slot_reset,
 	.resume = qlge_io_resume,
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index b47d5b35024e..a7cc56007b33 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -833,15 +833,8 @@ static void rtl_unlock_work(struct rtl8169_private *tp)
 
 static void rtl_tx_performance_tweak(struct pci_dev *pdev, u16 force)
 {
-	int cap = pci_pcie_cap(pdev);
-
-	if (cap) {
-		u16 ctl;
-
-		pci_read_config_word(pdev, cap + PCI_EXP_DEVCTL, &ctl);
-		ctl = (ctl & ~PCI_EXP_DEVCTL_READRQ) | force;
-		pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL, ctl);
-	}
+	pcie_capability_clear_and_set_word(pdev, PCI_EXP_DEVCTL,
+					   PCI_EXP_DEVCTL_READRQ, force);
 }
 
 struct rtl_cond {
@@ -4739,28 +4732,14 @@ static void rtl_ephy_init(struct rtl8169_private *tp, const struct ephy_info *e,
 
 static void rtl_disable_clock_request(struct pci_dev *pdev)
 {
-	int cap = pci_pcie_cap(pdev);
-
-	if (cap) {
-		u16 ctl;
-
-		pci_read_config_word(pdev, cap + PCI_EXP_LNKCTL, &ctl);
-		ctl &= ~PCI_EXP_LNKCTL_CLKREQ_EN;
-		pci_write_config_word(pdev, cap + PCI_EXP_LNKCTL, ctl);
-	}
+	pcie_capability_clear_word(pdev, PCI_EXP_LNKCTL,
+				   PCI_EXP_LNKCTL_CLKREQ_EN);
 }
 
 static void rtl_enable_clock_request(struct pci_dev *pdev)
 {
-	int cap = pci_pcie_cap(pdev);
-
-	if (cap) {
-		u16 ctl;
-
-		pci_read_config_word(pdev, cap + PCI_EXP_LNKCTL, &ctl);
-		ctl |= PCI_EXP_LNKCTL_CLKREQ_EN;
-		pci_write_config_word(pdev, cap + PCI_EXP_LNKCTL, ctl);
-	}
+	pcie_capability_set_word(pdev, PCI_EXP_LNKCTL,
+				 PCI_EXP_LNKCTL_CLKREQ_EN);
 }
 
 #define R8168_CPCMD_QUIRK_MASK (\
@@ -5405,14 +5384,9 @@ static void rtl_hw_start_8101(struct net_device *dev)
 		tp->event_slow &= ~RxFIFOOver;
 
 	if (tp->mac_version == RTL_GIGA_MAC_VER_13 ||
-	    tp->mac_version == RTL_GIGA_MAC_VER_16) {
-		int cap = pci_pcie_cap(pdev);
-
-		if (cap) {
-			pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL,
-					      PCI_EXP_DEVCTL_NOSNOOP_EN);
-		}
-	}
+	    tp->mac_version == RTL_GIGA_MAC_VER_16)
+		pcie_capability_set_word(pdev, PCI_EXP_DEVCTL,
+					 PCI_EXP_DEVCTL_NOSNOOP_EN);
 
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 
diff --git a/drivers/net/ethernet/sgi/ioc3-eth.c b/drivers/net/ethernet/sgi/ioc3-eth.c
index b5ba3084c7fc..3e5519a0acc7 100644
--- a/drivers/net/ethernet/sgi/ioc3-eth.c
+++ b/drivers/net/ethernet/sgi/ioc3-eth.c
@@ -1147,15 +1147,17 @@ static void __devinit ioc3_8250_register(struct ioc3_uartregs __iomem *uart)
 {
 #define COSMISC_CONSTANT 6
 
-	struct uart_port port = {
-		.irq		= 0,
-		.flags		= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF,
-		.iotype		= UPIO_MEM,
-		.regshift	= 0,
-		.uartclk	= (22000000 << 1) / COSMISC_CONSTANT,
-
-		.membase	= (unsigned char __iomem *) uart,
-		.mapbase	= (unsigned long) uart,
+	struct uart_8250_port port = {
+	        .port = {
+			.irq		= 0,
+			.flags		= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF,
+			.iotype		= UPIO_MEM,
+			.regshift	= 0,
+			.uartclk	= (22000000 << 1) / COSMISC_CONSTANT,
+
+			.membase	= (unsigned char __iomem *) uart,
+			.mapbase	= (unsigned long) uart,
+                }
 	};
 	unsigned char lcr;
 
@@ -1164,7 +1166,7 @@ static void __devinit ioc3_8250_register(struct ioc3_uartregs __iomem *uart)
 	uart->iu_scr = COSMISC_CONSTANT,
 	uart->iu_lcr = lcr;
 	uart->iu_lcr;
-	serial8250_register_port(&port);
+	serial8250_register_8250_port(&port);
 }
 
 static void __devinit ioc3_serial_probe(struct pci_dev *pdev, struct ioc3 *ioc3)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index c136162e6473..3be88331d17a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1066,7 +1066,7 @@ static int stmmac_open(struct net_device *dev)
 	} else
 		priv->tm->enable = 1;
 #endif
-	clk_enable(priv->stmmac_clk);
+	clk_prepare_enable(priv->stmmac_clk);
 
 	stmmac_check_ether_addr(priv);
 
@@ -1188,7 +1188,7 @@ open_error:
 	if (priv->phydev)
 		phy_disconnect(priv->phydev);
 
-	clk_disable(priv->stmmac_clk);
+	clk_disable_unprepare(priv->stmmac_clk);
 
 	return ret;
 }
@@ -1246,7 +1246,7 @@ static int stmmac_release(struct net_device *dev)
 #ifdef CONFIG_STMMAC_DEBUG_FS
 	stmmac_exit_fs();
 #endif
-	clk_disable(priv->stmmac_clk);
+	clk_disable_unprepare(priv->stmmac_clk);
 
 	return 0;
 }
@@ -2178,7 +2178,7 @@ int stmmac_suspend(struct net_device *ndev)
 	else {
 		stmmac_set_mac(priv->ioaddr, false);
 		/* Disable clock in case of PWM is off */
-		clk_disable(priv->stmmac_clk);
+		clk_disable_unprepare(priv->stmmac_clk);
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
 	return 0;
@@ -2203,7 +2203,7 @@ int stmmac_resume(struct net_device *ndev)
 		priv->hw->mac->pmt(priv->ioaddr, 0);
 	else
 		/* enable the clk prevously disabled */
-		clk_enable(priv->stmmac_clk);
+		clk_prepare_enable(priv->stmmac_clk);
 
 	netif_device_attach(ndev);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_timer.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_timer.c
index 2a0e1abde7e7..4ccd4e2977b7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_timer.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_timer.c
@@ -97,19 +97,19 @@ static struct clk *timer_clock;
 static void stmmac_tmu_start(unsigned int new_freq)
 {
 	clk_set_rate(timer_clock, new_freq);
-	clk_enable(timer_clock);
+	clk_prepare_enable(timer_clock);
 }
 
 static void stmmac_tmu_stop(void)
 {
-	clk_disable(timer_clock);
+	clk_disable_unprepare(timer_clock);
 }
 
 int stmmac_open_ext_timer(struct net_device *dev, struct stmmac_timer *tm)
 {
 	timer_clock = clk_get(NULL, TMU_CHANNEL);
 
-	if (timer_clock == NULL)
+	if (IS_ERR(timer_clock))
 		return -1;
 
 	if (tmu2_register_user(stmmac_timer_handler, (void *)dev) < 0) {
@@ -126,7 +126,7 @@ int stmmac_open_ext_timer(struct net_device *dev, struct stmmac_timer *tm)
 
 int stmmac_close_ext_timer(void)
 {
-	clk_disable(timer_clock);
+	clk_disable_unprepare(timer_clock);
 	tmu2_unregister_user();
 	clk_put(timer_clock);
 	return 0;
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index c2a0fe393267..3208dca66758 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -9762,9 +9762,8 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 	union niu_parent_id parent_id;
 	struct net_device *dev;
 	struct niu *np;
-	int err, pos;
+	int err;
 	u64 dma_mask;
-	u16 val16;
 
 	niu_driver_version();
 
@@ -9787,8 +9786,7 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 		goto err_out_disable_pdev;
 	}
 
-	pos = pci_pcie_cap(pdev);
-	if (pos <= 0) {
+	if (!pci_is_pcie(pdev)) {
 		dev_err(&pdev->dev, "Cannot find PCI Express capability, aborting\n");
 		goto err_out_free_res;
 	}
@@ -9813,14 +9811,11 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 		goto err_out_free_dev;
 	}
 
-	pci_read_config_word(pdev, pos + PCI_EXP_DEVCTL, &val16);
-	val16 &= ~PCI_EXP_DEVCTL_NOSNOOP_EN;
-	val16 |= (PCI_EXP_DEVCTL_CERE |
-		  PCI_EXP_DEVCTL_NFERE |
-		  PCI_EXP_DEVCTL_FERE |
-		  PCI_EXP_DEVCTL_URRE |
-		  PCI_EXP_DEVCTL_RELAX_EN);
-	pci_write_config_word(pdev, pos + PCI_EXP_DEVCTL, val16);
+	pcie_capability_clear_and_set_word(pdev, PCI_EXP_DEVCTL,
+		PCI_EXP_DEVCTL_NOSNOOP_EN,
+		PCI_EXP_DEVCTL_CERE | PCI_EXP_DEVCTL_NFERE |
+		PCI_EXP_DEVCTL_FERE | PCI_EXP_DEVCTL_URRE |
+		PCI_EXP_DEVCTL_RELAX_EN);
 
 	dma_mask = DMA_BIT_MASK(44);
 	err = pci_set_dma_mask(pdev, dma_mask);
diff --git a/drivers/net/irda/irtty-sir.c b/drivers/net/irda/irtty-sir.c
index 3352b2443e58..30087ca23a0f 100644
--- a/drivers/net/irda/irtty-sir.c
+++ b/drivers/net/irda/irtty-sir.c
@@ -124,8 +124,8 @@ static int irtty_change_speed(struct sir_dev *dev, unsigned speed)
 	tty = priv->tty;
 
 	mutex_lock(&tty->termios_mutex);
-	old_termios = *(tty->termios);
-	cflag = tty->termios->c_cflag;
+	old_termios = tty->termios;
+	cflag = tty->termios.c_cflag;
 	tty_encode_baud_rate(tty, speed, speed);
 	if (tty->ops->set_termios)
 		tty->ops->set_termios(tty, &old_termios);
@@ -281,15 +281,15 @@ static inline void irtty_stop_receiver(struct tty_struct *tty, int stop)
 	int cflag;
 
 	mutex_lock(&tty->termios_mutex);
-	old_termios = *(tty->termios);
-	cflag = tty->termios->c_cflag;
+	old_termios = tty->termios;
+	cflag = tty->termios.c_cflag;
 	
 	if (stop)
 		cflag &= ~CREAD;
 	else
 		cflag |= CREAD;
 
-	tty->termios->c_cflag = cflag;
+	tty->termios.c_cflag = cflag;
 	if (tty->ops->set_termios)
 		tty->ops->set_termios(tty, &old_termios);
 	mutex_unlock(&tty->termios_mutex);
diff --git a/drivers/net/irda/sh_sir.c b/drivers/net/irda/sh_sir.c
index 256eddf1f75a..795109425568 100644
--- a/drivers/net/irda/sh_sir.c
+++ b/drivers/net/irda/sh_sir.c
@@ -280,7 +280,7 @@ static int sh_sir_set_baudrate(struct sh_sir_self *self, u32 baudrate)
 	}
 
 	clk = clk_get(NULL, "irda_clk");
-	if (!clk) {
+	if (IS_ERR(clk)) {
 		dev_err(dev, "can not get irda_clk\n");
 		return -EIO;
 	}
diff --git a/drivers/net/phy/bcm87xx.c b/drivers/net/phy/bcm87xx.c
index 2346b38b9837..799789518e87 100644
--- a/drivers/net/phy/bcm87xx.c
+++ b/drivers/net/phy/bcm87xx.c
@@ -229,3 +229,5 @@ static void __exit bcm87xx_exit(void)
 		ARRAY_SIZE(bcm87xx_driver));
 }
 module_exit(bcm87xx_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index cf287e0eb408..2165d5fdb8c0 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -21,6 +21,12 @@
 #include <linux/phy.h>
 #include <linux/micrel_phy.h>
 
+/* Operation Mode Strap Override */
+#define MII_KSZPHY_OMSO				0x16
+#define KSZPHY_OMSO_B_CAST_OFF			(1 << 9)
+#define KSZPHY_OMSO_RMII_OVERRIDE		(1 << 1)
+#define KSZPHY_OMSO_MII_OVERRIDE		(1 << 0)
+
 /* general Interrupt control/status reg in vendor specific block. */
 #define MII_KSZPHY_INTCS			0x1B
 #define	KSZPHY_INTCS_JABBER			(1 << 15)
@@ -101,6 +107,13 @@ static int kszphy_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+static int ksz8021_config_init(struct phy_device *phydev)
+{
+	const u16 val = KSZPHY_OMSO_B_CAST_OFF | KSZPHY_OMSO_RMII_OVERRIDE;
+	phy_write(phydev, MII_KSZPHY_OMSO, val);
+	return 0;
+}
+
 static int ks8051_config_init(struct phy_device *phydev)
 {
 	int regval;
@@ -128,9 +141,22 @@ static struct phy_driver ksphy_driver[] = {
 	.config_intr	= ks8737_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
 }, {
-	.phy_id		= PHY_ID_KS8041,
+	.phy_id		= PHY_ID_KSZ8021,
+	.phy_id_mask	= 0x00ffffff,
+	.name		= "Micrel KSZ8021",
+	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
+			   SUPPORTED_Asym_Pause),
+	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.config_init	= ksz8021_config_init,
+	.config_aneg	= genphy_config_aneg,
+	.read_status	= genphy_read_status,
+	.ack_interrupt	= kszphy_ack_interrupt,
+	.config_intr	= kszphy_config_intr,
+	.driver		= { .owner = THIS_MODULE,},
+}, {
+	.phy_id		= PHY_ID_KSZ8041,
 	.phy_id_mask	= 0x00fffff0,
-	.name		= "Micrel KS8041",
+	.name		= "Micrel KSZ8041",
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
@@ -141,9 +167,9 @@ static struct phy_driver ksphy_driver[] = {
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
 }, {
-	.phy_id		= PHY_ID_KS8051,
+	.phy_id		= PHY_ID_KSZ8051,
 	.phy_id_mask	= 0x00fffff0,
-	.name		= "Micrel KS8051",
+	.name		= "Micrel KSZ8051",
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
@@ -154,8 +180,8 @@ static struct phy_driver ksphy_driver[] = {
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
 }, {
-	.phy_id		= PHY_ID_KS8001,
-	.name		= "Micrel KS8001 or KS8721",
+	.phy_id		= PHY_ID_KSZ8001,
+	.name		= "Micrel KSZ8001 or KS8721",
 	.phy_id_mask	= 0x00ffffff,
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
@@ -201,10 +227,11 @@ MODULE_LICENSE("GPL");
 
 static struct mdio_device_id __maybe_unused micrel_tbl[] = {
 	{ PHY_ID_KSZ9021, 0x000ffffe },
-	{ PHY_ID_KS8001, 0x00ffffff },
+	{ PHY_ID_KSZ8001, 0x00ffffff },
 	{ PHY_ID_KS8737, 0x00fffff0 },
-	{ PHY_ID_KS8041, 0x00fffff0 },
-	{ PHY_ID_KS8051, 0x00fffff0 },
+	{ PHY_ID_KSZ8021, 0x00ffffff },
+	{ PHY_ID_KSZ8041, 0x00fffff0 },
+	{ PHY_ID_KSZ8051, 0x00fffff0 },
 	{ }
 };
 
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index 6d6192316b30..88e3991464e7 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -56,6 +56,32 @@ static int smsc_phy_config_init(struct phy_device *phydev)
 	return smsc_phy_ack_interrupt (phydev);
 }
 
+static int lan87xx_config_init(struct phy_device *phydev)
+{
+	/*
+	 * Make sure the EDPWRDOWN bit is NOT set. Setting this bit on
+	 * LAN8710/LAN8720 PHY causes the PHY to misbehave, likely due
+	 * to a bug on the chip.
+	 *
+	 * When the system is powered on with the network cable being
+	 * disconnected all the way until after ifconfig ethX up is
+	 * issued for the LAN port with this PHY, connecting the cable
+	 * afterwards does not cause LINK change detection, while the
+	 * expected behavior is the Link UP being detected.
+	 */
+	int rc = phy_read(phydev, MII_LAN83C185_CTRL_STATUS);
+	if (rc < 0)
+		return rc;
+
+	rc &= ~MII_LAN83C185_EDPWRDOWN;
+
+	rc = phy_write(phydev, MII_LAN83C185_CTRL_STATUS, rc);
+	if (rc < 0)
+		return rc;
+
+	return smsc_phy_ack_interrupt(phydev);
+}
+
 static int lan911x_config_init(struct phy_device *phydev)
 {
 	return smsc_phy_ack_interrupt(phydev);
@@ -162,7 +188,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
-	.config_init	= smsc_phy_config_init,
+	.config_init	= lan87xx_config_init,
 
 	/* IRQ related */
 	.ack_interrupt	= smsc_phy_ack_interrupt,
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index cbf7047decc0..20f31d0d1536 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -570,7 +570,7 @@ static int pppoe_release(struct socket *sock)
 
 	po = pppox_sk(sk);
 
-	if (sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND)) {
+	if (sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND | PPPOX_ZOMBIE)) {
 		dev_put(po->pppoe_dev);
 		po->pppoe_dev = NULL;
 	}
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 341b65dbbcd3..f8cd61f449a4 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -848,7 +848,7 @@ static struct netpoll_info *team_netpoll_info(struct team *team)
 }
 #endif
 
-static void __team_port_change_check(struct team_port *port, bool linkup);
+static void __team_port_change_port_added(struct team_port *port, bool linkup);
 
 static int team_port_add(struct team *team, struct net_device *port_dev)
 {
@@ -948,7 +948,7 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 	team_port_enable(team, port);
 	list_add_tail_rcu(&port->list, &team->port_list);
 	__team_compute_features(team);
-	__team_port_change_check(port, !!netif_carrier_ok(port_dev));
+	__team_port_change_port_added(port, !!netif_carrier_ok(port_dev));
 	__team_options_change_check(team);
 
 	netdev_info(dev, "Port device %s added\n", portname);
@@ -983,6 +983,8 @@ err_set_mtu:
 	return err;
 }
 
+static void __team_port_change_port_removed(struct team_port *port);
+
 static int team_port_del(struct team *team, struct net_device *port_dev)
 {
 	struct net_device *dev = team->dev;
@@ -999,8 +1001,7 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 	__team_option_inst_mark_removed_port(team, port);
 	__team_options_change_check(team);
 	__team_option_inst_del_port(team, port);
-	port->removed = true;
-	__team_port_change_check(port, false);
+	__team_port_change_port_removed(port);
 	team_port_disable(team, port);
 	list_del_rcu(&port->list);
 	netdev_rx_handler_unregister(port_dev);
@@ -1652,8 +1653,8 @@ static int team_nl_cmd_noop(struct sk_buff *skb, struct genl_info *info)
 
 	hdr = genlmsg_put(msg, info->snd_pid, info->snd_seq,
 			  &team_nl_family, 0, TEAM_CMD_NOOP);
-	if (IS_ERR(hdr)) {
-		err = PTR_ERR(hdr);
+	if (!hdr) {
+		err = -EMSGSIZE;
 		goto err_msg_put;
 	}
 
@@ -1847,8 +1848,8 @@ start_again:
 
 	hdr = genlmsg_put(skb, pid, seq, &team_nl_family, flags | NLM_F_MULTI,
 			  TEAM_CMD_OPTIONS_GET);
-	if (IS_ERR(hdr))
-		return PTR_ERR(hdr);
+	if (!hdr)
+		return -EMSGSIZE;
 
 	if (nla_put_u32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex))
 		goto nla_put_failure;
@@ -2067,8 +2068,8 @@ static int team_nl_fill_port_list_get(struct sk_buff *skb,
 
 	hdr = genlmsg_put(skb, pid, seq, &team_nl_family, flags,
 			  TEAM_CMD_PORT_LIST_GET);
-	if (IS_ERR(hdr))
-		return PTR_ERR(hdr);
+	if (!hdr)
+		return -EMSGSIZE;
 
 	if (nla_put_u32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex))
 		goto nla_put_failure;
@@ -2251,13 +2252,11 @@ static void __team_options_change_check(struct team *team)
 }
 
 /* rtnl lock is held */
-static void __team_port_change_check(struct team_port *port, bool linkup)
+
+static void __team_port_change_send(struct team_port *port, bool linkup)
 {
 	int err;
 
-	if (!port->removed && port->state.linkup == linkup)
-		return;
-
 	port->changed = true;
 	port->state.linkup = linkup;
 	team_refresh_port_linkup(port);
@@ -2282,6 +2281,23 @@ send_event:
 
 }
 
+static void __team_port_change_check(struct team_port *port, bool linkup)
+{
+	if (port->state.linkup != linkup)
+		__team_port_change_send(port, linkup);
+}
+
+static void __team_port_change_port_added(struct team_port *port, bool linkup)
+{
+	__team_port_change_send(port, linkup);
+}
+
+static void __team_port_change_port_removed(struct team_port *port)
+{
+	port->removed = true;
+	__team_port_change_send(port, false);
+}
+
 static void team_port_change_check(struct team_port *port, bool linkup)
 {
 	struct team *team = port->team;
diff --git a/drivers/net/usb/asix_devices.c b/drivers/net/usb/asix_devices.c
index 4fd48df6b989..32e31c5c5dc6 100644
--- a/drivers/net/usb/asix_devices.c
+++ b/drivers/net/usb/asix_devices.c
@@ -962,6 +962,10 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x2001, 0x3c05),
 	.driver_info = (unsigned long) &ax88772_info,
 }, {
+       // DLink DUB-E100 H/W Ver C1
+       USB_DEVICE (0x2001, 0x1a02),
+       .driver_info = (unsigned long) &ax88772_info,
+}, {
 	// Linksys USB1000
 	USB_DEVICE (0x1737, 0x0039),
 	.driver_info = (unsigned long) &ax88178_info,
diff --git a/drivers/net/usb/hso.c b/drivers/net/usb/hso.c
index 62f30b46fa42..605a4baa9b7b 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -1107,7 +1107,6 @@ static void _hso_serial_set_termios(struct tty_struct *tty,
 				    struct ktermios *old)
 {
 	struct hso_serial *serial = tty->driver_data;
-	struct ktermios *termios;
 
 	if (!serial) {
 		printk(KERN_ERR "%s: no tty structures", __func__);
@@ -1119,16 +1118,15 @@ static void _hso_serial_set_termios(struct tty_struct *tty,
 	/*
 	 *	Fix up unsupported bits
 	 */
-	termios = tty->termios;
-	termios->c_iflag &= ~IXON; /* disable enable XON/XOFF flow control */
+	tty->termios.c_iflag &= ~IXON; /* disable enable XON/XOFF flow control */
 
-	termios->c_cflag &=
+	tty->termios.c_cflag &=
 		~(CSIZE		/* no size */
 		| PARENB	/* disable parity bit */
 		| CBAUD		/* clear current baud rate */
 		| CBAUDEX);	/* clear current buad rate */
 
-	termios->c_cflag |= CS8;	/* character size 8 bits */
+	tty->termios.c_cflag |= CS8;	/* character size 8 bits */
 
 	/* baud rate 115200 */
 	tty_encode_baud_rate(tty, 115200, 115200);
@@ -1425,14 +1423,14 @@ static void hso_serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 
 	if (old)
 		D5("Termios called with: cflags new[%d] - old[%d]",
-		   tty->termios->c_cflag, old->c_cflag);
+		   tty->termios.c_cflag, old->c_cflag);
 
 	/* the actual setup */
 	spin_lock_irqsave(&serial->serial_lock, flags);
 	if (serial->port.count)
 		_hso_serial_set_termios(tty, old);
 	else
-		tty->termios = old;
+		tty->termios = *old;
 	spin_unlock_irqrestore(&serial->serial_lock, flags);
 
 	/* done */
@@ -2289,9 +2287,11 @@ static int hso_serial_common_create(struct hso_serial *serial, int num_urbs,
 	if (minor < 0)
 		goto exit;
 
+	tty_port_init(&serial->port);
+
 	/* register our minor number */
-	serial->parent->dev = tty_register_device(tty_drv, minor,
-					&serial->parent->interface->dev);
+	serial->parent->dev = tty_port_register_device(&serial->port, tty_drv,
+			minor, &serial->parent->interface->dev);
 	dev = serial->parent->dev;
 	dev_set_drvdata(dev, serial->parent);
 	i = device_create_file(dev, &dev_attr_hsotype);
@@ -2300,7 +2300,6 @@ static int hso_serial_common_create(struct hso_serial *serial, int num_urbs,
 	serial->minor = minor;
 	serial->magic = HSO_SERIAL_MAGIC;
 	spin_lock_init(&serial->serial_lock);
-	tty_port_init(&serial->port);
 	serial->num_rx_urbs = num_urbs;
 
 	/* RX, allocate urb and initialize */
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index b1ba68f1a049..3543c9e57824 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -366,16 +366,20 @@ static const struct usb_device_id products[] = {
 	},
 
 	/* 2. Combined interface devices matching on class+protocol */
+	{	/* Huawei E367 and possibly others in "Windows mode" */
+		USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 7),
+		.driver_info        = (unsigned long)&qmi_wwan_info,
+	},
 	{	/* Huawei E392, E398 and possibly others in "Windows mode" */
 		USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 17),
 		.driver_info        = (unsigned long)&qmi_wwan_shared,
 	},
-	{	/* Pantech UML290 */
-		USB_DEVICE_AND_INTERFACE_INFO(0x106c, 0x3718, USB_CLASS_VENDOR_SPEC, 0xf0, 0xff),
+	{	/* Pantech UML290, P4200 and more */
+		USB_VENDOR_AND_INTERFACE_INFO(0x106c, USB_CLASS_VENDOR_SPEC, 0xf0, 0xff),
 		.driver_info        = (unsigned long)&qmi_wwan_shared,
 	},
 	{	/* Pantech UML290 - newer firmware */
-		USB_DEVICE_AND_INTERFACE_INFO(0x106c, 0x3718, USB_CLASS_VENDOR_SPEC, 0xf1, 0xff),
+		USB_VENDOR_AND_INTERFACE_INFO(0x106c, USB_CLASS_VENDOR_SPEC, 0xf1, 0xff),
 		.driver_info        = (unsigned long)&qmi_wwan_shared,
 	},
 
@@ -383,6 +387,7 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x19d2, 0x0055, 1)},	/* ZTE (Vodafone) K3520-Z */
 	{QMI_FIXED_INTF(0x19d2, 0x0063, 4)},	/* ZTE (Vodafone) K3565-Z */
 	{QMI_FIXED_INTF(0x19d2, 0x0104, 4)},	/* ZTE (Vodafone) K4505-Z */
+	{QMI_FIXED_INTF(0x19d2, 0x0157, 5)},	/* ZTE MF683 */
 	{QMI_FIXED_INTF(0x19d2, 0x0167, 4)},	/* ZTE MF820D */
 	{QMI_FIXED_INTF(0x19d2, 0x0326, 4)},	/* ZTE MF821D */
 	{QMI_FIXED_INTF(0x19d2, 0x1008, 4)},	/* ZTE (Vodafone) K3570-Z */
diff --git a/drivers/net/usb/smsc75xx.c b/drivers/net/usb/smsc75xx.c
index f5ab6e613ec8..376143e8a1aa 100644
--- a/drivers/net/usb/smsc75xx.c
+++ b/drivers/net/usb/smsc75xx.c
@@ -1253,6 +1253,7 @@ static struct usb_driver smsc75xx_driver = {
 	.probe		= usbnet_probe,
 	.suspend	= usbnet_suspend,
 	.resume		= usbnet_resume,
+	.reset_resume	= usbnet_resume,
 	.disconnect	= usbnet_disconnect,
 	.disable_hub_initiated_lpm = 1,
 };
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 2588848f4a82..d066f2516e47 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -2982,6 +2982,10 @@ static u32 ath9k_hw_ar9300_get_eeprom(struct ath_hw *ah,
 	case EEP_RX_MASK:
 		return pBase->txrxMask & 0xf;
 	case EEP_PAPRD:
+		if (AR_SREV_9462(ah))
+			return false;
+		if (!ah->config.enable_paprd);
+			return false;
 		return !!(pBase->featureEnable & BIT(5));
 	case EEP_CHAIN_MASK_REDUCE:
 		return (pBase->miscConfiguration >> 0x3) & 0x1;
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 68b643c8943c..c8ef30127adb 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -1577,6 +1577,8 @@ int ath9k_init_debug(struct ath_hw *ah)
 			    sc->debug.debugfs_phy, sc, &fops_tx_chainmask);
 	debugfs_create_file("disable_ani", S_IRUSR | S_IWUSR,
 			    sc->debug.debugfs_phy, sc, &fops_disable_ani);
+	debugfs_create_bool("paprd", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			    &sc->sc_ah->config.enable_paprd);
 	debugfs_create_file("regidx", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
 			    sc, &fops_regidx);
 	debugfs_create_file("regval", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 48af40151d23..4faf0a395876 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -2497,10 +2497,6 @@ int ath9k_hw_fill_cap_info(struct ath_hw *ah)
 		pCap->rx_status_len = sizeof(struct ar9003_rxs);
 		pCap->tx_desc_len = sizeof(struct ar9003_txc);
 		pCap->txs_len = sizeof(struct ar9003_txs);
-		if (!ah->config.paprd_disable &&
-		    ah->eep_ops->get_eeprom(ah, EEP_PAPRD) &&
-		    !AR_SREV_9462(ah))
-			pCap->hw_caps |= ATH9K_HW_CAP_PAPRD;
 	} else {
 		pCap->tx_desc_len = sizeof(struct ath_desc);
 		if (AR_SREV_9280_20(ah))
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 6599a75f01fe..de6968fc64f4 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -236,7 +236,6 @@ enum ath9k_hw_caps {
 	ATH9K_HW_CAP_LDPC			= BIT(6),
 	ATH9K_HW_CAP_FASTCLOCK			= BIT(7),
 	ATH9K_HW_CAP_SGI_20			= BIT(8),
-	ATH9K_HW_CAP_PAPRD			= BIT(9),
 	ATH9K_HW_CAP_ANT_DIV_COMB		= BIT(10),
 	ATH9K_HW_CAP_2GHZ			= BIT(11),
 	ATH9K_HW_CAP_5GHZ			= BIT(12),
@@ -287,12 +286,12 @@ struct ath9k_ops_config {
 	u8 pcie_clock_req;
 	u32 pcie_waen;
 	u8 analog_shiftreg;
-	u8 paprd_disable;
 	u32 ofdm_trig_low;
 	u32 ofdm_trig_high;
 	u32 cck_trig_high;
 	u32 cck_trig_low;
 	u32 enable_ani;
+	u32 enable_paprd;
 	int serialize_regmode;
 	bool rx_intr_mitigation;
 	bool tx_intr_mitigation;
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 825a29cc9313..7b88b9c39ccd 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -423,7 +423,7 @@ set_timer:
 		cal_interval = min(cal_interval, (u32)short_cal_interval);
 
 	mod_timer(&common->ani.timer, jiffies + msecs_to_jiffies(cal_interval));
-	if ((sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_PAPRD) && ah->caldata) {
+	if (ah->eep_ops->get_eeprom(ah, EEP_PAPRD) && ah->caldata) {
 		if (!ah->caldata->paprd_done)
 			ieee80211_queue_work(sc->hw, &sc->paprd_work);
 		else if (!ah->paprd_table_write_done)
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index a978984d78a5..ef11dc639461 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -113,41 +113,32 @@ static void ath_pci_aspm_init(struct ath_common *common)
 	struct ath_hw *ah = sc->sc_ah;
 	struct pci_dev *pdev = to_pci_dev(sc->dev);
 	struct pci_dev *parent;
-	int pos;
-	u8 aspm;
+	u16 aspm;
 
 	if (!ah->is_pciexpress)
 		return;
 
-	pos = pci_pcie_cap(pdev);
-	if (!pos)
-		return;
-
 	parent = pdev->bus->self;
 	if (!parent)
 		return;
 
 	if (ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_NONE) {
 		/* Bluetooth coexistance requires disabling ASPM. */
-		pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &aspm);
-		aspm &= ~(PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
-		pci_write_config_byte(pdev, pos + PCI_EXP_LNKCTL, aspm);
+		pcie_capability_clear_word(pdev, PCI_EXP_LNKCTL,
+			PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
 
 		/*
 		 * Both upstream and downstream PCIe components should
 		 * have the same ASPM settings.
 		 */
-		pos = pci_pcie_cap(parent);
-		pci_read_config_byte(parent, pos + PCI_EXP_LNKCTL, &aspm);
-		aspm &= ~(PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
-		pci_write_config_byte(parent, pos + PCI_EXP_LNKCTL, aspm);
+		pcie_capability_clear_word(parent, PCI_EXP_LNKCTL,
+			PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
 
 		ath_info(common, "Disabling ASPM since BTCOEX is enabled\n");
 		return;
 	}
 
-	pos = pci_pcie_cap(parent);
-	pci_read_config_byte(parent, pos +  PCI_EXP_LNKCTL, &aspm);
+	pcie_capability_read_word(parent, PCI_EXP_LNKCTL, &aspm);
 	if (aspm & (PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1)) {
 		ah->aspm_enabled = true;
 		/* Initialize PCIe PM and SERDES registers. */
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/b43/Kconfig
index 3876c7ea54f4..7a28d21ac389 100644
--- a/drivers/net/wireless/b43/Kconfig
+++ b/drivers/net/wireless/b43/Kconfig
@@ -34,8 +34,8 @@ config B43_BCMA
 config B43_BCMA_EXTRA
 	bool "Hardware support that overlaps with the brcmsmac driver"
 	depends on B43_BCMA
-	default n if BRCMSMAC || BRCMSMAC_MODULE
-	default	y
+	default n if BRCMSMAC
+	default y
 
 config B43_SSB
 	bool
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 49765d34b4e0..7c4ee72f9d56 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -638,6 +638,8 @@ static int brcmf_sdio_pd_probe(struct platform_device *pdev)
 
 		oobirq_entry = kzalloc(sizeof(struct brcmf_sdio_oobirq),
 				       GFP_KERNEL);
+		if (!oobirq_entry)
+			return -ENOMEM;
 		oobirq_entry->irq = res->start;
 		oobirq_entry->flags = res->flags & IRQF_TRIGGER_MASK;
 		list_add_tail(&oobirq_entry->list, &oobirq_lh);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 2621dd3d7dcd..6f70953f0bad 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -764,8 +764,11 @@ static void brcmf_c_arp_offload_set(struct brcmf_pub *drvr, int arp_mode)
 {
 	char iovbuf[32];
 	int retcode;
+	__le32 arp_mode_le;
 
-	brcmf_c_mkiovar("arp_ol", (char *)&arp_mode, 4, iovbuf, sizeof(iovbuf));
+	arp_mode_le = cpu_to_le32(arp_mode);
+	brcmf_c_mkiovar("arp_ol", (char *)&arp_mode_le, 4, iovbuf,
+			sizeof(iovbuf));
 	retcode = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
 				   iovbuf, sizeof(iovbuf));
 	retcode = retcode >= 0 ? 0 : retcode;
@@ -781,8 +784,11 @@ static void brcmf_c_arp_offload_enable(struct brcmf_pub *drvr, int arp_enable)
 {
 	char iovbuf[32];
 	int retcode;
+	__le32 arp_enable_le;
 
-	brcmf_c_mkiovar("arpoe", (char *)&arp_enable, 4,
+	arp_enable_le = cpu_to_le32(arp_enable);
+
+	brcmf_c_mkiovar("arpoe", (char *)&arp_enable_le, 4,
 			iovbuf, sizeof(iovbuf));
 	retcode = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
 				   iovbuf, sizeof(iovbuf));
@@ -800,10 +806,10 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 	char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];	/*  Room for
 				 "event_msgs" + '\0' + bitvec  */
 	char buf[128], *ptr;
-	u32 roaming = 1;
-	uint bcn_timeout = 3;
-	int scan_assoc_time = 40;
-	int scan_unassoc_time = 40;
+	__le32 roaming_le = cpu_to_le32(1);
+	__le32 bcn_timeout_le = cpu_to_le32(3);
+	__le32 scan_assoc_time_le = cpu_to_le32(40);
+	__le32 scan_unassoc_time_le = cpu_to_le32(40);
 	int i;
 	struct brcmf_bus_dcmd *cmdlst;
 	struct list_head *cur, *q;
@@ -829,14 +835,14 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 
 	/* Setup timeout if Beacons are lost and roam is off to report
 		 link down */
-	brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
+	brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout_le, 4, iovbuf,
 		    sizeof(iovbuf));
 	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
 	/* Enable/Disable build-in roaming to allowed ext supplicant to take
 		 of romaing */
-	brcmf_c_mkiovar("roam_off", (char *)&roaming, 4,
+	brcmf_c_mkiovar("roam_off", (char *)&roaming_le, 4,
 		      iovbuf, sizeof(iovbuf));
 	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
@@ -848,9 +854,9 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 				  sizeof(iovbuf));
 
 	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_CHANNEL_TIME,
-			 (char *)&scan_assoc_time, sizeof(scan_assoc_time));
+		 (char *)&scan_assoc_time_le, sizeof(scan_assoc_time_le));
 	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_UNASSOC_TIME,
-			 (char *)&scan_unassoc_time, sizeof(scan_unassoc_time));
+		 (char *)&scan_unassoc_time_le, sizeof(scan_unassoc_time_le));
 
 	/* Set and enable ARP offload feature */
 	brcmf_c_arp_offload_set(drvr, BRCMF_ARPOL_MODE);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index c36e92312443..50b5553b6964 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -500,8 +500,10 @@ static void wl_iscan_prep(struct brcmf_scan_params_le *params_le,
 	params_le->active_time = cpu_to_le32(-1);
 	params_le->passive_time = cpu_to_le32(-1);
 	params_le->home_time = cpu_to_le32(-1);
-	if (ssid && ssid->SSID_len)
-		memcpy(&params_le->ssid_le, ssid, sizeof(struct brcmf_ssid));
+	if (ssid && ssid->SSID_len) {
+		params_le->ssid_le.SSID_len = cpu_to_le32(ssid->SSID_len);
+		memcpy(&params_le->ssid_le.SSID, ssid->SSID, ssid->SSID_len);
+	}
 }
 
 static s32
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index 7ed7d7577024..64a48f06d68b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -77,7 +77,7 @@
 					 NL80211_RRF_NO_IBSS)
 
 static const struct ieee80211_regdomain brcms_regdom_x2 = {
-	.n_reg_rules = 7,
+	.n_reg_rules = 6,
 	.alpha2 = "X2",
 	.reg_rules = {
 		BRCM_2GHZ_2412_2462,
diff --git a/drivers/net/wireless/iwlegacy/common.h b/drivers/net/wireless/iwlegacy/common.h
index 5f5017767b99..724682669060 100644
--- a/drivers/net/wireless/iwlegacy/common.h
+++ b/drivers/net/wireless/iwlegacy/common.h
@@ -1832,10 +1832,8 @@ int il_enqueue_hcmd(struct il_priv *il, struct il_host_cmd *cmd);
 static inline u16
 il_pcie_link_ctl(struct il_priv *il)
 {
-	int pos;
 	u16 pci_lnk_ctl;
-	pos = pci_pcie_cap(il->pci_dev);
-	pci_read_config_word(il->pci_dev, pos + PCI_EXP_LNKCTL, &pci_lnk_ctl);
+	pcie_capability_read_word(il->pci_dev, PCI_EXP_LNKCTL, &pci_lnk_ctl);
 	return pci_lnk_ctl;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 1e86ea2266d4..063ecaff5b56 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -675,13 +675,10 @@ static void iwl_set_pwr_vmain(struct iwl_trans *trans)
 static u16 iwl_pciexp_link_ctrl(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	int pos;
 	u16 pci_lnk_ctl;
 
-	struct pci_dev *pci_dev = trans_pcie->pci_dev;
-
-	pos = pci_pcie_cap(pci_dev);
-	pci_read_config_word(pci_dev, pos + PCI_EXP_LNKCTL, &pci_lnk_ctl);
+	pcie_capability_read_word(trans_pcie->pci_dev, PCI_EXP_LNKCTL,
+				  &pci_lnk_ctl);
 	return pci_lnk_ctl;
 }
 
@@ -1442,6 +1439,7 @@ static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 	return err;
 
 err_free_irq:
+	trans_pcie->irq_requested = false;
 	free_irq(trans_pcie->irq, trans);
 error:
 	iwl_free_isr_ict(trans);
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 80f75d3ba84a..5983631a1b1a 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -372,13 +372,11 @@ static void rtl_pci_parse_configuration(struct pci_dev *pdev,
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 
 	u8 tmp;
-	int pos;
-	u8 linkctrl_reg;
+	u16 linkctrl_reg;
 
 	/*Link Control Register */
-	pos = pci_pcie_cap(pdev);
-	pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &linkctrl_reg);
-	pcipriv->ndis_adapter.linkctrl_reg = linkctrl_reg;
+	pcie_capability_read_word(pdev, PCI_EXP_LNKCTL, &linkctrl_reg);
+	pcipriv->ndis_adapter.linkctrl_reg = (u8)linkctrl_reg;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Link Control Register =%x\n",
 		 pcipriv->ndis_adapter.linkctrl_reg);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 44febfde9493..8a7b864faca3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -315,7 +315,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 	u16 box_reg = 0, box_extreg = 0;
 	u8 u1b_tmp;
 	bool isfw_read = false;
-	bool bwrite_sucess = false;
+	bool bwrite_success = false;
 	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[2];
@@ -354,7 +354,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		}
 	}
 
-	while (!bwrite_sucess) {
+	while (!bwrite_success) {
 		wait_writeh2c_limmit--;
 		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -491,7 +491,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		}
 
-		bwrite_sucess = true;
+		bwrite_success = true;
 
 		rtlhal->last_hmeboxnum = boxnum + 1;
 		if (rtlhal->last_hmeboxnum == 4)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
index 04c3aef8a4f6..2925094b2d91 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
@@ -117,6 +117,7 @@
 
 #define CHIP_VER_B			BIT(4)
 #define CHIP_92C_BITMASK		BIT(0)
+#define CHIP_UNKNOWN			BIT(7)
 #define CHIP_92C_1T2R			0x03
 #define CHIP_92C			0x01
 #define CHIP_88C			0x00
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index bd0da7ef290b..dd4bb0950a57 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -994,8 +994,16 @@ static enum version_8192c _rtl92ce_read_chip_version(struct ieee80211_hw *hw)
 		version = (value32 & TYPE_ID) ? VERSION_A_CHIP_92C :
 			   VERSION_A_CHIP_88C;
 	} else {
-		version = (value32 & TYPE_ID) ? VERSION_B_CHIP_92C :
-			   VERSION_B_CHIP_88C;
+		version = (enum version_8192c) (CHIP_VER_B |
+				((value32 & TYPE_ID) ? CHIP_92C_BITMASK : 0) |
+				((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0));
+		if ((!IS_CHIP_VENDOR_UMC(version)) && (value32 &
+		     CHIP_VER_RTL_MASK)) {
+			version = (enum version_8192c)(version |
+				   ((((value32 & CHIP_VER_RTL_MASK) == BIT(12))
+				   ? CHIP_VENDOR_UMC_B_CUT : CHIP_UNKNOWN) |
+				   CHIP_VENDOR_UMC));
+		}
 	}
 
 	switch (version) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index 3aa927f8b9b9..7d8f96405f42 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -162,10 +162,12 @@ int rtl92c_init_sw_vars(struct ieee80211_hw *hw)
 
 	/* request fw */
 	if (IS_VENDOR_UMC_A_CUT(rtlhal->version) &&
-	    !IS_92C_SERIAL(rtlhal->version))
+	    !IS_92C_SERIAL(rtlhal->version)) {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cfwU.bin";
-	else if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version))
+	} else if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version)) {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cfwU_B.bin";
+		pr_info("****** This B_CUT device may not work with kernels 3.6 and earlier\n");
+	}
 
 	rtlpriv->max_fw_size = 0x4000;
 	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/fw.c b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
index 895ae6c1f354..eb22dccc418b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
@@ -365,7 +365,7 @@ static void _rtl92d_fill_h2c_command(struct ieee80211_hw *hw,
 	u8 u1b_tmp;
 	bool isfw_read = false;
 	u8 buf_index = 0;
-	bool bwrite_sucess = false;
+	bool bwrite_success = false;
 	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[2];
@@ -408,7 +408,7 @@ static void _rtl92d_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		}
 	}
-	while (!bwrite_sucess) {
+	while (!bwrite_success) {
 		wait_writeh2c_limmit--;
 		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -515,7 +515,7 @@ static void _rtl92d_fill_h2c_command(struct ieee80211_hw *hw,
 				 "switch case not processed\n");
 			break;
 		}
-		bwrite_sucess = true;
+		bwrite_success = true;
 		rtlhal->last_hmeboxnum = boxnum + 1;
 		if (rtlhal->last_hmeboxnum == 4)
 			rtlhal->last_hmeboxnum = 0;
diff --git a/drivers/oprofile/cpu_buffer.c b/drivers/oprofile/cpu_buffer.c
index b8ef8ddcc292..8aa73fac6ad4 100644
--- a/drivers/oprofile/cpu_buffer.c
+++ b/drivers/oprofile/cpu_buffer.c
@@ -451,14 +451,9 @@ static void wq_sync_buffer(struct work_struct *work)
 {
 	struct oprofile_cpu_buffer *b =
 		container_of(work, struct oprofile_cpu_buffer, work.work);
-	if (b->cpu != smp_processor_id()) {
-		printk(KERN_DEBUG "WQ on CPU%d, prefer CPU%d\n",
-		       smp_processor_id(), b->cpu);
-
-		if (!cpu_online(b->cpu)) {
-			cancel_delayed_work(&b->work);
-			return;
-		}
+	if (b->cpu != smp_processor_id() && !cpu_online(b->cpu)) {
+		cancel_delayed_work(&b->work);
+		return;
 	}
 	sync_buffer(b->cpu);
 
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index ffddc4f64268..fb6a1fe21b93 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -477,14 +477,12 @@ dino_card_setup(struct pci_bus *bus, void __iomem *base_addr)
 	if (ccio_allocate_resource(dino_dev->hba.dev, res, _8MB,
 				F_EXTEND(0xf0000000UL) | _8MB,
 				F_EXTEND(0xffffffffUL) &~ _8MB, _8MB) < 0) {
-		struct list_head *ln, *tmp_ln;
+		struct pci_dev *dev, *tmp;
 
 		printk(KERN_ERR "Dino: cannot attach bus %s\n",
 		       dev_name(bus->bridge));
 		/* kill the bus, we can't do anything with it */
-		list_for_each_safe(ln, tmp_ln, &bus->devices) {
-			struct pci_dev *dev = pci_dev_b(ln);
-
+		list_for_each_entry_safe(dev, tmp, &bus->devices, bus_list) {
 			list_del(&dev->bus_list);
 		}
 			
@@ -549,7 +547,6 @@ dino_card_fixup(struct pci_dev *dev)
 static void __init
 dino_fixup_bus(struct pci_bus *bus)
 {
-	struct list_head *ln;
         struct pci_dev *dev;
         struct dino_device *dino_dev = DINO_DEV(parisc_walk_tree(bus->bridge));
 
@@ -596,8 +593,7 @@ dino_fixup_bus(struct pci_bus *bus)
 	}
 
 
-	list_for_each(ln, &bus->devices) {
-		dev = pci_dev_b(ln);
+	list_for_each_entry(dev, &bus->devices, bus_list) {
 		if (is_card_dino(&dino_dev->hba.dev->id))
 			dino_card_fixup(dev);
 
diff --git a/drivers/parisc/lba_pci.c b/drivers/parisc/lba_pci.c
index 4f9cf2456f4e..fdd63a6a62d6 100644
--- a/drivers/parisc/lba_pci.c
+++ b/drivers/parisc/lba_pci.c
@@ -629,7 +629,7 @@ truncate_pat_collision(struct resource *root, struct resource *new)
 static void
 lba_fixup_bus(struct pci_bus *bus)
 {
-	struct list_head *ln;
+	struct pci_dev *dev;
 #ifdef FBB_SUPPORT
 	u16 status;
 #endif
@@ -710,9 +710,8 @@ lba_fixup_bus(struct pci_bus *bus)
 
 	}
 
-	list_for_each(ln, &bus->devices) {
+	list_for_each_entry(dev, &bus->devices, bus_list) {
 		int i;
-		struct pci_dev *dev = pci_dev_b(ln);
 
 		DBG("lba_fixup_bus() %s\n", pci_name(dev));
 
@@ -770,7 +769,7 @@ lba_fixup_bus(struct pci_bus *bus)
 	}
 
 	/* Lastly enable FBB/PERR/SERR on all devices too */
-	list_for_each(ln, &bus->devices) {
+	list_for_each_entry(dev, &bus->devices, bus_list) {
 		(void) pci_read_config_word(dev, PCI_COMMAND, &status);
 		status |= PCI_COMMAND_PARITY | PCI_COMMAND_SERR | fbb_enable;
 		(void) pci_write_config_word(dev, PCI_COMMAND, status);
diff --git a/drivers/parport/parport_gsc.c b/drivers/parport/parport_gsc.c
index 5d6de380e42b..352f96180bc7 100644
--- a/drivers/parport/parport_gsc.c
+++ b/drivers/parport/parport_gsc.c
@@ -271,6 +271,7 @@ struct parport *__devinit parport_gsc_probe_port (unsigned long base,
 	if (!parport_SPP_supported (p)) {
 		/* No port. */
 		kfree (priv);
+		kfree(ops);
 		return NULL;
 	}
 	parport_PS2_supported (p);
diff --git a/drivers/parport/parport_serial.c b/drivers/parport/parport_serial.c
index e9c32274df3f..1631eeaf440e 100644
--- a/drivers/parport/parport_serial.c
+++ b/drivers/parport/parport_serial.c
@@ -62,6 +62,7 @@ enum parport_pc_pci_cards {
 	timedia_9079a,
 	timedia_9079b,
 	timedia_9079c,
+	wch_ch353_2s1p,
 };
 
 /* each element directly indexed from enum list, above */
@@ -145,6 +146,7 @@ static struct parport_pc_pci cards[] __devinitdata = {
 	/* timedia_9079a */             { 1, { { 2, 3 }, } },
 	/* timedia_9079b */             { 1, { { 2, 3 }, } },
 	/* timedia_9079c */             { 1, { { 2, 3 }, } },
+	/* wch_ch353_2s1p*/             { 1, { { 2, -1}, } },
 };
 
 static struct pci_device_id parport_serial_pci_tbl[] = {
@@ -243,7 +245,8 @@ static struct pci_device_id parport_serial_pci_tbl[] = {
 	{ 0x1409, 0x7168, 0x1409, 0xb079, 0, 0, timedia_9079a },
 	{ 0x1409, 0x7168, 0x1409, 0xc079, 0, 0, timedia_9079b },
 	{ 0x1409, 0x7168, 0x1409, 0xd079, 0, 0, timedia_9079c },
-
+	/* WCH CARDS */
+	{ 0x4348, 0x7053, 0x4348, 0x3253, 0, 0, wch_ch353_2s1p},
 	{ 0, } /* terminate list */
 };
 MODULE_DEVICE_TABLE(pci,parport_serial_pci_tbl);
@@ -460,6 +463,12 @@ static struct pciserial_board pci_parport_serial_boards[] __devinitdata = {
 		.base_baud	= 921600,
 		.uart_offset	= 8,
 	},
+	[wch_ch353_2s1p] = {
+		.flags          = FL_BASE0|FL_BASE_BARS,
+		.num_ports      = 2,
+		.base_baud      = 115200,
+		.uart_offset    = 8,
+	},
 };
 
 struct parport_serial_private {
diff --git a/drivers/pci/.gitignore b/drivers/pci/.gitignore
deleted file mode 100644
index f297ca8d313e..000000000000
--- a/drivers/pci/.gitignore
+++ /dev/null
@@ -1,4 +0,0 @@
-classlist.h
-devlist.h
-gen-devlist
-
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index 848bfb84c04c..6d51aa68ec7a 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -3,7 +3,6 @@
 #
 config ARCH_SUPPORTS_MSI
 	bool
-	default n
 
 config PCI_MSI
 	bool "Message Signaled Interrupts (MSI and MSI-X)"
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index ba91a7e17519..3af0478c057b 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -469,3 +469,205 @@ void pci_cfg_access_unlock(struct pci_dev *dev)
 	raw_spin_unlock_irqrestore(&pci_lock, flags);
 }
 EXPORT_SYMBOL_GPL(pci_cfg_access_unlock);
+
+static inline int pcie_cap_version(const struct pci_dev *dev)
+{
+	return dev->pcie_flags_reg & PCI_EXP_FLAGS_VERS;
+}
+
+static inline bool pcie_cap_has_devctl(const struct pci_dev *dev)
+{
+	return true;
+}
+
+static inline bool pcie_cap_has_lnkctl(const struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+
+	return pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       type == PCI_EXP_TYPE_ENDPOINT ||
+	       type == PCI_EXP_TYPE_LEG_END;
+}
+
+static inline bool pcie_cap_has_sltctl(const struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+
+	return pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       (type == PCI_EXP_TYPE_DOWNSTREAM &&
+		dev->pcie_flags_reg & PCI_EXP_FLAGS_SLOT);
+}
+
+static inline bool pcie_cap_has_rtctl(const struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+
+	return pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       type == PCI_EXP_TYPE_RC_EC;
+}
+
+static bool pcie_capability_reg_implemented(struct pci_dev *dev, int pos)
+{
+	if (!pci_is_pcie(dev))
+		return false;
+
+	switch (pos) {
+	case PCI_EXP_FLAGS_TYPE:
+		return true;
+	case PCI_EXP_DEVCAP:
+	case PCI_EXP_DEVCTL:
+	case PCI_EXP_DEVSTA:
+		return pcie_cap_has_devctl(dev);
+	case PCI_EXP_LNKCAP:
+	case PCI_EXP_LNKCTL:
+	case PCI_EXP_LNKSTA:
+		return pcie_cap_has_lnkctl(dev);
+	case PCI_EXP_SLTCAP:
+	case PCI_EXP_SLTCTL:
+	case PCI_EXP_SLTSTA:
+		return pcie_cap_has_sltctl(dev);
+	case PCI_EXP_RTCTL:
+	case PCI_EXP_RTCAP:
+	case PCI_EXP_RTSTA:
+		return pcie_cap_has_rtctl(dev);
+	case PCI_EXP_DEVCAP2:
+	case PCI_EXP_DEVCTL2:
+	case PCI_EXP_LNKCAP2:
+	case PCI_EXP_LNKCTL2:
+	case PCI_EXP_LNKSTA2:
+		return pcie_cap_version(dev) > 1;
+	default:
+		return false;
+	}
+}
+
+/*
+ * Note that these accessor functions are only for the "PCI Express
+ * Capability" (see PCIe spec r3.0, sec 7.8).  They do not apply to the
+ * other "PCI Express Extended Capabilities" (AER, VC, ACS, MFVC, etc.)
+ */
+int pcie_capability_read_word(struct pci_dev *dev, int pos, u16 *val)
+{
+	int ret;
+
+	*val = 0;
+	if (pos & 1)
+		return -EINVAL;
+
+	if (pcie_capability_reg_implemented(dev, pos)) {
+		ret = pci_read_config_word(dev, pci_pcie_cap(dev) + pos, val);
+		/*
+		 * Reset *val to 0 if pci_read_config_word() fails, it may
+		 * have been written as 0xFFFF if hardware error happens
+		 * during pci_read_config_word().
+		 */
+		if (ret)
+			*val = 0;
+		return ret;
+	}
+
+	/*
+	 * For Functions that do not implement the Slot Capabilities,
+	 * Slot Status, and Slot Control registers, these spaces must
+	 * be hardwired to 0b, with the exception of the Presence Detect
+	 * State bit in the Slot Status register of Downstream Ports,
+	 * which must be hardwired to 1b.  (PCIe Base Spec 3.0, sec 7.8)
+	 */
+	if (pci_is_pcie(dev) && pos == PCI_EXP_SLTSTA &&
+		 pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM) {
+		*val = PCI_EXP_SLTSTA_PDS;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(pcie_capability_read_word);
+
+int pcie_capability_read_dword(struct pci_dev *dev, int pos, u32 *val)
+{
+	int ret;
+
+	*val = 0;
+	if (pos & 3)
+		return -EINVAL;
+
+	if (pcie_capability_reg_implemented(dev, pos)) {
+		ret = pci_read_config_dword(dev, pci_pcie_cap(dev) + pos, val);
+		/*
+		 * Reset *val to 0 if pci_read_config_dword() fails, it may
+		 * have been written as 0xFFFFFFFF if hardware error happens
+		 * during pci_read_config_dword().
+		 */
+		if (ret)
+			*val = 0;
+		return ret;
+	}
+
+	if (pci_is_pcie(dev) && pos == PCI_EXP_SLTCTL &&
+		 pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM) {
+		*val = PCI_EXP_SLTSTA_PDS;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(pcie_capability_read_dword);
+
+int pcie_capability_write_word(struct pci_dev *dev, int pos, u16 val)
+{
+	if (pos & 1)
+		return -EINVAL;
+
+	if (!pcie_capability_reg_implemented(dev, pos))
+		return 0;
+
+	return pci_write_config_word(dev, pci_pcie_cap(dev) + pos, val);
+}
+EXPORT_SYMBOL(pcie_capability_write_word);
+
+int pcie_capability_write_dword(struct pci_dev *dev, int pos, u32 val)
+{
+	if (pos & 3)
+		return -EINVAL;
+
+	if (!pcie_capability_reg_implemented(dev, pos))
+		return 0;
+
+	return pci_write_config_dword(dev, pci_pcie_cap(dev) + pos, val);
+}
+EXPORT_SYMBOL(pcie_capability_write_dword);
+
+int pcie_capability_clear_and_set_word(struct pci_dev *dev, int pos,
+				       u16 clear, u16 set)
+{
+	int ret;
+	u16 val;
+
+	ret = pcie_capability_read_word(dev, pos, &val);
+	if (!ret) {
+		val &= ~clear;
+		val |= set;
+		ret = pcie_capability_write_word(dev, pos, val);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(pcie_capability_clear_and_set_word);
+
+int pcie_capability_clear_and_set_dword(struct pci_dev *dev, int pos,
+					u32 clear, u32 set)
+{
+	int ret;
+	u32 val;
+
+	ret = pcie_capability_read_dword(dev, pos, &val);
+	if (!ret) {
+		val &= ~clear;
+		val |= set;
+		ret = pcie_capability_write_dword(dev, pos, val);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(pcie_capability_clear_and_set_dword);
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 4b0970b46e0b..6241fd05bd41 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -87,11 +87,15 @@ EXPORT_SYMBOL_GPL(pci_bus_resource_n);
 void pci_bus_remove_resources(struct pci_bus *bus)
 {
 	int i;
+	struct pci_bus_resource *bus_res, *tmp;
 
 	for (i = 0; i < PCI_BRIDGE_RESOURCE_NUM; i++)
 		bus->resource[i] = NULL;
 
-	pci_free_resource_list(&bus->resources);
+	list_for_each_entry_safe(bus_res, tmp, &bus->resources, list) {
+		list_del(&bus_res->list);
+		kfree(bus_res);
+	}
 }
 
 /**
diff --git a/drivers/pci/hotplug/Kconfig b/drivers/pci/hotplug/Kconfig
index 66f29bc00be4..b0e46dede1a9 100644
--- a/drivers/pci/hotplug/Kconfig
+++ b/drivers/pci/hotplug/Kconfig
@@ -17,28 +17,6 @@ menuconfig HOTPLUG_PCI
 
 if HOTPLUG_PCI
 
-config HOTPLUG_PCI_FAKE
-	tristate "Fake PCI Hotplug driver"
-	help
-	  Say Y here if you want to use the fake PCI hotplug driver. It can
-	  be used to simulate PCI hotplug events if even if your system is
-	  not PCI hotplug capable.
-
-	  This driver will "emulate" removing PCI devices from the system.
-	  If the "power" file is written to with "0" then the specified PCI
-	  device will be completely removed from the kernel.
-
-	  WARNING, this does NOT turn off the power to the PCI device.
-	  This is a "logical" removal, not a physical or electrical
-	  removal.
-
-	  Use this module at your own risk.  You have been warned!
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called fakephp.
-
-	  When in doubt, say N.
-
 config HOTPLUG_PCI_COMPAQ
 	tristate "Compaq PCI Hotplug driver"
 	depends on X86 && PCI_BIOS
@@ -143,7 +121,7 @@ config HOTPLUG_PCI_SHPC
 
 config HOTPLUG_PCI_RPA
 	tristate "RPA PCI Hotplug driver"
-	depends on PPC_PSERIES && EEH && !HOTPLUG_PCI_FAKE
+	depends on PPC_PSERIES && EEH
 	help
 	  Say Y here if you have a RPA system that supports PCI Hotplug.
 
diff --git a/drivers/pci/hotplug/Makefile b/drivers/pci/hotplug/Makefile
index 6cd9f3c9887d..c459cd4e39c2 100644
--- a/drivers/pci/hotplug/Makefile
+++ b/drivers/pci/hotplug/Makefile
@@ -23,9 +23,6 @@ obj-$(CONFIG_HOTPLUG_PCI_ACPI)		+= acpiphp.o
 
 obj-$(CONFIG_HOTPLUG_PCI_ACPI_IBM)	+= acpiphp_ibm.o
 
-# Link this last so it doesn't claim devices that have a real hotplug driver
-obj-$(CONFIG_HOTPLUG_PCI_FAKE)		+= fakephp.o
-
 pci_hotplug-objs	:=	pci_hotplug_core.o pcihp_slot.o
 
 ifdef CONFIG_HOTPLUG_PCI_CPCI
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index ad6fd6695495..3d6d4fd1e3c5 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -115,6 +115,35 @@ static const struct acpi_dock_ops acpiphp_dock_ops = {
 	.handler = handle_hotplug_event_func,
 };
 
+/* Check whether the PCI device is managed by native PCIe hotplug driver */
+static bool device_is_managed_by_native_pciehp(struct pci_dev *pdev)
+{
+	u32 reg32;
+	acpi_handle tmp;
+	struct acpi_pci_root *root;
+
+	/* Check whether the PCIe port supports native PCIe hotplug */
+	if (pcie_capability_read_dword(pdev, PCI_EXP_SLTCAP, &reg32))
+		return false;
+	if (!(reg32 & PCI_EXP_SLTCAP_HPC))
+		return false;
+
+	/*
+	 * Check whether native PCIe hotplug has been enabled for
+	 * this PCIe hierarchy.
+	 */
+	tmp = acpi_find_root_bridge_handle(pdev);
+	if (!tmp)
+		return false;
+	root = acpi_pci_find_root(tmp);
+	if (!root)
+		return false;
+	if (!(root->osc_control_set & OSC_PCI_EXPRESS_NATIVE_HP_CONTROL))
+		return false;
+
+	return true;
+}
+
 /* callback routine to register each ACPI PCI slot object */
 static acpi_status
 register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
@@ -142,16 +171,8 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	function = adr & 0xffff;
 
 	pdev = pbus->self;
-	if (pdev && pci_is_pcie(pdev)) {
-		tmp = acpi_find_root_bridge_handle(pdev);
-		if (tmp) {
-			struct acpi_pci_root *root = acpi_pci_find_root(tmp);
-
-			if (root && (root->osc_control_set &
-					OSC_PCI_EXPRESS_NATIVE_HP_CONTROL))
-				return AE_OK;
-		}
-	}
+	if (pdev && device_is_managed_by_native_pciehp(pdev))
+		return AE_OK;
 
 	newfunc = kzalloc(sizeof(struct acpiphp_func), GFP_KERNEL);
 	if (!newfunc)
@@ -382,10 +403,10 @@ static inline void config_p2p_bridge_flags(struct acpiphp_bridge *bridge)
 
 
 /* allocate and initialize host bridge data structure */
-static void add_host_bridge(acpi_handle *handle)
+static void add_host_bridge(struct acpi_pci_root *root)
 {
 	struct acpiphp_bridge *bridge;
-	struct acpi_pci_root *root = acpi_pci_find_root(handle);
+	acpi_handle handle = root->device->handle;
 
 	bridge = kzalloc(sizeof(struct acpiphp_bridge), GFP_KERNEL);
 	if (bridge == NULL)
@@ -468,11 +489,12 @@ find_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
 
 
 /* find hot-pluggable slots, and then find P2P bridge */
-static int add_bridge(acpi_handle handle)
+static int add_bridge(struct acpi_pci_root *root)
 {
 	acpi_status status;
 	unsigned long long tmp;
 	acpi_handle dummy_handle;
+	acpi_handle handle = root->device->handle;
 
 	/* if the bridge doesn't have _STA, we assume it is always there */
 	status = acpi_get_handle(handle, "_STA", &dummy_handle);
@@ -490,7 +512,7 @@ static int add_bridge(acpi_handle handle)
 	/* check if this bridge has ejectable slots */
 	if (detect_ejectable_slots(handle) > 0) {
 		dbg("found PCI host-bus bridge with hot-pluggable slots\n");
-		add_host_bridge(handle);
+		add_host_bridge(root);
 	}
 
 	/* search P2P bridges under this host bridge */
@@ -588,9 +610,10 @@ cleanup_p2p_bridge(acpi_handle handle, u32 lvl, void *context, void **rv)
 	return AE_OK;
 }
 
-static void remove_bridge(acpi_handle handle)
+static void remove_bridge(struct acpi_pci_root *root)
 {
 	struct acpiphp_bridge *bridge;
+	acpi_handle handle = root->device->handle;
 
 	/* cleanup p2p bridges under this host bridge
 	   in a depth-first manner */
@@ -869,17 +892,6 @@ static int __ref enable_device(struct acpiphp_slot *slot)
 	return retval;
 }
 
-static void disable_bridges(struct pci_bus *bus)
-{
-	struct pci_dev *dev;
-	list_for_each_entry(dev, &bus->devices, bus_list) {
-		if (dev->subordinate) {
-			disable_bridges(dev->subordinate);
-			pci_disable_device(dev);
-		}
-	}
-}
-
 /* return first device in slot, acquiring a reference on it */
 static struct pci_dev *dev_in_slot(struct acpiphp_slot *slot)
 {
@@ -931,12 +943,7 @@ static int disable_device(struct acpiphp_slot *slot)
 	 * here.
 	 */
 	while ((pdev = dev_in_slot(slot))) {
-		pci_stop_bus_device(pdev);
-		if (pdev->subordinate) {
-			disable_bridges(pdev->subordinate);
-			pci_disable_device(pdev);
-		}
-		__pci_remove_bus_device(pdev);
+		pci_stop_and_remove_bus_device(pdev);
 		pci_dev_put(pdev);
 	}
 
@@ -1477,34 +1484,6 @@ int __init acpiphp_get_num_slots(void)
 }
 
 
-#if 0
-/**
- * acpiphp_for_each_slot - call function for each slot
- * @fn: callback function
- * @data: context to be passed to callback function
- */
-static int acpiphp_for_each_slot(acpiphp_callback fn, void *data)
-{
-	struct list_head *node;
-	struct acpiphp_bridge *bridge;
-	struct acpiphp_slot *slot;
-	int retval = 0;
-
-	list_for_each (node, &bridge_list) {
-		bridge = (struct acpiphp_bridge *)node;
-		for (slot = bridge->slots; slot; slot = slot->next) {
-			retval = fn(slot, data);
-			if (!retval)
-				goto err_exit;
-		}
-	}
-
- err_exit:
-	return retval;
-}
-#endif
-
-
 /**
  * acpiphp_enable_slot - power on slot
  * @slot: ACPI PHP slot
diff --git a/drivers/pci/hotplug/cpcihp_generic.c b/drivers/pci/hotplug/cpcihp_generic.c
index 81af764c629b..a6a71c41cdf8 100644
--- a/drivers/pci/hotplug/cpcihp_generic.c
+++ b/drivers/pci/hotplug/cpcihp_generic.c
@@ -154,12 +154,8 @@ static int __init cpcihp_generic_init(void)
 	if(!r)
 		return -EBUSY;
 
-	bus = pci_find_bus(0, bridge_busnr);
-	if (!bus) {
-		err("Invalid bus number %d", bridge_busnr);
-		return -EINVAL;
-	}
-	dev = pci_get_slot(bus, PCI_DEVFN(bridge_slot, 0));
+	dev = pci_get_domain_bus_and_slot(0, bridge_busnr,
+					  PCI_DEVFN(bridge_slot, 0));
 	if(!dev || dev->hdr_type != PCI_HEADER_TYPE_BRIDGE) {
 		err("Invalid bridge device %s", bridge);
 		pci_dev_put(dev);
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index e43908d9b5df..36112fe212d3 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -2890,27 +2890,8 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 						func->mem_head = mem_node;
 					} else
 						return -ENOMEM;
-				} else if ((temp_register & 0x0BL) == 0x04) {
-					/* Map memory */
-					base = temp_register & 0xFFFFFFF0;
-					base = ~base + 1;
-
-					dbg("CND:      length = 0x%x\n", base);
-					mem_node = get_resource(&(resources->mem_head), base);
-
-					/* allocate the resource to the board */
-					if (mem_node) {
-						base = mem_node->base;
-
-						mem_node->next = func->mem_head;
-						func->mem_head = mem_node;
-					} else
-						return -ENOMEM;
-				} else if ((temp_register & 0x0BL) == 0x06) {
-					/* Those bits are reserved, we can't handle this */
-					return 1;
 				} else {
-					/* Requesting space below 1M */
+					/* Reserved bits or requesting space below 1M */
 					return NOT_ENOUGH_RESOURCES;
 				}
 
diff --git a/drivers/pci/hotplug/fakephp.c b/drivers/pci/hotplug/fakephp.c
deleted file mode 100644
index a019c9a712be..000000000000
--- a/drivers/pci/hotplug/fakephp.c
+++ /dev/null
@@ -1,164 +0,0 @@
-/* Works like the fakephp driver used to, except a little better.
- *
- * - It's possible to remove devices with subordinate busses.
- * - New PCI devices that appear via any method, not just a fakephp triggered
- *   rescan, will be noticed.
- * - Devices that are removed via any method, not just a fakephp triggered
- *   removal, will also be noticed.
- *
- * Uses nothing from the pci-hotplug subsystem.
- *
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/list.h>
-#include <linux/kobject.h>
-#include <linux/sysfs.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-#include "../pci.h"
-
-struct legacy_slot {
-	struct kobject		kobj;
-	struct pci_dev		*dev;
-	struct list_head	list;
-};
-
-static LIST_HEAD(legacy_list);
-
-static ssize_t legacy_show(struct kobject *kobj, struct attribute *attr,
-			   char *buf)
-{
-	struct legacy_slot *slot = container_of(kobj, typeof(*slot), kobj);
-	strcpy(buf, "1\n");
-	return 2;
-}
-
-static void remove_callback(void *data)
-{
-	pci_stop_and_remove_bus_device((struct pci_dev *)data);
-}
-
-static ssize_t legacy_store(struct kobject *kobj, struct attribute *attr,
-			    const char *buf, size_t len)
-{
-	struct legacy_slot *slot = container_of(kobj, typeof(*slot), kobj);
-	unsigned long val;
-
-	if (strict_strtoul(buf, 0, &val) < 0)
-		return -EINVAL;
-
-	if (val)
-		pci_rescan_bus(slot->dev->bus);
-	else
-		sysfs_schedule_callback(&slot->dev->dev.kobj, remove_callback,
-					slot->dev, THIS_MODULE);
-	return len;
-}
-
-static struct attribute *legacy_attrs[] = {
-	&(struct attribute){ .name = "power", .mode = 0644 },
-	NULL,
-};
-
-static void legacy_release(struct kobject *kobj)
-{
-	struct legacy_slot *slot = container_of(kobj, typeof(*slot), kobj);
-
-	pci_dev_put(slot->dev);
-	kfree(slot);
-}
-
-static struct kobj_type legacy_ktype = {
-	.sysfs_ops = &(const struct sysfs_ops){
-		.store = legacy_store, .show = legacy_show
-	},
-	.release = &legacy_release,
-	.default_attrs = legacy_attrs,
-};
-
-static int legacy_add_slot(struct pci_dev *pdev)
-{
-	struct legacy_slot *slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-
-	if (!slot)
-		return -ENOMEM;
-
-	if (kobject_init_and_add(&slot->kobj, &legacy_ktype,
-				 &pci_slots_kset->kobj, "%s",
-				 dev_name(&pdev->dev))) {
-		dev_warn(&pdev->dev, "Failed to created legacy fake slot\n");
-		return -EINVAL;
-	}
-	slot->dev = pci_dev_get(pdev);
-
-	list_add(&slot->list, &legacy_list);
-
-	return 0;
-}
-
-static int legacy_notify(struct notifier_block *nb,
-			 unsigned long action, void *data)
-{
-	struct pci_dev *pdev = to_pci_dev(data);
-
-	if (action == BUS_NOTIFY_ADD_DEVICE) {
-		legacy_add_slot(pdev);
-	} else if (action == BUS_NOTIFY_DEL_DEVICE) {
-		struct legacy_slot *slot;
-
-		list_for_each_entry(slot, &legacy_list, list)
-			if (slot->dev == pdev)
-				goto found;
-
-		dev_warn(&pdev->dev, "Missing legacy fake slot?");
-		return -ENODEV;
-found:
-		kobject_del(&slot->kobj);
-		list_del(&slot->list);
-		kobject_put(&slot->kobj);
-	}
-
-	return 0;
-}
-
-static struct notifier_block legacy_notifier = {
-	.notifier_call = legacy_notify
-};
-
-static int __init init_legacy(void)
-{
-	struct pci_dev *pdev = NULL;
-
-	/* Add existing devices */
-	for_each_pci_dev(pdev)
-		legacy_add_slot(pdev);
-
-	/* Be alerted of any new ones */
-	bus_register_notifier(&pci_bus_type, &legacy_notifier);
-	return 0;
-}
-module_init(init_legacy);
-
-static void __exit remove_legacy(void)
-{
-	struct legacy_slot *slot, *tmp;
-
-	bus_unregister_notifier(&pci_bus_type, &legacy_notifier);
-
-	list_for_each_entry_safe(slot, tmp, &legacy_list, list) {
-		list_del(&slot->list);
-		kobject_del(&slot->kobj);
-		kobject_put(&slot->kobj);
-	}
-}
-module_exit(remove_legacy);
-
-
-MODULE_AUTHOR("Trent Piepho <xyzzy@speakeasy.org>");
-MODULE_DESCRIPTION("Legacy version of the fakephp interface");
-MODULE_LICENSE("GPL");
diff --git a/drivers/pci/hotplug/pciehp_acpi.c b/drivers/pci/hotplug/pciehp_acpi.c
index 376d70d17176..24d709b7388c 100644
--- a/drivers/pci/hotplug/pciehp_acpi.c
+++ b/drivers/pci/hotplug/pciehp_acpi.c
@@ -81,16 +81,12 @@ static struct list_head __initdata dummy_slots = LIST_HEAD_INIT(dummy_slots);
 /* Dummy driver for dumplicate name detection */
 static int __init dummy_probe(struct pcie_device *dev)
 {
-	int pos;
 	u32 slot_cap;
 	acpi_handle handle;
 	struct dummy_slot *slot, *tmp;
 	struct pci_dev *pdev = dev->port;
 
-	pos = pci_pcie_cap(pdev);
-	if (!pos)
-		return -ENODEV;
-	pci_read_config_dword(pdev, pos + PCI_EXP_SLTCAP, &slot_cap);
+	pcie_capability_read_dword(pdev, PCI_EXP_SLTCAP, &slot_cap);
 	slot = kzalloc(sizeof(*slot), GFP_KERNEL);
 	if (!slot)
 		return -ENOMEM;
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 365c6b96c642..916bf4f53aba 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -300,24 +300,24 @@ static int pciehp_suspend (struct pcie_device *dev)
 
 static int pciehp_resume (struct pcie_device *dev)
 {
+	struct controller *ctrl;
+	struct slot *slot;
+	u8 status;
+
 	dev_info(&dev->device, "%s ENTRY\n", __func__);
-	if (pciehp_force) {
-		struct controller *ctrl = get_service_data(dev);
-		struct slot *slot;
-		u8 status;
+	ctrl = get_service_data(dev);
 
-		/* reinitialize the chipset's event detection logic */
-		pcie_enable_notification(ctrl);
+	/* reinitialize the chipset's event detection logic */
+	pcie_enable_notification(ctrl);
 
-		slot = ctrl->slot;
+	slot = ctrl->slot;
 
-		/* Check if slot is occupied */
-		pciehp_get_adapter_status(slot, &status);
-		if (status)
-			pciehp_enable_slot(slot);
-		else
-			pciehp_disable_slot(slot);
-	}
+	/* Check if slot is occupied */
+	pciehp_get_adapter_status(slot, &status);
+	if (status)
+		pciehp_enable_slot(slot);
+	else
+		pciehp_disable_slot(slot);
 	return 0;
 }
 #endif /* PM */
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 302451e8289d..13b2eaf7ba43 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -44,25 +44,25 @@
 static inline int pciehp_readw(struct controller *ctrl, int reg, u16 *value)
 {
 	struct pci_dev *dev = ctrl->pcie->port;
-	return pci_read_config_word(dev, pci_pcie_cap(dev) + reg, value);
+	return pcie_capability_read_word(dev, reg, value);
 }
 
 static inline int pciehp_readl(struct controller *ctrl, int reg, u32 *value)
 {
 	struct pci_dev *dev = ctrl->pcie->port;
-	return pci_read_config_dword(dev, pci_pcie_cap(dev) + reg, value);
+	return pcie_capability_read_dword(dev, reg, value);
 }
 
 static inline int pciehp_writew(struct controller *ctrl, int reg, u16 value)
 {
 	struct pci_dev *dev = ctrl->pcie->port;
-	return pci_write_config_word(dev, pci_pcie_cap(dev) + reg, value);
+	return pcie_capability_write_word(dev, reg, value);
 }
 
 static inline int pciehp_writel(struct controller *ctrl, int reg, u32 value)
 {
 	struct pci_dev *dev = ctrl->pcie->port;
-	return pci_write_config_dword(dev, pci_pcie_cap(dev) + reg, value);
+	return pcie_capability_write_dword(dev, reg, value);
 }
 
 /* Power Control Command */
@@ -855,10 +855,6 @@ struct controller *pcie_init(struct pcie_device *dev)
 		goto abort;
 	}
 	ctrl->pcie = dev;
-	if (!pci_pcie_cap(pdev)) {
-		ctrl_err(ctrl, "Cannot find PCI Express capability\n");
-		goto abort_ctrl;
-	}
 	if (pciehp_readl(ctrl, PCI_EXP_SLTCAP, &slot_cap)) {
 		ctrl_err(ctrl, "Cannot read SLOTCAP register\n");
 		goto abort_ctrl;
diff --git a/drivers/pci/hotplug/pcihp_slot.c b/drivers/pci/hotplug/pcihp_slot.c
index 8c05a18c9770..fec2d5b75440 100644
--- a/drivers/pci/hotplug/pcihp_slot.c
+++ b/drivers/pci/hotplug/pcihp_slot.c
@@ -96,17 +96,11 @@ static void program_hpp_type1(struct pci_dev *dev, struct hpp_type1 *hpp)
 static void program_hpp_type2(struct pci_dev *dev, struct hpp_type2 *hpp)
 {
 	int pos;
-	u16 reg16;
 	u32 reg32;
 
 	if (!hpp)
 		return;
 
-	/* Find PCI Express capability */
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return;
-
 	if (hpp->revision > 1) {
 		dev_warn(&dev->dev, "PCIe settings rev %d not supported\n",
 			 hpp->revision);
@@ -114,17 +108,13 @@ static void program_hpp_type2(struct pci_dev *dev, struct hpp_type2 *hpp)
 	}
 
 	/* Initialize Device Control Register */
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &reg16);
-	reg16 = (reg16 & hpp->pci_exp_devctl_and) | hpp->pci_exp_devctl_or;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, reg16);
+	pcie_capability_clear_and_set_word(dev, PCI_EXP_DEVCTL,
+			~hpp->pci_exp_devctl_and, hpp->pci_exp_devctl_or);
 
 	/* Initialize Link Control Register */
-	if (dev->subordinate) {
-		pci_read_config_word(dev, pos + PCI_EXP_LNKCTL, &reg16);
-		reg16 = (reg16 & hpp->pci_exp_lnkctl_and)
-			| hpp->pci_exp_lnkctl_or;
-		pci_write_config_word(dev, pos + PCI_EXP_LNKCTL, reg16);
-	}
+	if (dev->subordinate)
+		pcie_capability_clear_and_set_word(dev, PCI_EXP_LNKCTL,
+			~hpp->pci_exp_lnkctl_and, hpp->pci_exp_lnkctl_or);
 
 	/* Find Advanced Error Reporting Enhanced Capability */
 	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index 74bbaf82638d..aeccc911abb8 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -433,8 +433,8 @@ static int sriov_init(struct pci_dev *dev, int pos)
 	struct resource *res;
 	struct pci_dev *pdev;
 
-	if (dev->pcie_type != PCI_EXP_TYPE_RC_END &&
-	    dev->pcie_type != PCI_EXP_TYPE_ENDPOINT)
+	if (pci_pcie_type(dev) != PCI_EXP_TYPE_RC_END &&
+	    pci_pcie_type(dev) != PCI_EXP_TYPE_ENDPOINT)
 		return -ENODEV;
 
 	pci_read_config_word(dev, pos + PCI_SRIOV_CTRL, &ctrl);
@@ -503,7 +503,7 @@ found:
 	iov->self = dev;
 	pci_read_config_dword(dev, pos + PCI_SRIOV_CAP, &iov->cap);
 	pci_read_config_byte(dev, pos + PCI_SRIOV_FUNC_LINK, &iov->link);
-	if (dev->pcie_type == PCI_EXP_TYPE_RC_END)
+	if (pci_pcie_type(dev) == PCI_EXP_TYPE_RC_END)
 		iov->link = PCI_DEVFN(PCI_SLOT(dev->devfn), iov->link);
 
 	if (pdev)
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index d6fd6b6d9d4b..9e1d2959e226 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -139,7 +139,6 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 		return retval;
 	return count;
 }
-static DRIVER_ATTR(new_id, S_IWUSR, NULL, store_new_id);
 
 /**
  * store_remove_id - remove a PCI device ID from this driver
@@ -185,38 +184,16 @@ store_remove_id(struct device_driver *driver, const char *buf, size_t count)
 		return retval;
 	return count;
 }
-static DRIVER_ATTR(remove_id, S_IWUSR, NULL, store_remove_id);
 
-static int
-pci_create_newid_files(struct pci_driver *drv)
-{
-	int error = 0;
+static struct driver_attribute pci_drv_attrs[] = {
+	__ATTR(new_id, S_IWUSR, NULL, store_new_id),
+	__ATTR(remove_id, S_IWUSR, NULL, store_remove_id),
+	__ATTR_NULL,
+};
 
-	if (drv->probe != NULL) {
-		error = driver_create_file(&drv->driver, &driver_attr_new_id);
-		if (error == 0) {
-			error = driver_create_file(&drv->driver,
-					&driver_attr_remove_id);
-			if (error)
-				driver_remove_file(&drv->driver,
-						&driver_attr_new_id);
-		}
-	}
-	return error;
-}
-
-static void pci_remove_newid_files(struct pci_driver *drv)
-{
-	driver_remove_file(&drv->driver, &driver_attr_remove_id);
-	driver_remove_file(&drv->driver, &driver_attr_new_id);
-}
-#else /* !CONFIG_HOTPLUG */
-static inline int pci_create_newid_files(struct pci_driver *drv)
-{
-	return 0;
-}
-static inline void pci_remove_newid_files(struct pci_driver *drv) {}
-#endif
+#else
+#define pci_drv_attrs	NULL
+#endif /* CONFIG_HOTPLUG */
 
 /**
  * pci_match_id - See if a pci device matches a given pci_id table
@@ -1162,8 +1139,6 @@ const struct dev_pm_ops pci_dev_pm_ops = {
 int __pci_register_driver(struct pci_driver *drv, struct module *owner,
 			  const char *mod_name)
 {
-	int error;
-
 	/* initialize common driver fields */
 	drv->driver.name = drv->name;
 	drv->driver.bus = &pci_bus_type;
@@ -1174,19 +1149,7 @@ int __pci_register_driver(struct pci_driver *drv, struct module *owner,
 	INIT_LIST_HEAD(&drv->dynids.list);
 
 	/* register with core */
-	error = driver_register(&drv->driver);
-	if (error)
-		goto out;
-
-	error = pci_create_newid_files(drv);
-	if (error)
-		goto out_newid;
-out:
-	return error;
-
-out_newid:
-	driver_unregister(&drv->driver);
-	goto out;
+	return driver_register(&drv->driver);
 }
 
 /**
@@ -1202,7 +1165,6 @@ out_newid:
 void
 pci_unregister_driver(struct pci_driver *drv)
 {
-	pci_remove_newid_files(drv);
 	driver_unregister(&drv->driver);
 	pci_free_dynids(drv);
 }
@@ -1302,6 +1264,7 @@ struct bus_type pci_bus_type = {
 	.shutdown	= pci_device_shutdown,
 	.dev_attrs	= pci_dev_attrs,
 	.bus_attrs	= pci_bus_attrs,
+	.drv_attrs	= pci_drv_attrs,
 	.pm		= PCI_PM_OPS_PTR,
 };
 
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index ab4bf5a4c2f1..54858838f098 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -254,52 +254,17 @@ int pci_bus_find_capability(struct pci_bus *bus, unsigned int devfn, int cap)
 }
 
 /**
- * pci_pcie_cap2 - query for devices' PCI_CAP_ID_EXP v2 capability structure
- * @dev: PCI device to check
- *
- * Like pci_pcie_cap() but also checks that the PCIe capability version is
- * >= 2.  Note that v1 capability structures could be sparse in that not
- * all register fields were required.  v2 requires the entire structure to
- * be present size wise, while still allowing for non-implemented registers
- * to exist but they must be hardwired to 0.
- *
- * Due to the differences in the versions of capability structures, one
- * must be careful not to try and access non-existant registers that may
- * exist in early versions - v1 - of Express devices.
- *
- * Returns the offset of the PCIe capability structure as long as the
- * capability version is >= 2; otherwise 0 is returned.
- */
-static int pci_pcie_cap2(struct pci_dev *dev)
-{
-	u16 flags;
-	int pos;
-
-	pos = pci_pcie_cap(dev);
-	if (pos) {
-		pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &flags);
-		if ((flags & PCI_EXP_FLAGS_VERS) < 2)
-			pos = 0;
-	}
-
-	return pos;
-}
-
-/**
- * pci_find_ext_capability - Find an extended capability
+ * pci_find_next_ext_capability - Find an extended capability
  * @dev: PCI device to query
+ * @start: address at which to start looking (0 to start at beginning of list)
  * @cap: capability code
  *
- * Returns the address of the requested extended capability structure
+ * Returns the address of the next matching extended capability structure
  * within the device's PCI configuration space or 0 if the device does
- * not support it.  Possible values for @cap:
- *
- *  %PCI_EXT_CAP_ID_ERR		Advanced Error Reporting
- *  %PCI_EXT_CAP_ID_VC		Virtual Channel
- *  %PCI_EXT_CAP_ID_DSN		Device Serial Number
- *  %PCI_EXT_CAP_ID_PWR		Power Budgeting
+ * not support it.  Some capabilities can occur several times, e.g., the
+ * vendor-specific capability, and this provides a way to find them all.
  */
-int pci_find_ext_capability(struct pci_dev *dev, int cap)
+int pci_find_next_ext_capability(struct pci_dev *dev, int start, int cap)
 {
 	u32 header;
 	int ttl;
@@ -311,6 +276,9 @@ int pci_find_ext_capability(struct pci_dev *dev, int cap)
 	if (dev->cfg_size <= PCI_CFG_SPACE_SIZE)
 		return 0;
 
+	if (start)
+		pos = start;
+
 	if (pci_read_config_dword(dev, pos, &header) != PCIBIOS_SUCCESSFUL)
 		return 0;
 
@@ -322,7 +290,7 @@ int pci_find_ext_capability(struct pci_dev *dev, int cap)
 		return 0;
 
 	while (ttl-- > 0) {
-		if (PCI_EXT_CAP_ID(header) == cap)
+		if (PCI_EXT_CAP_ID(header) == cap && pos != start)
 			return pos;
 
 		pos = PCI_EXT_CAP_NEXT(header);
@@ -335,6 +303,26 @@ int pci_find_ext_capability(struct pci_dev *dev, int cap)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(pci_find_next_ext_capability);
+
+/**
+ * pci_find_ext_capability - Find an extended capability
+ * @dev: PCI device to query
+ * @cap: capability code
+ *
+ * Returns the address of the requested extended capability structure
+ * within the device's PCI configuration space or 0 if the device does
+ * not support it.  Possible values for @cap:
+ *
+ *  %PCI_EXT_CAP_ID_ERR		Advanced Error Reporting
+ *  %PCI_EXT_CAP_ID_VC		Virtual Channel
+ *  %PCI_EXT_CAP_ID_DSN		Device Serial Number
+ *  %PCI_EXT_CAP_ID_PWR		Power Budgeting
+ */
+int pci_find_ext_capability(struct pci_dev *dev, int cap)
+{
+	return pci_find_next_ext_capability(dev, 0, cap);
+}
 EXPORT_SYMBOL_GPL(pci_find_ext_capability);
 
 static int __pci_find_next_ht_cap(struct pci_dev *dev, int pos, int ht_cap)
@@ -854,21 +842,6 @@ EXPORT_SYMBOL(pci_choose_state);
 
 #define PCI_EXP_SAVE_REGS	7
 
-#define pcie_cap_has_devctl(type, flags)	1
-#define pcie_cap_has_lnkctl(type, flags)		\
-		((flags & PCI_EXP_FLAGS_VERS) > 1 ||	\
-		 (type == PCI_EXP_TYPE_ROOT_PORT ||	\
-		  type == PCI_EXP_TYPE_ENDPOINT ||	\
-		  type == PCI_EXP_TYPE_LEG_END))
-#define pcie_cap_has_sltctl(type, flags)		\
-		((flags & PCI_EXP_FLAGS_VERS) > 1 ||	\
-		 ((type == PCI_EXP_TYPE_ROOT_PORT) ||	\
-		  (type == PCI_EXP_TYPE_DOWNSTREAM &&	\
-		   (flags & PCI_EXP_FLAGS_SLOT))))
-#define pcie_cap_has_rtctl(type, flags)			\
-		((flags & PCI_EXP_FLAGS_VERS) > 1 ||	\
-		 (type == PCI_EXP_TYPE_ROOT_PORT ||	\
-		  type == PCI_EXP_TYPE_RC_EC))
 
 static struct pci_cap_saved_state *pci_find_saved_cap(
 	struct pci_dev *pci_dev, char cap)
@@ -885,13 +858,11 @@ static struct pci_cap_saved_state *pci_find_saved_cap(
 
 static int pci_save_pcie_state(struct pci_dev *dev)
 {
-	int pos, i = 0;
+	int i = 0;
 	struct pci_cap_saved_state *save_state;
 	u16 *cap;
-	u16 flags;
 
-	pos = pci_pcie_cap(dev);
-	if (!pos)
+	if (!pci_is_pcie(dev))
 		return 0;
 
 	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_EXP);
@@ -899,60 +870,37 @@ static int pci_save_pcie_state(struct pci_dev *dev)
 		dev_err(&dev->dev, "buffer not found in %s\n", __func__);
 		return -ENOMEM;
 	}
-	cap = (u16 *)&save_state->cap.data[0];
-
-	pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &flags);
 
-	if (pcie_cap_has_devctl(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &cap[i++]);
-	if (pcie_cap_has_lnkctl(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_LNKCTL, &cap[i++]);
-	if (pcie_cap_has_sltctl(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_SLTCTL, &cap[i++]);
-	if (pcie_cap_has_rtctl(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_RTCTL, &cap[i++]);
-
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return 0;
+	cap = (u16 *)&save_state->cap.data[0];
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL, &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_LNKCTL, &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_SLTCTL, &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_RTCTL,  &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL2, &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_LNKCTL2, &cap[i++]);
+	pcie_capability_read_word(dev, PCI_EXP_SLTCTL2, &cap[i++]);
 
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &cap[i++]);
-	pci_read_config_word(dev, pos + PCI_EXP_LNKCTL2, &cap[i++]);
-	pci_read_config_word(dev, pos + PCI_EXP_SLTCTL2, &cap[i++]);
 	return 0;
 }
 
 static void pci_restore_pcie_state(struct pci_dev *dev)
 {
-	int i = 0, pos;
+	int i = 0;
 	struct pci_cap_saved_state *save_state;
 	u16 *cap;
-	u16 flags;
 
 	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_EXP);
-	pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
-	if (!save_state || pos <= 0)
-		return;
-	cap = (u16 *)&save_state->cap.data[0];
-
-	pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &flags);
-
-	if (pcie_cap_has_devctl(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, cap[i++]);
-	if (pcie_cap_has_lnkctl(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_LNKCTL, cap[i++]);
-	if (pcie_cap_has_sltctl(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_SLTCTL, cap[i++]);
-	if (pcie_cap_has_rtctl(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_RTCTL, cap[i++]);
-
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
+	if (!save_state)
 		return;
 
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, cap[i++]);
-	pci_write_config_word(dev, pos + PCI_EXP_LNKCTL2, cap[i++]);
-	pci_write_config_word(dev, pos + PCI_EXP_SLTCTL2, cap[i++]);
+	cap = (u16 *)&save_state->cap.data[0];
+	pcie_capability_write_word(dev, PCI_EXP_DEVCTL, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_LNKCTL, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_SLTCTL, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_RTCTL, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_DEVCTL2, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_LNKCTL2, cap[i++]);
+	pcie_capability_write_word(dev, PCI_EXP_SLTCTL2, cap[i++]);
 }
 
 
@@ -1543,7 +1491,7 @@ void pci_pme_wakeup_bus(struct pci_bus *bus)
 
 /**
  * pci_wakeup - Wake up a PCI device
- * @dev: Device to handle.
+ * @pci_dev: Device to handle.
  * @ign: ignored parameter
  */
 static int pci_wakeup(struct pci_dev *pci_dev, void *ign)
@@ -2067,35 +2015,24 @@ void pci_free_cap_save_buffers(struct pci_dev *dev)
  */
 void pci_enable_ari(struct pci_dev *dev)
 {
-	int pos;
 	u32 cap;
-	u16 ctrl;
 	struct pci_dev *bridge;
 
 	if (pcie_ari_disabled || !pci_is_pcie(dev) || dev->devfn)
 		return;
 
-	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ARI);
-	if (!pos)
+	if (!pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ARI))
 		return;
 
 	bridge = dev->bus->self;
 	if (!bridge)
 		return;
 
-	/* ARI is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(bridge);
-	if (!pos)
-		return;
-
-	pci_read_config_dword(bridge, pos + PCI_EXP_DEVCAP2, &cap);
+	pcie_capability_read_dword(bridge, PCI_EXP_DEVCAP2, &cap);
 	if (!(cap & PCI_EXP_DEVCAP2_ARI))
 		return;
 
-	pci_read_config_word(bridge, pos + PCI_EXP_DEVCTL2, &ctrl);
-	ctrl |= PCI_EXP_DEVCTL2_ARI;
-	pci_write_config_word(bridge, pos + PCI_EXP_DEVCTL2, ctrl);
-
+	pcie_capability_set_word(bridge, PCI_EXP_DEVCTL2, PCI_EXP_DEVCTL2_ARI);
 	bridge->ari_enabled = 1;
 }
 
@@ -2110,20 +2047,14 @@ void pci_enable_ari(struct pci_dev *dev)
  */
 void pci_enable_ido(struct pci_dev *dev, unsigned long type)
 {
-	int pos;
-	u16 ctrl;
+	u16 ctrl = 0;
 
-	/* ID-based Ordering is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
 	if (type & PCI_EXP_IDO_REQUEST)
 		ctrl |= PCI_EXP_IDO_REQ_EN;
 	if (type & PCI_EXP_IDO_COMPLETION)
 		ctrl |= PCI_EXP_IDO_CMP_EN;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
+	if (ctrl)
+		pcie_capability_set_word(dev, PCI_EXP_DEVCTL2, ctrl);
 }
 EXPORT_SYMBOL(pci_enable_ido);
 
@@ -2134,20 +2065,14 @@ EXPORT_SYMBOL(pci_enable_ido);
  */
 void pci_disable_ido(struct pci_dev *dev, unsigned long type)
 {
-	int pos;
-	u16 ctrl;
+	u16 ctrl = 0;
 
-	/* ID-based Ordering is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
 	if (type & PCI_EXP_IDO_REQUEST)
-		ctrl &= ~PCI_EXP_IDO_REQ_EN;
+		ctrl |= PCI_EXP_IDO_REQ_EN;
 	if (type & PCI_EXP_IDO_COMPLETION)
-		ctrl &= ~PCI_EXP_IDO_CMP_EN;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
+		ctrl |= PCI_EXP_IDO_CMP_EN;
+	if (ctrl)
+		pcie_capability_clear_word(dev, PCI_EXP_DEVCTL2, ctrl);
 }
 EXPORT_SYMBOL(pci_disable_ido);
 
@@ -2172,17 +2097,11 @@ EXPORT_SYMBOL(pci_disable_ido);
  */
 int pci_enable_obff(struct pci_dev *dev, enum pci_obff_signal_type type)
 {
-	int pos;
 	u32 cap;
 	u16 ctrl;
 	int ret;
 
-	/* OBFF is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return -ENOTSUPP;
-
-	pci_read_config_dword(dev, pos + PCI_EXP_DEVCAP2, &cap);
+	pcie_capability_read_dword(dev, PCI_EXP_DEVCAP2, &cap);
 	if (!(cap & PCI_EXP_OBFF_MASK))
 		return -ENOTSUPP; /* no OBFF support at all */
 
@@ -2193,7 +2112,7 @@ int pci_enable_obff(struct pci_dev *dev, enum pci_obff_signal_type type)
 			return ret;
 	}
 
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL2, &ctrl);
 	if (cap & PCI_EXP_OBFF_WAKE)
 		ctrl |= PCI_EXP_OBFF_WAKE_EN;
 	else {
@@ -2211,7 +2130,7 @@ int pci_enable_obff(struct pci_dev *dev, enum pci_obff_signal_type type)
 			return -ENOTSUPP;
 		}
 	}
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
+	pcie_capability_write_word(dev, PCI_EXP_DEVCTL2, ctrl);
 
 	return 0;
 }
@@ -2225,17 +2144,7 @@ EXPORT_SYMBOL(pci_enable_obff);
  */
 void pci_disable_obff(struct pci_dev *dev)
 {
-	int pos;
-	u16 ctrl;
-
-	/* OBFF is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
-	ctrl &= ~PCI_EXP_OBFF_WAKE_EN;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
+	pcie_capability_clear_word(dev, PCI_EXP_DEVCTL2, PCI_EXP_OBFF_WAKE_EN);
 }
 EXPORT_SYMBOL(pci_disable_obff);
 
@@ -2248,15 +2157,9 @@ EXPORT_SYMBOL(pci_disable_obff);
  */
 static bool pci_ltr_supported(struct pci_dev *dev)
 {
-	int pos;
 	u32 cap;
 
-	/* LTR is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return false;
-
-	pci_read_config_dword(dev, pos + PCI_EXP_DEVCAP2, &cap);
+	pcie_capability_read_dword(dev, PCI_EXP_DEVCAP2, &cap);
 
 	return cap & PCI_EXP_DEVCAP2_LTR;
 }
@@ -2273,22 +2176,15 @@ static bool pci_ltr_supported(struct pci_dev *dev)
  */
 int pci_enable_ltr(struct pci_dev *dev)
 {
-	int pos;
-	u16 ctrl;
 	int ret;
 
-	if (!pci_ltr_supported(dev))
-		return -ENOTSUPP;
-
-	/* LTR is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return -ENOTSUPP;
-
 	/* Only primary function can enable/disable LTR */
 	if (PCI_FUNC(dev->devfn) != 0)
 		return -EINVAL;
 
+	if (!pci_ltr_supported(dev))
+		return -ENOTSUPP;
+
 	/* Enable upstream ports first */
 	if (dev->bus->self) {
 		ret = pci_enable_ltr(dev->bus->self);
@@ -2296,11 +2192,7 @@ int pci_enable_ltr(struct pci_dev *dev)
 			return ret;
 	}
 
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
-	ctrl |= PCI_EXP_LTR_EN;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
-
-	return 0;
+	return pcie_capability_set_word(dev, PCI_EXP_DEVCTL2, PCI_EXP_LTR_EN);
 }
 EXPORT_SYMBOL(pci_enable_ltr);
 
@@ -2310,24 +2202,14 @@ EXPORT_SYMBOL(pci_enable_ltr);
  */
 void pci_disable_ltr(struct pci_dev *dev)
 {
-	int pos;
-	u16 ctrl;
-
-	if (!pci_ltr_supported(dev))
-		return;
-
-	/* LTR is a PCIe cap v2 feature */
-	pos = pci_pcie_cap2(dev);
-	if (!pos)
-		return;
-
 	/* Only primary function can enable/disable LTR */
 	if (PCI_FUNC(dev->devfn) != 0)
 		return;
 
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &ctrl);
-	ctrl &= ~PCI_EXP_LTR_EN;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, ctrl);
+	if (!pci_ltr_supported(dev))
+		return;
+
+	pcie_capability_clear_word(dev, PCI_EXP_DEVCTL2, PCI_EXP_LTR_EN);
 }
 EXPORT_SYMBOL(pci_disable_ltr);
 
@@ -2410,9 +2292,6 @@ void pci_enable_acs(struct pci_dev *dev)
 	if (!pci_acs_enable)
 		return;
 
-	if (!pci_is_pcie(dev))
-		return;
-
 	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ACS);
 	if (!pos)
 		return;
@@ -2460,8 +2339,8 @@ bool pci_acs_enabled(struct pci_dev *pdev, u16 acs_flags)
 		acs_flags &= (PCI_ACS_RR | PCI_ACS_CR |
 			      PCI_ACS_EC | PCI_ACS_DT);
 
-	if (pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM ||
-	    pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT ||
+	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_DOWNSTREAM ||
+	    pci_pcie_type(pdev) == PCI_EXP_TYPE_ROOT_PORT ||
 	    pdev->multifunction) {
 		pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ACS);
 		if (!pos)
@@ -3177,15 +3056,10 @@ EXPORT_SYMBOL(pci_set_dma_seg_boundary);
 static int pcie_flr(struct pci_dev *dev, int probe)
 {
 	int i;
-	int pos;
 	u32 cap;
-	u16 status, control;
-
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return -ENOTTY;
+	u16 status;
 
-	pci_read_config_dword(dev, pos + PCI_EXP_DEVCAP, &cap);
+	pcie_capability_read_dword(dev, PCI_EXP_DEVCAP, &cap);
 	if (!(cap & PCI_EXP_DEVCAP_FLR))
 		return -ENOTTY;
 
@@ -3197,7 +3071,7 @@ static int pcie_flr(struct pci_dev *dev, int probe)
 		if (i)
 			msleep((1 << (i - 1)) * 100);
 
-		pci_read_config_word(dev, pos + PCI_EXP_DEVSTA, &status);
+		pcie_capability_read_word(dev, PCI_EXP_DEVSTA, &status);
 		if (!(status & PCI_EXP_DEVSTA_TRPND))
 			goto clear;
 	}
@@ -3206,9 +3080,7 @@ static int pcie_flr(struct pci_dev *dev, int probe)
 			"proceeding with reset anyway\n");
 
 clear:
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &control);
-	control |= PCI_EXP_DEVCTL_BCR_FLR;
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, control);
+	pcie_capability_set_word(dev, PCI_EXP_DEVCTL, PCI_EXP_DEVCTL_BCR_FLR);
 
 	msleep(100);
 
@@ -3576,18 +3448,11 @@ EXPORT_SYMBOL(pcix_set_mmrbc);
  */
 int pcie_get_readrq(struct pci_dev *dev)
 {
-	int ret, cap;
 	u16 ctl;
 
-	cap = pci_pcie_cap(dev);
-	if (!cap)
-		return -EINVAL;
-
-	ret = pci_read_config_word(dev, cap + PCI_EXP_DEVCTL, &ctl);
-	if (!ret)
-		ret = 128 << ((ctl & PCI_EXP_DEVCTL_READRQ) >> 12);
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL, &ctl);
 
-	return ret;
+	return 128 << ((ctl & PCI_EXP_DEVCTL_READRQ) >> 12);
 }
 EXPORT_SYMBOL(pcie_get_readrq);
 
@@ -3601,19 +3466,11 @@ EXPORT_SYMBOL(pcie_get_readrq);
  */
 int pcie_set_readrq(struct pci_dev *dev, int rq)
 {
-	int cap, err = -EINVAL;
-	u16 ctl, v;
+	u16 v;
 
 	if (rq < 128 || rq > 4096 || !is_power_of_2(rq))
-		goto out;
-
-	cap = pci_pcie_cap(dev);
-	if (!cap)
-		goto out;
+		return -EINVAL;
 
-	err = pci_read_config_word(dev, cap + PCI_EXP_DEVCTL, &ctl);
-	if (err)
-		goto out;
 	/*
 	 * If using the "performance" PCIe config, we clamp the
 	 * read rq size to the max packet size to prevent the
@@ -3631,14 +3488,8 @@ int pcie_set_readrq(struct pci_dev *dev, int rq)
 
 	v = (ffs(rq) - 8) << 12;
 
-	if ((ctl & PCI_EXP_DEVCTL_READRQ) != v) {
-		ctl &= ~PCI_EXP_DEVCTL_READRQ;
-		ctl |= v;
-		err = pci_write_config_word(dev, cap + PCI_EXP_DEVCTL, ctl);
-	}
-
-out:
-	return err;
+	return pcie_capability_clear_and_set_word(dev, PCI_EXP_DEVCTL,
+						  PCI_EXP_DEVCTL_READRQ, v);
 }
 EXPORT_SYMBOL(pcie_set_readrq);
 
@@ -3651,18 +3502,11 @@ EXPORT_SYMBOL(pcie_set_readrq);
  */
 int pcie_get_mps(struct pci_dev *dev)
 {
-	int ret, cap;
 	u16 ctl;
 
-	cap = pci_pcie_cap(dev);
-	if (!cap)
-		return -EINVAL;
-
-	ret = pci_read_config_word(dev, cap + PCI_EXP_DEVCTL, &ctl);
-	if (!ret)
-		ret = 128 << ((ctl & PCI_EXP_DEVCTL_PAYLOAD) >> 5);
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL, &ctl);
 
-	return ret;
+	return 128 << ((ctl & PCI_EXP_DEVCTL_PAYLOAD) >> 5);
 }
 
 /**
@@ -3675,32 +3519,18 @@ int pcie_get_mps(struct pci_dev *dev)
  */
 int pcie_set_mps(struct pci_dev *dev, int mps)
 {
-	int cap, err = -EINVAL;
-	u16 ctl, v;
+	u16 v;
 
 	if (mps < 128 || mps > 4096 || !is_power_of_2(mps))
-		goto out;
+		return -EINVAL;
 
 	v = ffs(mps) - 8;
 	if (v > dev->pcie_mpss) 
-		goto out;
+		return -EINVAL;
 	v <<= 5;
 
-	cap = pci_pcie_cap(dev);
-	if (!cap)
-		goto out;
-
-	err = pci_read_config_word(dev, cap + PCI_EXP_DEVCTL, &ctl);
-	if (err)
-		goto out;
-
-	if ((ctl & PCI_EXP_DEVCTL_PAYLOAD) != v) {
-		ctl &= ~PCI_EXP_DEVCTL_PAYLOAD;
-		ctl |= v;
-		err = pci_write_config_word(dev, cap + PCI_EXP_DEVCTL, ctl);
-	}
-out:
-	return err;
+	return pcie_capability_clear_and_set_word(dev, PCI_EXP_DEVCTL,
+						  PCI_EXP_DEVCTL_PAYLOAD, v);
 }
 
 /**
diff --git a/drivers/pci/pcie/aer/aer_inject.c b/drivers/pci/pcie/aer/aer_inject.c
index 52229863e9fe..4e24cb8a94ae 100644
--- a/drivers/pci/pcie/aer/aer_inject.c
+++ b/drivers/pci/pcie/aer/aer_inject.c
@@ -288,7 +288,7 @@ static struct pci_dev *pcie_find_root_port(struct pci_dev *dev)
 	while (1) {
 		if (!pci_is_pcie(dev))
 			break;
-		if (dev->pcie_type == PCI_EXP_TYPE_ROOT_PORT)
+		if (pci_pcie_type(dev) == PCI_EXP_TYPE_ROOT_PORT)
 			return dev;
 		if (!dev->bus->self)
 			break;
diff --git a/drivers/pci/pcie/aer/aerdrv.c b/drivers/pci/pcie/aer/aerdrv.c
index 58ad7917553c..030cf12d5468 100644
--- a/drivers/pci/pcie/aer/aerdrv.c
+++ b/drivers/pci/pcie/aer/aerdrv.c
@@ -48,7 +48,7 @@ static pci_ers_result_t aer_error_detected(struct pci_dev *dev,
 static void aer_error_resume(struct pci_dev *dev);
 static pci_ers_result_t aer_root_reset(struct pci_dev *dev);
 
-static struct pci_error_handlers aer_error_handlers = {
+static const struct pci_error_handlers aer_error_handlers = {
 	.error_detected = aer_error_detected,
 	.resume		= aer_error_resume,
 };
@@ -81,10 +81,11 @@ bool pci_aer_available(void)
 static int set_device_error_reporting(struct pci_dev *dev, void *data)
 {
 	bool enable = *((bool *)data);
+	int type = pci_pcie_type(dev);
 
-	if ((dev->pcie_type == PCI_EXP_TYPE_ROOT_PORT) ||
-	    (dev->pcie_type == PCI_EXP_TYPE_UPSTREAM) ||
-	    (dev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM)) {
+	if ((type == PCI_EXP_TYPE_ROOT_PORT) ||
+	    (type == PCI_EXP_TYPE_UPSTREAM) ||
+	    (type == PCI_EXP_TYPE_DOWNSTREAM)) {
 		if (enable)
 			pci_enable_pcie_error_reporting(dev);
 		else
@@ -121,19 +122,17 @@ static void set_downstream_devices_error_reporting(struct pci_dev *dev,
 static void aer_enable_rootport(struct aer_rpc *rpc)
 {
 	struct pci_dev *pdev = rpc->rpd->port;
-	int pos, aer_pos;
+	int aer_pos;
 	u16 reg16;
 	u32 reg32;
 
-	pos = pci_pcie_cap(pdev);
 	/* Clear PCIe Capability's Device Status */
-	pci_read_config_word(pdev, pos+PCI_EXP_DEVSTA, &reg16);
-	pci_write_config_word(pdev, pos+PCI_EXP_DEVSTA, reg16);
+	pcie_capability_read_word(pdev, PCI_EXP_DEVSTA, &reg16);
+	pcie_capability_write_word(pdev, PCI_EXP_DEVSTA, reg16);
 
 	/* Disable system error generation in response to error messages */
-	pci_read_config_word(pdev, pos + PCI_EXP_RTCTL, &reg16);
-	reg16 &= ~(SYSTEM_ERROR_INTR_ON_MESG_MASK);
-	pci_write_config_word(pdev, pos + PCI_EXP_RTCTL, reg16);
+	pcie_capability_clear_word(pdev, PCI_EXP_RTCTL,
+				   SYSTEM_ERROR_INTR_ON_MESG_MASK);
 
 	aer_pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ERR);
 	/* Clear error status */
@@ -395,9 +394,8 @@ static void aer_error_resume(struct pci_dev *dev)
 	u16 reg16;
 
 	/* Clean up Root device status */
-	pos = pci_pcie_cap(dev);
-	pci_read_config_word(dev, pos + PCI_EXP_DEVSTA, &reg16);
-	pci_write_config_word(dev, pos + PCI_EXP_DEVSTA, reg16);
+	pcie_capability_read_word(dev, PCI_EXP_DEVSTA, &reg16);
+	pcie_capability_write_word(dev, PCI_EXP_DEVSTA, reg16);
 
 	/* Clean AER Root Error Status */
 	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
diff --git a/drivers/pci/pcie/aer/aerdrv_acpi.c b/drivers/pci/pcie/aer/aerdrv_acpi.c
index 124f20ff11b2..5194a7d41730 100644
--- a/drivers/pci/pcie/aer/aerdrv_acpi.c
+++ b/drivers/pci/pcie/aer/aerdrv_acpi.c
@@ -60,7 +60,7 @@ static int aer_hest_parse(struct acpi_hest_header *hest_hdr, void *data)
 	p = (struct acpi_hest_aer_common *)(hest_hdr + 1);
 	if (p->flags & ACPI_HEST_GLOBAL) {
 		if ((pci_is_pcie(info->pci_dev) &&
-		     info->pci_dev->pcie_type == pcie_type) || bridge)
+		     pci_pcie_type(info->pci_dev) == pcie_type) || bridge)
 			ff = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
 	} else
 		if (hest_match_pci(p, info->pci_dev))
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index 0ca053538146..06bad96af415 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -32,53 +32,28 @@ static bool nosourceid;
 module_param(forceload, bool, 0);
 module_param(nosourceid, bool, 0);
 
+#define	PCI_EXP_AER_FLAGS	(PCI_EXP_DEVCTL_CERE | PCI_EXP_DEVCTL_NFERE | \
+				 PCI_EXP_DEVCTL_FERE | PCI_EXP_DEVCTL_URRE)
+
 int pci_enable_pcie_error_reporting(struct pci_dev *dev)
 {
-	u16 reg16 = 0;
-	int pos;
-
 	if (pcie_aer_get_firmware_first(dev))
 		return -EIO;
 
-	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
-	if (!pos)
+	if (!pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR))
 		return -EIO;
 
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return -EIO;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &reg16);
-	reg16 |= (PCI_EXP_DEVCTL_CERE |
-		PCI_EXP_DEVCTL_NFERE |
-		PCI_EXP_DEVCTL_FERE |
-		PCI_EXP_DEVCTL_URRE);
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, reg16);
-
-	return 0;
+	return pcie_capability_set_word(dev, PCI_EXP_DEVCTL, PCI_EXP_AER_FLAGS);
 }
 EXPORT_SYMBOL_GPL(pci_enable_pcie_error_reporting);
 
 int pci_disable_pcie_error_reporting(struct pci_dev *dev)
 {
-	u16 reg16 = 0;
-	int pos;
-
 	if (pcie_aer_get_firmware_first(dev))
 		return -EIO;
 
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return -EIO;
-
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &reg16);
-	reg16 &= ~(PCI_EXP_DEVCTL_CERE |
-		PCI_EXP_DEVCTL_NFERE |
-		PCI_EXP_DEVCTL_FERE |
-		PCI_EXP_DEVCTL_URRE);
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL, reg16);
-
-	return 0;
+	return pcie_capability_clear_word(dev, PCI_EXP_DEVCTL,
+					  PCI_EXP_AER_FLAGS);
 }
 EXPORT_SYMBOL_GPL(pci_disable_pcie_error_reporting);
 
@@ -151,18 +126,12 @@ static bool is_error_source(struct pci_dev *dev, struct aer_err_info *e_info)
 	 */
 	if (atomic_read(&dev->enable_cnt) == 0)
 		return false;
-	pos = pci_pcie_cap(dev);
-	if (!pos)
-		return false;
 
 	/* Check if AER is enabled */
-	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL, &reg16);
-	if (!(reg16 & (
-		PCI_EXP_DEVCTL_CERE |
-		PCI_EXP_DEVCTL_NFERE |
-		PCI_EXP_DEVCTL_FERE |
-		PCI_EXP_DEVCTL_URRE)))
+	pcie_capability_read_word(dev, PCI_EXP_DEVCTL, &reg16);
+	if (!(reg16 & PCI_EXP_AER_FLAGS))
 		return false;
+
 	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
 	if (!pos)
 		return false;
@@ -240,7 +209,7 @@ static bool find_source_device(struct pci_dev *parent,
 static int report_error_detected(struct pci_dev *dev, void *data)
 {
 	pci_ers_result_t vote;
-	struct pci_error_handlers *err_handler;
+	const struct pci_error_handlers *err_handler;
 	struct aer_broadcast_data *result_data;
 	result_data = (struct aer_broadcast_data *) data;
 
@@ -274,7 +243,7 @@ static int report_error_detected(struct pci_dev *dev, void *data)
 static int report_mmio_enabled(struct pci_dev *dev, void *data)
 {
 	pci_ers_result_t vote;
-	struct pci_error_handlers *err_handler;
+	const struct pci_error_handlers *err_handler;
 	struct aer_broadcast_data *result_data;
 	result_data = (struct aer_broadcast_data *) data;
 
@@ -292,7 +261,7 @@ static int report_mmio_enabled(struct pci_dev *dev, void *data)
 static int report_slot_reset(struct pci_dev *dev, void *data)
 {
 	pci_ers_result_t vote;
-	struct pci_error_handlers *err_handler;
+	const struct pci_error_handlers *err_handler;
 	struct aer_broadcast_data *result_data;
 	result_data = (struct aer_broadcast_data *) data;
 
@@ -309,7 +278,7 @@ static int report_slot_reset(struct pci_dev *dev, void *data)
 
 static int report_resume(struct pci_dev *dev, void *data)
 {
-	struct pci_error_handlers *err_handler;
+	const struct pci_error_handlers *err_handler;
 
 	dev->error_state = pci_channel_io_normal;
 
@@ -465,7 +434,7 @@ static pci_ers_result_t reset_link(struct pci_dev *dev)
 
 	if (driver && driver->reset_link) {
 		status = driver->reset_link(udev);
-	} else if (udev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM) {
+	} else if (pci_pcie_type(udev) == PCI_EXP_TYPE_DOWNSTREAM) {
 		status = default_downstream_reset_link(udev);
 	} else {
 		dev_printk(KERN_DEBUG, &dev->dev,
@@ -540,14 +509,12 @@ static void do_recovery(struct pci_dev *dev, int severity)
 				"resume",
 				report_resume);
 
-	dev_printk(KERN_DEBUG, &dev->dev,
-		"AER driver successfully recovered\n");
+	dev_info(&dev->dev, "AER: Device recovery successful\n");
 	return;
 
 failed:
 	/* TODO: Should kernel panic here? */
-	dev_printk(KERN_DEBUG, &dev->dev,
-		"AER driver didn't recover\n");
+	dev_info(&dev->dev, "AER: Device recovery failed\n");
 }
 
 /**
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index b500840a143b..213753b283a6 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -125,21 +125,16 @@ static int policy_to_clkpm_state(struct pcie_link_state *link)
 
 static void pcie_set_clkpm_nocheck(struct pcie_link_state *link, int enable)
 {
-	int pos;
-	u16 reg16;
 	struct pci_dev *child;
 	struct pci_bus *linkbus = link->pdev->subordinate;
 
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		pos = pci_pcie_cap(child);
-		if (!pos)
-			return;
-		pci_read_config_word(child, pos + PCI_EXP_LNKCTL, &reg16);
 		if (enable)
-			reg16 |= PCI_EXP_LNKCTL_CLKREQ_EN;
+			pcie_capability_set_word(child, PCI_EXP_LNKCTL,
+						 PCI_EXP_LNKCTL_CLKREQ_EN);
 		else
-			reg16 &= ~PCI_EXP_LNKCTL_CLKREQ_EN;
-		pci_write_config_word(child, pos + PCI_EXP_LNKCTL, reg16);
+			pcie_capability_clear_word(child, PCI_EXP_LNKCTL,
+						   PCI_EXP_LNKCTL_CLKREQ_EN);
 	}
 	link->clkpm_enabled = !!enable;
 }
@@ -157,7 +152,7 @@ static void pcie_set_clkpm(struct pcie_link_state *link, int enable)
 
 static void pcie_clkpm_cap_init(struct pcie_link_state *link, int blacklist)
 {
-	int pos, capable = 1, enabled = 1;
+	int capable = 1, enabled = 1;
 	u32 reg32;
 	u16 reg16;
 	struct pci_dev *child;
@@ -165,16 +160,13 @@ static void pcie_clkpm_cap_init(struct pcie_link_state *link, int blacklist)
 
 	/* All functions should have the same cap and state, take the worst */
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		pos = pci_pcie_cap(child);
-		if (!pos)
-			return;
-		pci_read_config_dword(child, pos + PCI_EXP_LNKCAP, &reg32);
+		pcie_capability_read_dword(child, PCI_EXP_LNKCAP, &reg32);
 		if (!(reg32 & PCI_EXP_LNKCAP_CLKPM)) {
 			capable = 0;
 			enabled = 0;
 			break;
 		}
-		pci_read_config_word(child, pos + PCI_EXP_LNKCTL, &reg16);
+		pcie_capability_read_word(child, PCI_EXP_LNKCTL, &reg16);
 		if (!(reg16 & PCI_EXP_LNKCTL_CLKREQ_EN))
 			enabled = 0;
 	}
@@ -190,7 +182,7 @@ static void pcie_clkpm_cap_init(struct pcie_link_state *link, int blacklist)
  */
 static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 {
-	int ppos, cpos, same_clock = 1;
+	int same_clock = 1;
 	u16 reg16, parent_reg, child_reg[8];
 	unsigned long start_jiffies;
 	struct pci_dev *child, *parent = link->pdev;
@@ -203,46 +195,43 @@ static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 	BUG_ON(!pci_is_pcie(child));
 
 	/* Check downstream component if bit Slot Clock Configuration is 1 */
-	cpos = pci_pcie_cap(child);
-	pci_read_config_word(child, cpos + PCI_EXP_LNKSTA, &reg16);
+	pcie_capability_read_word(child, PCI_EXP_LNKSTA, &reg16);
 	if (!(reg16 & PCI_EXP_LNKSTA_SLC))
 		same_clock = 0;
 
 	/* Check upstream component if bit Slot Clock Configuration is 1 */
-	ppos = pci_pcie_cap(parent);
-	pci_read_config_word(parent, ppos + PCI_EXP_LNKSTA, &reg16);
+	pcie_capability_read_word(parent, PCI_EXP_LNKSTA, &reg16);
 	if (!(reg16 & PCI_EXP_LNKSTA_SLC))
 		same_clock = 0;
 
 	/* Configure downstream component, all functions */
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		cpos = pci_pcie_cap(child);
-		pci_read_config_word(child, cpos + PCI_EXP_LNKCTL, &reg16);
+		pcie_capability_read_word(child, PCI_EXP_LNKCTL, &reg16);
 		child_reg[PCI_FUNC(child->devfn)] = reg16;
 		if (same_clock)
 			reg16 |= PCI_EXP_LNKCTL_CCC;
 		else
 			reg16 &= ~PCI_EXP_LNKCTL_CCC;
-		pci_write_config_word(child, cpos + PCI_EXP_LNKCTL, reg16);
+		pcie_capability_write_word(child, PCI_EXP_LNKCTL, reg16);
 	}
 
 	/* Configure upstream component */
-	pci_read_config_word(parent, ppos + PCI_EXP_LNKCTL, &reg16);
+	pcie_capability_read_word(parent, PCI_EXP_LNKCTL, &reg16);
 	parent_reg = reg16;
 	if (same_clock)
 		reg16 |= PCI_EXP_LNKCTL_CCC;
 	else
 		reg16 &= ~PCI_EXP_LNKCTL_CCC;
-	pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, reg16);
+	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, reg16);
 
 	/* Retrain link */
 	reg16 |= PCI_EXP_LNKCTL_RL;
-	pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, reg16);
+	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, reg16);
 
 	/* Wait for link training end. Break out after waiting for timeout */
 	start_jiffies = jiffies;
 	for (;;) {
-		pci_read_config_word(parent, ppos + PCI_EXP_LNKSTA, &reg16);
+		pcie_capability_read_word(parent, PCI_EXP_LNKSTA, &reg16);
 		if (!(reg16 & PCI_EXP_LNKSTA_LT))
 			break;
 		if (time_after(jiffies, start_jiffies + LINK_RETRAIN_TIMEOUT))
@@ -255,12 +244,10 @@ static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 	/* Training failed. Restore common clock configurations */
 	dev_printk(KERN_ERR, &parent->dev,
 		   "ASPM: Could not configure common clock\n");
-	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		cpos = pci_pcie_cap(child);
-		pci_write_config_word(child, cpos + PCI_EXP_LNKCTL,
-				      child_reg[PCI_FUNC(child->devfn)]);
-	}
-	pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, parent_reg);
+	list_for_each_entry(child, &linkbus->devices, bus_list)
+		pcie_capability_write_word(child, PCI_EXP_LNKCTL,
+					   child_reg[PCI_FUNC(child->devfn)]);
+	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, parent_reg);
 }
 
 /* Convert L0s latency encoding to ns */
@@ -305,16 +292,14 @@ struct aspm_register_info {
 static void pcie_get_aspm_reg(struct pci_dev *pdev,
 			      struct aspm_register_info *info)
 {
-	int pos;
 	u16 reg16;
 	u32 reg32;
 
-	pos = pci_pcie_cap(pdev);
-	pci_read_config_dword(pdev, pos + PCI_EXP_LNKCAP, &reg32);
+	pcie_capability_read_dword(pdev, PCI_EXP_LNKCAP, &reg32);
 	info->support = (reg32 & PCI_EXP_LNKCAP_ASPMS) >> 10;
 	info->latency_encoding_l0s = (reg32 & PCI_EXP_LNKCAP_L0SEL) >> 12;
 	info->latency_encoding_l1  = (reg32 & PCI_EXP_LNKCAP_L1EL) >> 15;
-	pci_read_config_word(pdev, pos + PCI_EXP_LNKCTL, &reg16);
+	pcie_capability_read_word(pdev, PCI_EXP_LNKCTL, &reg16);
 	info->enabled = reg16 & PCI_EXP_LNKCTL_ASPMC;
 }
 
@@ -412,7 +397,7 @@ static void pcie_aspm_cap_init(struct pcie_link_state *link, int blacklist)
 	 * do ASPM for now.
 	 */
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		if (child->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
+		if (pci_pcie_type(child) == PCI_EXP_TYPE_PCI_BRIDGE) {
 			link->aspm_disable = ASPM_STATE_ALL;
 			break;
 		}
@@ -420,17 +405,15 @@ static void pcie_aspm_cap_init(struct pcie_link_state *link, int blacklist)
 
 	/* Get and check endpoint acceptable latencies */
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		int pos;
 		u32 reg32, encoding;
 		struct aspm_latency *acceptable =
 			&link->acceptable[PCI_FUNC(child->devfn)];
 
-		if (child->pcie_type != PCI_EXP_TYPE_ENDPOINT &&
-		    child->pcie_type != PCI_EXP_TYPE_LEG_END)
+		if (pci_pcie_type(child) != PCI_EXP_TYPE_ENDPOINT &&
+		    pci_pcie_type(child) != PCI_EXP_TYPE_LEG_END)
 			continue;
 
-		pos = pci_pcie_cap(child);
-		pci_read_config_dword(child, pos + PCI_EXP_DEVCAP, &reg32);
+		pcie_capability_read_dword(child, PCI_EXP_DEVCAP, &reg32);
 		/* Calculate endpoint L0s acceptable latency */
 		encoding = (reg32 & PCI_EXP_DEVCAP_L0S) >> 6;
 		acceptable->l0s = calc_l0s_acceptable(encoding);
@@ -444,13 +427,7 @@ static void pcie_aspm_cap_init(struct pcie_link_state *link, int blacklist)
 
 static void pcie_config_aspm_dev(struct pci_dev *pdev, u32 val)
 {
-	u16 reg16;
-	int pos = pci_pcie_cap(pdev);
-
-	pci_read_config_word(pdev, pos + PCI_EXP_LNKCTL, &reg16);
-	reg16 &= ~0x3;
-	reg16 |= val;
-	pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
+	pcie_capability_clear_and_set_word(pdev, PCI_EXP_LNKCTL, 0x3, val);
 }
 
 static void pcie_config_aspm_link(struct pcie_link_state *link, u32 state)
@@ -505,7 +482,6 @@ static void free_link_state(struct pcie_link_state *link)
 static int pcie_aspm_sanity_check(struct pci_dev *pdev)
 {
 	struct pci_dev *child;
-	int pos;
 	u32 reg32;
 
 	/*
@@ -513,8 +489,7 @@ static int pcie_aspm_sanity_check(struct pci_dev *pdev)
 	 * very strange. Disable ASPM for the whole slot
 	 */
 	list_for_each_entry(child, &pdev->subordinate->devices, bus_list) {
-		pos = pci_pcie_cap(child);
-		if (!pos)
+		if (!pci_is_pcie(child))
 			return -EINVAL;
 
 		/*
@@ -530,7 +505,7 @@ static int pcie_aspm_sanity_check(struct pci_dev *pdev)
 		 * Disable ASPM for pre-1.1 PCIe device, we follow MS to use
 		 * RBER bit to determine if a function is 1.1 version device
 		 */
-		pci_read_config_dword(child, pos + PCI_EXP_DEVCAP, &reg32);
+		pcie_capability_read_dword(child, PCI_EXP_DEVCAP, &reg32);
 		if (!(reg32 & PCI_EXP_DEVCAP_RBER) && !aspm_force) {
 			dev_printk(KERN_INFO, &child->dev, "disabling ASPM"
 				" on pre-1.1 PCIe device.  You can enable it"
@@ -552,7 +527,7 @@ static struct pcie_link_state *alloc_pcie_link_state(struct pci_dev *pdev)
 	INIT_LIST_HEAD(&link->children);
 	INIT_LIST_HEAD(&link->link);
 	link->pdev = pdev;
-	if (pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM) {
+	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_DOWNSTREAM) {
 		struct pcie_link_state *parent;
 		parent = pdev->bus->parent->self->link_state;
 		if (!parent) {
@@ -585,12 +560,12 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 
 	if (!pci_is_pcie(pdev) || pdev->link_state)
 		return;
-	if (pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT &&
-	    pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM)
+	if (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
+	    pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM)
 		return;
 
 	/* VIA has a strange chipset, root port is under a bridge */
-	if (pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT &&
+	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_ROOT_PORT &&
 	    pdev->bus->self)
 		return;
 
@@ -647,8 +622,8 @@ static void pcie_update_aspm_capable(struct pcie_link_state *root)
 		if (link->root != root)
 			continue;
 		list_for_each_entry(child, &linkbus->devices, bus_list) {
-			if ((child->pcie_type != PCI_EXP_TYPE_ENDPOINT) &&
-			    (child->pcie_type != PCI_EXP_TYPE_LEG_END))
+			if ((pci_pcie_type(child) != PCI_EXP_TYPE_ENDPOINT) &&
+			    (pci_pcie_type(child) != PCI_EXP_TYPE_LEG_END))
 				continue;
 			pcie_aspm_check_latency(child);
 		}
@@ -663,8 +638,8 @@ void pcie_aspm_exit_link_state(struct pci_dev *pdev)
 
 	if (!pci_is_pcie(pdev) || !parent || !parent->link_state)
 		return;
-	if ((parent->pcie_type != PCI_EXP_TYPE_ROOT_PORT) &&
-	    (parent->pcie_type != PCI_EXP_TYPE_DOWNSTREAM))
+	if ((pci_pcie_type(parent) != PCI_EXP_TYPE_ROOT_PORT) &&
+	    (pci_pcie_type(parent) != PCI_EXP_TYPE_DOWNSTREAM))
 		return;
 
 	down_read(&pci_bus_sem);
@@ -704,8 +679,8 @@ void pcie_aspm_pm_state_change(struct pci_dev *pdev)
 
 	if (aspm_disabled || !pci_is_pcie(pdev) || !link)
 		return;
-	if ((pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT) &&
-	    (pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM))
+	if ((pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT) &&
+	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM))
 		return;
 	/*
 	 * Devices changed PM state, we should recheck if latency
@@ -729,8 +704,8 @@ void pcie_aspm_powersave_config_link(struct pci_dev *pdev)
 	if (aspm_policy != POLICY_POWERSAVE)
 		return;
 
-	if ((pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT) &&
-	    (pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM))
+	if ((pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT) &&
+	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM))
 		return;
 
 	down_read(&pci_bus_sem);
@@ -757,8 +732,8 @@ static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
 	if (!pci_is_pcie(pdev))
 		return;
 
-	if (pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT ||
-	    pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM)
+	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_ROOT_PORT ||
+	    pci_pcie_type(pdev) == PCI_EXP_TYPE_DOWNSTREAM)
 		parent = pdev;
 	if (!parent || !parent->link_state)
 		return;
@@ -933,8 +908,8 @@ void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev)
 	struct pcie_link_state *link_state = pdev->link_state;
 
 	if (!pci_is_pcie(pdev) ||
-	    (pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT &&
-	     pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
+	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
+	     pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
 		return;
 
 	if (link_state->aspm_support)
@@ -950,8 +925,8 @@ void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev)
 	struct pcie_link_state *link_state = pdev->link_state;
 
 	if (!pci_is_pcie(pdev) ||
-	    (pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT &&
-	     pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
+	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
+	     pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
 		return;
 
 	if (link_state->aspm_support)
diff --git a/drivers/pci/pcie/pme.c b/drivers/pci/pcie/pme.c
index 001f1b78f39c..9ca0dc9ffd84 100644
--- a/drivers/pci/pcie/pme.c
+++ b/drivers/pci/pcie/pme.c
@@ -57,17 +57,12 @@ struct pcie_pme_service_data {
  */
 void pcie_pme_interrupt_enable(struct pci_dev *dev, bool enable)
 {
-	int rtctl_pos;
-	u16 rtctl;
-
-	rtctl_pos = pci_pcie_cap(dev) + PCI_EXP_RTCTL;
-
-	pci_read_config_word(dev, rtctl_pos, &rtctl);
 	if (enable)
-		rtctl |= PCI_EXP_RTCTL_PMEIE;
+		pcie_capability_set_word(dev, PCI_EXP_RTCTL,
+					 PCI_EXP_RTCTL_PMEIE);
 	else
-		rtctl &= ~PCI_EXP_RTCTL_PMEIE;
-	pci_write_config_word(dev, rtctl_pos, rtctl);
+		pcie_capability_clear_word(dev, PCI_EXP_RTCTL,
+					   PCI_EXP_RTCTL_PMEIE);
 }
 
 /**
@@ -120,7 +115,7 @@ static bool pcie_pme_from_pci_bridge(struct pci_bus *bus, u8 devfn)
 	if (!dev)
 		return false;
 
-	if (pci_is_pcie(dev) && dev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
+	if (pci_is_pcie(dev) && pci_pcie_type(dev) == PCI_EXP_TYPE_PCI_BRIDGE) {
 		down_read(&pci_bus_sem);
 		if (pcie_pme_walk_bus(bus))
 			found = true;
@@ -226,18 +221,15 @@ static void pcie_pme_work_fn(struct work_struct *work)
 	struct pcie_pme_service_data *data =
 			container_of(work, struct pcie_pme_service_data, work);
 	struct pci_dev *port = data->srv->port;
-	int rtsta_pos;
 	u32 rtsta;
 
-	rtsta_pos = pci_pcie_cap(port) + PCI_EXP_RTSTA;
-
 	spin_lock_irq(&data->lock);
 
 	for (;;) {
 		if (data->noirq)
 			break;
 
-		pci_read_config_dword(port, rtsta_pos, &rtsta);
+		pcie_capability_read_dword(port, PCI_EXP_RTSTA, &rtsta);
 		if (rtsta & PCI_EXP_RTSTA_PME) {
 			/*
 			 * Clear PME status of the port.  If there are other
@@ -276,17 +268,14 @@ static irqreturn_t pcie_pme_irq(int irq, void *context)
 {
 	struct pci_dev *port;
 	struct pcie_pme_service_data *data;
-	int rtsta_pos;
 	u32 rtsta;
 	unsigned long flags;
 
 	port = ((struct pcie_device *)context)->port;
 	data = get_service_data((struct pcie_device *)context);
 
-	rtsta_pos = pci_pcie_cap(port) + PCI_EXP_RTSTA;
-
 	spin_lock_irqsave(&data->lock, flags);
-	pci_read_config_dword(port, rtsta_pos, &rtsta);
+	pcie_capability_read_dword(port, PCI_EXP_RTSTA, &rtsta);
 
 	if (!(rtsta & PCI_EXP_RTSTA_PME)) {
 		spin_unlock_irqrestore(&data->lock, flags);
@@ -335,13 +324,13 @@ static void pcie_pme_mark_devices(struct pci_dev *port)
 		struct pci_dev *dev;
 
 		/* Check if this is a root port event collector. */
-		if (port->pcie_type != PCI_EXP_TYPE_RC_EC || !bus)
+		if (pci_pcie_type(port) != PCI_EXP_TYPE_RC_EC || !bus)
 			return;
 
 		down_read(&pci_bus_sem);
 		list_for_each_entry(dev, &bus->devices, bus_list)
 			if (pci_is_pcie(dev)
-			    && dev->pcie_type == PCI_EXP_TYPE_RC_END)
+			    && pci_pcie_type(dev) == PCI_EXP_TYPE_RC_END)
 				pcie_pme_set_native(dev, NULL);
 		up_read(&pci_bus_sem);
 	}
diff --git a/drivers/pci/pcie/portdrv_bus.c b/drivers/pci/pcie/portdrv_bus.c
index 18bf90f748f6..67be55a7f260 100644
--- a/drivers/pci/pcie/portdrv_bus.c
+++ b/drivers/pci/pcie/portdrv_bus.c
@@ -38,7 +38,7 @@ static int pcie_port_bus_match(struct device *dev, struct device_driver *drv)
 		return 0;
 
 	if ((driver->port_type != PCIE_ANY_PORT) &&
-	    (driver->port_type != pciedev->port->pcie_type))
+	    (driver->port_type != pci_pcie_type(pciedev->port)))
 		return 0;
 
 	return 1;
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 75915b30ad19..d03a7a39b2d8 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -200,10 +200,13 @@ static int init_service_irqs(struct pci_dev *dev, int *irqs, int mask)
 {
 	int i, irq = -1;
 
-	/* We have to use INTx if MSI cannot be used for PCIe PME or pciehp. */
+	/*
+	 * If MSI cannot be used for PCIe PME or hotplug, we have to use
+	 * INTx or other interrupts, e.g. system shared interrupt.
+	 */
 	if (((mask & PCIE_PORT_SERVICE_PME) && pcie_pme_no_msi()) ||
 	    ((mask & PCIE_PORT_SERVICE_HP) && pciehp_no_msi())) {
-		if (dev->pin)
+		if (dev->irq)
 			irq = dev->irq;
 		goto no_msi;
 	}
@@ -212,8 +215,12 @@ static int init_service_irqs(struct pci_dev *dev, int *irqs, int mask)
 	if (!pcie_port_enable_msix(dev, irqs, mask))
 		return 0;
 
-	/* We're not going to use MSI-X, so try MSI and fall back to INTx */
-	if (!pci_enable_msi(dev) || dev->pin)
+	/*
+	 * We're not going to use MSI-X, so try MSI and fall back to INTx.
+	 * If neither MSI/MSI-X nor INTx available, try other interrupt.  On
+	 * some platforms, root port doesn't support MSI/MSI-X/INTx in RC mode.
+	 */
+	if (!pci_enable_msi(dev) || dev->irq)
 		irq = dev->irq;
 
  no_msi:
@@ -246,8 +253,7 @@ static void cleanup_service_irqs(struct pci_dev *dev)
  */
 static int get_port_device_capability(struct pci_dev *dev)
 {
-	int services = 0, pos;
-	u16 reg16;
+	int services = 0;
 	u32 reg32;
 	int cap_mask = 0;
 	int err;
@@ -265,11 +271,9 @@ static int get_port_device_capability(struct pci_dev *dev)
 			return 0;
 	}
 
-	pos = pci_pcie_cap(dev);
-	pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &reg16);
 	/* Hot-Plug Capable */
-	if ((cap_mask & PCIE_PORT_SERVICE_HP) && (reg16 & PCI_EXP_FLAGS_SLOT)) {
-		pci_read_config_dword(dev, pos + PCI_EXP_SLTCAP, &reg32);
+	if (cap_mask & PCIE_PORT_SERVICE_HP) {
+		pcie_capability_read_dword(dev, PCI_EXP_SLTCAP, &reg32);
 		if (reg32 & PCI_EXP_SLTCAP_HPC) {
 			services |= PCIE_PORT_SERVICE_HP;
 			/*
@@ -277,10 +281,8 @@ static int get_port_device_capability(struct pci_dev *dev)
 			 * enabled by the BIOS and the hot-plug service driver
 			 * is not loaded.
 			 */
-			pos += PCI_EXP_SLTCTL;
-			pci_read_config_word(dev, pos, &reg16);
-			reg16 &= ~(PCI_EXP_SLTCTL_CCIE | PCI_EXP_SLTCTL_HPIE);
-			pci_write_config_word(dev, pos, reg16);
+			pcie_capability_clear_word(dev, PCI_EXP_SLTCTL,
+				PCI_EXP_SLTCTL_CCIE | PCI_EXP_SLTCTL_HPIE);
 		}
 	}
 	/* AER capable */
@@ -298,7 +300,7 @@ static int get_port_device_capability(struct pci_dev *dev)
 		services |= PCIE_PORT_SERVICE_VC;
 	/* Root ports are capable of generating PME too */
 	if ((cap_mask & PCIE_PORT_SERVICE_PME)
-	    && dev->pcie_type == PCI_EXP_TYPE_ROOT_PORT) {
+	    && pci_pcie_type(dev) == PCI_EXP_TYPE_ROOT_PORT) {
 		services |= PCIE_PORT_SERVICE_PME;
 		/*
 		 * Disable PME interrupt on this port in case it's been enabled
@@ -336,7 +338,7 @@ static int pcie_device_init(struct pci_dev *pdev, int service, int irq)
 	device->release = release_pcie_device;	/* callback to free pcie dev */
 	dev_set_name(device, "%s:pcie%02x",
 		     pci_name(pdev),
-		     get_descriptor_id(pdev->pcie_type, service));
+		     get_descriptor_id(pci_pcie_type(pdev), service));
 	device->parent = &pdev->dev;
 	device_enable_async_suspend(device);
 
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index e76b44777dbf..0761d90ca279 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -64,14 +64,7 @@ __setup("pcie_ports=", pcie_port_setup);
  */
 void pcie_clear_root_pme_status(struct pci_dev *dev)
 {
-	int rtsta_pos;
-	u32 rtsta;
-
-	rtsta_pos = pci_pcie_cap(dev) + PCI_EXP_RTSTA;
-
-	pci_read_config_dword(dev, rtsta_pos, &rtsta);
-	rtsta |= PCI_EXP_RTSTA_PME;
-	pci_write_config_dword(dev, rtsta_pos, rtsta);
+	pcie_capability_set_dword(dev, PCI_EXP_RTSTA, PCI_EXP_RTSTA_PME);
 }
 
 static int pcie_portdrv_restore_config(struct pci_dev *dev)
@@ -95,7 +88,7 @@ static int pcie_port_resume_noirq(struct device *dev)
 	 * which breaks ACPI-based runtime wakeup on PCI Express, so clear those
 	 * bits now just in case (shouldn't hurt).
 	 */
-	if(pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT)
+	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_ROOT_PORT)
 		pcie_clear_root_pme_status(pdev);
 	return 0;
 }
@@ -195,9 +188,9 @@ static int __devinit pcie_portdrv_probe(struct pci_dev *dev,
 	int status;
 
 	if (!pci_is_pcie(dev) ||
-	    ((dev->pcie_type != PCI_EXP_TYPE_ROOT_PORT) &&
-	     (dev->pcie_type != PCI_EXP_TYPE_UPSTREAM) &&
-	     (dev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM)))
+	    ((pci_pcie_type(dev) != PCI_EXP_TYPE_ROOT_PORT) &&
+	     (pci_pcie_type(dev) != PCI_EXP_TYPE_UPSTREAM) &&
+	     (pci_pcie_type(dev) != PCI_EXP_TYPE_DOWNSTREAM)))
 		return -ENODEV;
 
 	if (!dev->irq && dev->pin) {
@@ -385,11 +378,11 @@ static const struct pci_device_id port_pci_ids[] = { {
 };
 MODULE_DEVICE_TABLE(pci, port_pci_ids);
 
-static struct pci_error_handlers pcie_portdrv_err_handler = {
-		.error_detected = pcie_portdrv_error_detected,
-		.mmio_enabled = pcie_portdrv_mmio_enabled,
-		.slot_reset = pcie_portdrv_slot_reset,
-		.resume = pcie_portdrv_err_resume,
+static const struct pci_error_handlers pcie_portdrv_err_handler = {
+	.error_detected = pcie_portdrv_error_detected,
+	.mmio_enabled = pcie_portdrv_mmio_enabled,
+	.slot_reset = pcie_portdrv_slot_reset,
+	.resume = pcie_portdrv_err_resume,
 };
 
 static struct pci_driver pcie_portdriver = {
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 9f8a6b79a8ec..ec909afa90b6 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -606,10 +606,10 @@ static void pci_set_bus_speed(struct pci_bus *bus)
 		u32 linkcap;
 		u16 linksta;
 
-		pci_read_config_dword(bridge, pos + PCI_EXP_LNKCAP, &linkcap);
+		pcie_capability_read_dword(bridge, PCI_EXP_LNKCAP, &linkcap);
 		bus->max_bus_speed = pcie_link_speed[linkcap & 0xf];
 
-		pci_read_config_word(bridge, pos + PCI_EXP_LNKSTA, &linksta);
+		pcie_capability_read_word(bridge, PCI_EXP_LNKSTA, &linksta);
 		pcie_update_link_speed(bus, linksta);
 	}
 }
@@ -729,8 +729,10 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 
 	/* Check if setup is sensible at all */
 	if (!pass &&
-	    (primary != bus->number || secondary <= bus->number)) {
-		dev_dbg(&dev->dev, "bus configuration invalid, reconfiguring\n");
+	    (primary != bus->number || secondary <= bus->number ||
+	     secondary > subordinate)) {
+		dev_info(&dev->dev, "bridge configuration invalid ([bus %02x-%02x]), reconfiguring\n",
+			 secondary, subordinate);
 		broken = 1;
 	}
 
@@ -932,24 +934,16 @@ void set_pcie_port_type(struct pci_dev *pdev)
 	pdev->is_pcie = 1;
 	pdev->pcie_cap = pos;
 	pci_read_config_word(pdev, pos + PCI_EXP_FLAGS, &reg16);
-	pdev->pcie_type = (reg16 & PCI_EXP_FLAGS_TYPE) >> 4;
+	pdev->pcie_flags_reg = reg16;
 	pci_read_config_word(pdev, pos + PCI_EXP_DEVCAP, &reg16);
 	pdev->pcie_mpss = reg16 & PCI_EXP_DEVCAP_PAYLOAD;
 }
 
 void set_pcie_hotplug_bridge(struct pci_dev *pdev)
 {
-	int pos;
-	u16 reg16;
 	u32 reg32;
 
-	pos = pci_pcie_cap(pdev);
-	if (!pos)
-		return;
-	pci_read_config_word(pdev, pos + PCI_EXP_FLAGS, &reg16);
-	if (!(reg16 & PCI_EXP_FLAGS_SLOT))
-		return;
-	pci_read_config_dword(pdev, pos + PCI_EXP_SLTCAP, &reg32);
+	pcie_capability_read_dword(pdev, PCI_EXP_SLTCAP, &reg32);
 	if (reg32 & PCI_EXP_SLTCAP_HPC)
 		pdev->is_hotplug_bridge = 1;
 }
@@ -1163,8 +1157,7 @@ int pci_cfg_space_size(struct pci_dev *dev)
 	if (class == PCI_CLASS_BRIDGE_HOST)
 		return pci_cfg_space_size_ext(dev);
 
-	pos = pci_pcie_cap(dev);
-	if (!pos) {
+	if (!pci_is_pcie(dev)) {
 		pos = pci_find_capability(dev, PCI_CAP_ID_PCIX);
 		if (!pos)
 			goto fail;
@@ -1386,9 +1379,9 @@ static int only_one_child(struct pci_bus *bus)
 
 	if (!parent || !pci_is_pcie(parent))
 		return 0;
-	if (parent->pcie_type == PCI_EXP_TYPE_ROOT_PORT)
+	if (pci_pcie_type(parent) == PCI_EXP_TYPE_ROOT_PORT)
 		return 1;
-	if (parent->pcie_type == PCI_EXP_TYPE_DOWNSTREAM &&
+	if (pci_pcie_type(parent) == PCI_EXP_TYPE_DOWNSTREAM &&
 	    !pci_has_flag(PCI_SCAN_ALL_PCIE_DEVS))
 		return 1;
 	return 0;
@@ -1465,7 +1458,7 @@ static int pcie_find_smpss(struct pci_dev *dev, void *data)
 	 */
 	if (dev->is_hotplug_bridge && (!list_is_singular(&dev->bus->devices) ||
 	     (dev->bus->self &&
-	      dev->bus->self->pcie_type != PCI_EXP_TYPE_ROOT_PORT)))
+	      pci_pcie_type(dev->bus->self) != PCI_EXP_TYPE_ROOT_PORT)))
 		*smpss = 0;
 
 	if (*smpss > dev->pcie_mpss)
@@ -1481,7 +1474,8 @@ static void pcie_write_mps(struct pci_dev *dev, int mps)
 	if (pcie_bus_config == PCIE_BUS_PERFORMANCE) {
 		mps = 128 << dev->pcie_mpss;
 
-		if (dev->pcie_type != PCI_EXP_TYPE_ROOT_PORT && dev->bus->self)
+		if (pci_pcie_type(dev) != PCI_EXP_TYPE_ROOT_PORT &&
+		    dev->bus->self)
 			/* For "Performance", the assumption is made that
 			 * downstream communication will never be larger than
 			 * the MRRS.  So, the MPS only needs to be configured
@@ -1756,11 +1750,6 @@ int pci_bus_insert_busn_res(struct pci_bus *b, int bus, int bus_max)
 			   "busn_res: can not insert %pR under %s%pR (conflicts with %s %pR)\n",
 			    res, pci_is_root_bus(b) ? "domain " : "",
 			    parent_res, conflict->name, conflict);
-	else
-		dev_printk(KERN_DEBUG, &b->dev,
-			   "busn_res: %pR is inserted under %s%pR\n",
-			   res, pci_is_root_bus(b) ? "domain " : "",
-			   parent_res);
 
 	return conflict == NULL;
 }
diff --git a/drivers/pci/proc.c b/drivers/pci/proc.c
index 27911b55c2a5..eb907a8faf2a 100644
--- a/drivers/pci/proc.c
+++ b/drivers/pci/proc.c
@@ -434,25 +434,6 @@ int pci_proc_detach_device(struct pci_dev *dev)
 	return 0;
 }
 
-#if 0
-int pci_proc_attach_bus(struct pci_bus* bus)
-{
-	struct proc_dir_entry *de = bus->procdir;
-
-	if (!proc_initialized)
-		return -EACCES;
-
-	if (!de) {
-		char name[16];
-		sprintf(name, "%02x", bus->number);
-		de = bus->procdir = proc_mkdir(name, proc_bus_pci_dir);
-		if (!de)
-			return -ENOMEM;
-	}
-	return 0;
-}
-#endif  /*  0  */
-
 int pci_proc_detach_bus(struct pci_bus* bus)
 {
 	struct proc_dir_entry *de = bus->procdir;
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 51553179e967..7a451ff56ecc 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -3081,17 +3081,36 @@ static int reset_intel_generic_dev(struct pci_dev *dev, int probe)
 
 static int reset_intel_82599_sfp_virtfn(struct pci_dev *dev, int probe)
 {
-	int pos;
+	int i;
+	u16 status;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
-	if (!pos)
-		return -ENOTTY;
+	/*
+	 * http://www.intel.com/content/dam/doc/datasheet/82599-10-gbe-controller-datasheet.pdf
+	 *
+	 * The 82599 supports FLR on VFs, but FLR support is reported only
+	 * in the PF DEVCAP (sec 9.3.10.4), not in the VF DEVCAP (sec 9.5).
+	 * Therefore, we can't use pcie_flr(), which checks the VF DEVCAP.
+	 */
 
 	if (probe)
 		return 0;
 
-	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL,
-				PCI_EXP_DEVCTL_BCR_FLR);
+	/* Wait for Transaction Pending bit clean */
+	for (i = 0; i < 4; i++) {
+		if (i)
+			msleep((1 << (i - 1)) * 100);
+
+		pcie_capability_read_word(dev, PCI_EXP_DEVSTA, &status);
+		if (!(status & PCI_EXP_DEVSTA_TRPND))
+			goto clear;
+	}
+
+	dev_err(&dev->dev, "transaction is not cleared; "
+			"proceeding with reset anyway\n");
+
+clear:
+	pcie_capability_set_word(dev, PCI_EXP_DEVCTL, PCI_EXP_DEVCTL_BCR_FLR);
+
 	msleep(100);
 
 	return 0;
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index 04a4861b4749..513972f3ed13 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -32,152 +32,82 @@ static void pci_stop_dev(struct pci_dev *dev)
 
 static void pci_destroy_dev(struct pci_dev *dev)
 {
-	/* Remove the device from the device lists, and prevent any further
-	 * list accesses from this device */
 	down_write(&pci_bus_sem);
 	list_del(&dev->bus_list);
-	dev->bus_list.next = dev->bus_list.prev = NULL;
 	up_write(&pci_bus_sem);
 
 	pci_free_resources(dev);
 	pci_dev_put(dev);
 }
 
-/**
- * pci_remove_device_safe - remove an unused hotplug device
- * @dev: the device to remove
- *
- * Delete the device structure from the device lists and 
- * notify userspace (/sbin/hotplug), but only if the device
- * in question is not being used by a driver.
- * Returns 0 on success.
- */
-#if 0
-int pci_remove_device_safe(struct pci_dev *dev)
-{
-	if (pci_dev_driver(dev))
-		return -EBUSY;
-	pci_destroy_dev(dev);
-	return 0;
-}
-#endif  /*  0  */
-
-void pci_remove_bus(struct pci_bus *pci_bus)
+void pci_remove_bus(struct pci_bus *bus)
 {
-	pci_proc_detach_bus(pci_bus);
+	pci_proc_detach_bus(bus);
 
 	down_write(&pci_bus_sem);
-	list_del(&pci_bus->node);
-	pci_bus_release_busn_res(pci_bus);
+	list_del(&bus->node);
+	pci_bus_release_busn_res(bus);
 	up_write(&pci_bus_sem);
-	if (!pci_bus->is_added)
+	if (!bus->is_added)
 		return;
 
-	pci_remove_legacy_files(pci_bus);
-	device_unregister(&pci_bus->dev);
+	pci_remove_legacy_files(bus);
+	device_unregister(&bus->dev);
 }
 EXPORT_SYMBOL(pci_remove_bus);
 
-static void __pci_remove_behind_bridge(struct pci_dev *dev);
-/**
- * pci_stop_and_remove_bus_device - remove a PCI device and any children
- * @dev: the device to remove
- *
- * Remove a PCI device from the device lists, informing the drivers
- * that the device has been removed.  We also remove any subordinate
- * buses and children in a depth-first manner.
- *
- * For each device we remove, delete the device structure from the
- * device lists, remove the /proc entry, and notify userspace
- * (/sbin/hotplug).
- */
-void __pci_remove_bus_device(struct pci_dev *dev)
+static void pci_stop_bus_device(struct pci_dev *dev)
 {
-	if (dev->subordinate) {
-		struct pci_bus *b = dev->subordinate;
+	struct pci_bus *bus = dev->subordinate;
+	struct pci_dev *child, *tmp;
 
-		__pci_remove_behind_bridge(dev);
-		pci_remove_bus(b);
-		dev->subordinate = NULL;
+	/*
+	 * Stopping an SR-IOV PF device removes all the associated VFs,
+	 * which will update the bus->devices list and confuse the
+	 * iterator.  Therefore, iterate in reverse so we remove the VFs
+	 * first, then the PF.
+	 */
+	if (bus) {
+		list_for_each_entry_safe_reverse(child, tmp,
+						 &bus->devices, bus_list)
+			pci_stop_bus_device(child);
 	}
 
-	pci_destroy_dev(dev);
-}
-EXPORT_SYMBOL(__pci_remove_bus_device);
-
-void pci_stop_and_remove_bus_device(struct pci_dev *dev)
-{
-	pci_stop_bus_device(dev);
-	__pci_remove_bus_device(dev);
+	pci_stop_dev(dev);
 }
 
-static void __pci_remove_behind_bridge(struct pci_dev *dev)
+static void pci_remove_bus_device(struct pci_dev *dev)
 {
-	struct list_head *l, *n;
+	struct pci_bus *bus = dev->subordinate;
+	struct pci_dev *child, *tmp;
 
-	if (dev->subordinate)
-		list_for_each_safe(l, n, &dev->subordinate->devices)
-			__pci_remove_bus_device(pci_dev_b(l));
-}
+	if (bus) {
+		list_for_each_entry_safe(child, tmp,
+					 &bus->devices, bus_list)
+			pci_remove_bus_device(child);
 
-static void pci_stop_behind_bridge(struct pci_dev *dev)
-{
-	struct list_head *l, *n;
+		pci_remove_bus(bus);
+		dev->subordinate = NULL;
+	}
 
-	if (dev->subordinate)
-		list_for_each_safe(l, n, &dev->subordinate->devices)
-			pci_stop_bus_device(pci_dev_b(l));
+	pci_destroy_dev(dev);
 }
 
 /**
- * pci_stop_and_remove_behind_bridge - stop and remove all devices behind
- *					 a PCI bridge
- * @dev: PCI bridge device
+ * pci_stop_and_remove_bus_device - remove a PCI device and any children
+ * @dev: the device to remove
  *
- * Remove all devices on the bus, except for the parent bridge.
- * This also removes any child buses, and any devices they may
- * contain in a depth-first manner.
- */
-void pci_stop_and_remove_behind_bridge(struct pci_dev *dev)
-{
-	pci_stop_behind_bridge(dev);
-	__pci_remove_behind_bridge(dev);
-}
-
-static void pci_stop_bus_devices(struct pci_bus *bus)
-{
-	struct list_head *l, *n;
-
-	/*
-	 * VFs could be removed by pci_stop_and_remove_bus_device() in the
-	 *  pci_stop_bus_devices() code path for PF.
-	 *  aka, bus->devices get updated in the process.
-	 * but VFs are inserted after PFs when SRIOV is enabled for PF,
-	 * We can iterate the list backwards to get prev valid PF instead
-	 *  of removed VF.
-	 */
-	list_for_each_prev_safe(l, n, &bus->devices) {
-		struct pci_dev *dev = pci_dev_b(l);
-		pci_stop_bus_device(dev);
-	}
-}
-
-/**
- * pci_stop_bus_device - stop a PCI device and any children
- * @dev: the device to stop
+ * Remove a PCI device from the device lists, informing the drivers
+ * that the device has been removed.  We also remove any subordinate
+ * buses and children in a depth-first manner.
  *
- * Stop a PCI device (detach the driver, remove from the global list
- * and so on). This also stop any subordinate buses and children in a
- * depth-first manner.
+ * For each device we remove, delete the device structure from the
+ * device lists, remove the /proc entry, and notify userspace
+ * (/sbin/hotplug).
  */
-void pci_stop_bus_device(struct pci_dev *dev)
+void pci_stop_and_remove_bus_device(struct pci_dev *dev)
 {
-	if (dev->subordinate)
-		pci_stop_bus_devices(dev->subordinate);
-
-	pci_stop_dev(dev);
+	pci_stop_bus_device(dev);
+	pci_remove_bus_device(dev);
 }
-
 EXPORT_SYMBOL(pci_stop_and_remove_bus_device);
-EXPORT_SYMBOL(pci_stop_and_remove_behind_bridge);
-EXPORT_SYMBOL_GPL(pci_stop_bus_device);
diff --git a/drivers/pci/rom.c b/drivers/pci/rom.c
index 48ebdb237f3f..0b3037ab8b93 100644
--- a/drivers/pci/rom.c
+++ b/drivers/pci/rom.c
@@ -167,44 +167,6 @@ void __iomem *pci_map_rom(struct pci_dev *pdev, size_t *size)
 	return rom;
 }
 
-#if 0
-/**
- * pci_map_rom_copy - map a PCI ROM to kernel space, create a copy
- * @pdev: pointer to pci device struct
- * @size: pointer to receive size of pci window over ROM
- *
- * Return: kernel virtual pointer to image of ROM
- *
- * Map a PCI ROM into kernel space. If ROM is boot video ROM,
- * the shadow BIOS copy will be returned instead of the
- * actual ROM.
- */
-void __iomem *pci_map_rom_copy(struct pci_dev *pdev, size_t *size)
-{
-	struct resource *res = &pdev->resource[PCI_ROM_RESOURCE];
-	void __iomem *rom;
-
-	rom = pci_map_rom(pdev, size);
-	if (!rom)
-		return NULL;
-
-	if (res->flags & (IORESOURCE_ROM_COPY | IORESOURCE_ROM_SHADOW |
-			  IORESOURCE_ROM_BIOS_COPY))
-		return rom;
-
-	res->start = (unsigned long)kmalloc(*size, GFP_KERNEL);
-	if (!res->start)
-		return rom;
-
-	res->end = res->start + *size;
-	memcpy_fromio((void*)(unsigned long)res->start, rom, *size);
-	pci_unmap_rom(pdev, rom);
-	res->flags |= IORESOURCE_ROM_COPY;
-
-	return (void __iomem *)(unsigned long)res->start;
-}
-#endif  /*  0  */
-
 /**
  * pci_unmap_rom - unmap the ROM from kernel space
  * @pdev: pointer to pci device struct
@@ -226,27 +188,6 @@ void pci_unmap_rom(struct pci_dev *pdev, void __iomem *rom)
 		pci_disable_rom(pdev);
 }
 
-#if 0
-/**
- * pci_remove_rom - disable the ROM and remove its sysfs attribute
- * @pdev: pointer to pci device struct
- *
- * Remove the rom file in sysfs and disable ROM decoding.
- */
-void pci_remove_rom(struct pci_dev *pdev)
-{
-	struct resource *res = &pdev->resource[PCI_ROM_RESOURCE];
-
-	if (pci_resource_len(pdev, PCI_ROM_RESOURCE))
-		sysfs_remove_bin_file(&pdev->dev.kobj, pdev->rom_attr);
-	if (!(res->flags & (IORESOURCE_ROM_ENABLE |
-			    IORESOURCE_ROM_SHADOW |
-			    IORESOURCE_ROM_BIOS_COPY |
-			    IORESOURCE_ROM_COPY)))
-		pci_disable_rom(pdev);
-}
-#endif  /*  0  */
-
 /**
  * pci_cleanup_rom - free the ROM copy created by pci_map_rom_copy
  * @pdev: pointer to pci device struct
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 993d4a0a2469..bf969ba58e59 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -41,7 +41,7 @@ pci_find_upstream_pcie_bridge(struct pci_dev *pdev)
 			continue;
 		}
 		/* PCI device should connect to a PCIe bridge */
-		if (pdev->pcie_type != PCI_EXP_TYPE_PCI_BRIDGE) {
+		if (pci_pcie_type(pdev) != PCI_EXP_TYPE_PCI_BRIDGE) {
 			/* Busted hardware? */
 			WARN_ON_ONCE(1);
 			return NULL;
@@ -130,16 +130,14 @@ pci_find_next_bus(const struct pci_bus *from)
  * decrement the reference count by calling pci_dev_put().
  * If no device is found, %NULL is returned.
  */
-struct pci_dev * pci_get_slot(struct pci_bus *bus, unsigned int devfn)
+struct pci_dev *pci_get_slot(struct pci_bus *bus, unsigned int devfn)
 {
-	struct list_head *tmp;
 	struct pci_dev *dev;
 
 	WARN_ON(in_interrupt());
 	down_read(&pci_bus_sem);
 
-	list_for_each(tmp, &bus->devices) {
-		dev = pci_dev_b(tmp);
+	list_for_each_entry(dev, &bus->devices, bus_list) {
 		if (dev->devfn == devfn)
 			goto out;
 	}
@@ -245,30 +243,14 @@ struct pci_dev *pci_get_subsys(unsigned int vendor, unsigned int device,
 			       unsigned int ss_vendor, unsigned int ss_device,
 			       struct pci_dev *from)
 {
-	struct pci_dev *pdev;
-	struct pci_device_id *id;
-
-	/*
-	 * pci_find_subsys() can be called on the ide_setup() path,
-	 * super-early in boot.  But the down_read() will enable local
-	 * interrupts, which can cause some machines to crash.  So here we
-	 * detect and flag that situation and bail out early.
-	 */
-	if (unlikely(no_pci_devices()))
-		return NULL;
-
-	id = kzalloc(sizeof(*id), GFP_KERNEL);
-	if (!id)
-		return NULL;
-	id->vendor = vendor;
-	id->device = device;
-	id->subvendor = ss_vendor;
-	id->subdevice = ss_device;
-
-	pdev = pci_get_dev_by_id(id, from);
-	kfree(id);
-
-	return pdev;
+	struct pci_device_id id = {
+		.vendor = vendor,
+		.device = device,
+		.subvendor = ss_vendor,
+		.subdevice = ss_device,
+	};
+
+	return pci_get_dev_by_id(&id, from);
 }
 
 /**
@@ -307,19 +289,16 @@ pci_get_device(unsigned int vendor, unsigned int device, struct pci_dev *from)
  */
 struct pci_dev *pci_get_class(unsigned int class, struct pci_dev *from)
 {
-	struct pci_dev *dev;
-	struct pci_device_id *id;
-
-	id = kzalloc(sizeof(*id), GFP_KERNEL);
-	if (!id)
-		return NULL;
-	id->vendor = id->device = id->subvendor = id->subdevice = PCI_ANY_ID;
-	id->class_mask = PCI_ANY_ID;
-	id->class = class;
-
-	dev = pci_get_dev_by_id(id, from);
-	kfree(id);
-	return dev;
+	struct pci_device_id id = {
+		.vendor = PCI_ANY_ID,
+		.device = PCI_ANY_ID,
+		.subvendor = PCI_ANY_ID,
+		.subdevice = PCI_ANY_ID,
+		.class_mask = PCI_ANY_ID,
+		.class = class,
+	};
+
+	return pci_get_dev_by_id(&id, from);
 }
 
 /**
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index fb506137aaee..1e808ca338f8 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -697,6 +697,38 @@ static resource_size_t calculate_memsize(resource_size_t size,
 	return size;
 }
 
+resource_size_t __weak pcibios_window_alignment(struct pci_bus *bus,
+						unsigned long type)
+{
+	return 1;
+}
+
+#define PCI_P2P_DEFAULT_MEM_ALIGN	0x100000	/* 1MiB */
+#define PCI_P2P_DEFAULT_IO_ALIGN	0x1000		/* 4KiB */
+#define PCI_P2P_DEFAULT_IO_ALIGN_1K	0x400		/* 1KiB */
+
+static resource_size_t window_alignment(struct pci_bus *bus,
+					unsigned long type)
+{
+	resource_size_t align = 1, arch_align;
+
+	if (type & IORESOURCE_MEM)
+		align = PCI_P2P_DEFAULT_MEM_ALIGN;
+	else if (type & IORESOURCE_IO) {
+		/*
+		 * Per spec, I/O windows are 4K-aligned, but some
+		 * bridges have an extension to support 1K alignment.
+		 */
+		if (bus->self->io_window_1k)
+			align = PCI_P2P_DEFAULT_IO_ALIGN_1K;
+		else
+			align = PCI_P2P_DEFAULT_IO_ALIGN;
+	}
+
+	arch_align = pcibios_window_alignment(bus, type);
+	return max(align, arch_align);
+}
+
 /**
  * pbus_size_io() - size the io window of a given bus
  *
@@ -717,17 +749,12 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO);
 	unsigned long size = 0, size0 = 0, size1 = 0;
 	resource_size_t children_add_size = 0;
-	resource_size_t min_align = 4096, align;
+	resource_size_t min_align, io_align, align;
 
 	if (!b_res)
  		return;
 
-	/*
-	 * Per spec, I/O windows are 4K-aligned, but some bridges have an
-	 * extension to support 1K alignment.
-	 */
-	if (bus->self->io_window_1k)
-		min_align = 1024;
+	io_align = min_align = window_alignment(bus, IORESOURCE_IO);
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		int i;
 
@@ -754,8 +781,8 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 		}
 	}
 
-	if (min_align > 4096)
-		min_align = 4096;
+	if (min_align > io_align)
+		min_align = io_align;
 
 	size0 = calculate_iosize(size, min_size, size1,
 			resource_size(b_res), min_align);
@@ -785,6 +812,28 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 	}
 }
 
+static inline resource_size_t calculate_mem_align(resource_size_t *aligns,
+						  int max_order)
+{
+	resource_size_t align = 0;
+	resource_size_t min_align = 0;
+	int order;
+
+	for (order = 0; order <= max_order; order++) {
+		resource_size_t align1 = 1;
+
+		align1 <<= (order + 20);
+
+		if (!align)
+			min_align = align1;
+		else if (ALIGN(align + min_align, min_align) < align1)
+			min_align = align1 >> 1;
+		align += aligns[order];
+	}
+
+	return min_align;
+}
+
 /**
  * pbus_size_mem() - size the memory window of a given bus
  *
@@ -864,19 +913,9 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				children_add_size += get_res_add_size(realloc_head, r);
 		}
 	}
-	align = 0;
-	min_align = 0;
-	for (order = 0; order <= max_order; order++) {
-		resource_size_t align1 = 1;
 
-		align1 <<= (order + 20);
-
-		if (!align)
-			min_align = align1;
-		else if (ALIGN(align + min_align, min_align) < align1)
-			min_align = align1 >> 1;
-		align += aligns[order];
-	}
+	min_align = calculate_mem_align(aligns, max_order);
+	min_align = max(min_align, window_alignment(bus, b_res->flags & mask));
 	size0 = calculate_memsize(size, min_size, 0, resource_size(b_res), min_align);
 	if (children_add_size > add_size)
 		add_size = children_add_size;
diff --git a/drivers/pci/setup-irq.c b/drivers/pci/setup-irq.c
index eb219a1d16f7..9bd6864ec5d3 100644
--- a/drivers/pci/setup-irq.c
+++ b/drivers/pci/setup-irq.c
@@ -17,8 +17,13 @@
 #include <linux/ioport.h>
 #include <linux/cache.h>
 
+void __weak pcibios_update_irq(struct pci_dev *dev, int irq)
+{
+	dev_dbg(&dev->dev, "assigning IRQ %02d\n", irq);
+	pci_write_config_byte(dev, PCI_INTERRUPT_LINE, irq);
+}
 
-static void __init
+static void
 pdev_fixup_irq(struct pci_dev *dev,
 	       u8 (*swizzle)(struct pci_dev *, u8 *),
 	       int (*map_irq)(const struct pci_dev *, u8, u8))
@@ -54,7 +59,7 @@ pdev_fixup_irq(struct pci_dev *dev,
 	pcibios_update_irq(dev, irq);
 }
 
-void __init
+void
 pci_fixup_irqs(u8 (*swizzle)(struct pci_dev *, u8 *),
 	       int (*map_irq)(const struct pci_dev *, u8, u8))
 {
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index d6cc62cb4cf7..def8d0b5620c 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -982,7 +982,6 @@ static int pcifront_detach_devices(struct pcifront_device *pdev)
 	int err = 0;
 	int i, num_devs;
 	unsigned int domain, bus, slot, func;
-	struct pci_bus *pci_bus;
 	struct pci_dev *pci_dev;
 	char str[64];
 
@@ -1032,13 +1031,8 @@ static int pcifront_detach_devices(struct pcifront_device *pdev)
 			goto out;
 		}
 
-		pci_bus = pci_find_bus(domain, bus);
-		if (!pci_bus) {
-			dev_dbg(&pdev->xdev->dev, "Cannot get bus %04x:%02x\n",
-				domain, bus);
-			continue;
-		}
-		pci_dev = pci_get_slot(pci_bus, PCI_DEVFN(slot, func));
+		pci_dev = pci_get_domain_bus_and_slot(domain, bus,
+				PCI_DEVFN(slot, func));
 		if (!pci_dev) {
 			dev_dbg(&pdev->xdev->dev,
 				"Cannot get PCI device %04x:%02x:%02x.%d\n",
diff --git a/drivers/pcmcia/cardbus.c b/drivers/pcmcia/cardbus.c
index 24caeaf50529..9d3ac998fc1f 100644
--- a/drivers/pcmcia/cardbus.c
+++ b/drivers/pcmcia/cardbus.c
@@ -105,8 +105,17 @@ int __ref cb_alloc(struct pcmcia_socket *s)
  */
 void cb_free(struct pcmcia_socket *s)
 {
-	struct pci_dev *bridge = s->cb_dev;
+	struct pci_dev *bridge, *dev, *tmp;
+	struct pci_bus *bus;
 
-	if (bridge)
-		pci_stop_and_remove_behind_bridge(bridge);
+	bridge = s->cb_dev;
+	if (!bridge)
+		return;
+
+	bus = bridge->subordinate;
+	if (!bus)
+		return;
+
+	list_for_each_entry_safe(dev, tmp, &bus->devices, bus_list)
+		pci_stop_and_remove_bus_device(dev);
 }
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index c1892f321c46..80978196aae8 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -29,6 +29,13 @@ config APM_POWER
 	  Say Y here to enable support APM status emulation using
 	  battery class devices.
 
+config GENERIC_ADC_BATTERY
+	tristate "Generic battery support using IIO"
+	depends on IIO
+	help
+	  Say Y here to enable support for the generic battery driver
+	  which uses IIO framework to read adc.
+
 config MAX8925_POWER
 	tristate "MAX8925 battery charger support"
 	depends on MFD_MAX8925
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index ee58afb1e71f..e0b4d4284e1d 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -5,6 +5,7 @@ power_supply-$(CONFIG_SYSFS)		+= power_supply_sysfs.o
 power_supply-$(CONFIG_LEDS_TRIGGERS)	+= power_supply_leds.o
 
 obj-$(CONFIG_POWER_SUPPLY)	+= power_supply.o
+obj-$(CONFIG_GENERIC_ADC_BATTERY)	+= generic-adc-battery.o
 
 obj-$(CONFIG_PDA_POWER)		+= pda_power.o
 obj-$(CONFIG_APM_POWER)		+= apm_power.o
diff --git a/drivers/power/generic-adc-battery.c b/drivers/power/generic-adc-battery.c
new file mode 100644
index 000000000000..9bdf44470396
--- /dev/null
+++ b/drivers/power/generic-adc-battery.c
@@ -0,0 +1,422 @@
+/*
+ * Generic battery driver code using IIO
+ * Copyright (C) 2012, Anish Kumar <anish198519851985@gmail.com>
+ * based on jz4740-battery.c
+ * based on s3c_adc_battery.c
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file COPYING in the main directory of this archive for
+ * more details.
+ *
+ */
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+#include <linux/timer.h>
+#include <linux/jiffies.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/iio/consumer.h>
+#include <linux/iio/types.h>
+#include <linux/power/generic-adc-battery.h>
+
+#define JITTER_DEFAULT 10 /* hope 10ms is enough */
+
+enum gab_chan_type {
+	GAB_VOLTAGE = 0,
+	GAB_CURRENT,
+	GAB_POWER,
+	GAB_MAX_CHAN_TYPE
+};
+
+/*
+ * gab_chan_name suggests the standard channel names for commonly used
+ * channel types.
+ */
+static const char *const gab_chan_name[] = {
+	[GAB_VOLTAGE]	= "voltage",
+	[GAB_CURRENT]	= "current",
+	[GAB_POWER]		= "power",
+};
+
+struct gab {
+	struct power_supply	psy;
+	struct iio_channel	*channel[GAB_MAX_CHAN_TYPE];
+	struct gab_platform_data	*pdata;
+	struct delayed_work bat_work;
+	int	level;
+	int	status;
+	bool cable_plugged;
+};
+
+static struct gab *to_generic_bat(struct power_supply *psy)
+{
+	return container_of(psy, struct gab, psy);
+}
+
+static void gab_ext_power_changed(struct power_supply *psy)
+{
+	struct gab *adc_bat = to_generic_bat(psy);
+
+	schedule_delayed_work(&adc_bat->bat_work, msecs_to_jiffies(0));
+}
+
+static const enum power_supply_property gab_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
+	POWER_SUPPLY_PROP_MODEL_NAME,
+};
+
+/*
+ * This properties are set based on the received platform data and this
+ * should correspond one-to-one with enum chan_type.
+ */
+static const enum power_supply_property gab_dyn_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_POWER_NOW,
+};
+
+static bool gab_charge_finished(struct gab *adc_bat)
+{
+	struct gab_platform_data *pdata = adc_bat->pdata;
+	bool ret = gpio_get_value(pdata->gpio_charge_finished);
+	bool inv = pdata->gpio_inverted;
+
+	if (!gpio_is_valid(pdata->gpio_charge_finished))
+		return false;
+	return ret ^ inv;
+}
+
+static int gab_get_status(struct gab *adc_bat)
+{
+	struct gab_platform_data *pdata = adc_bat->pdata;
+	struct power_supply_info *bat_info;
+
+	bat_info = &pdata->battery_info;
+	if (adc_bat->level == bat_info->charge_full_design)
+		return POWER_SUPPLY_STATUS_FULL;
+	return adc_bat->status;
+}
+
+static enum gab_chan_type gab_prop_to_chan(enum power_supply_property psp)
+{
+	switch (psp) {
+	case POWER_SUPPLY_PROP_POWER_NOW:
+		return GAB_POWER;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		return GAB_VOLTAGE;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		return GAB_CURRENT;
+	default:
+		WARN_ON(1);
+		break;
+	}
+	return GAB_POWER;
+}
+
+static int read_channel(struct gab *adc_bat, enum power_supply_property psp,
+		int *result)
+{
+	int ret;
+	int chan_index;
+
+	chan_index = gab_prop_to_chan(psp);
+	ret = iio_read_channel_processed(adc_bat->channel[chan_index],
+			result);
+	if (ret < 0)
+		pr_err("read channel error\n");
+	return ret;
+}
+
+static int gab_get_property(struct power_supply *psy,
+		enum power_supply_property psp, union power_supply_propval *val)
+{
+	struct gab *adc_bat;
+	struct gab_platform_data *pdata;
+	struct power_supply_info *bat_info;
+	int result = 0;
+	int ret = 0;
+
+	adc_bat = to_generic_bat(psy);
+	if (!adc_bat) {
+		dev_err(psy->dev, "no battery infos ?!\n");
+		return -EINVAL;
+	}
+	pdata = adc_bat->pdata;
+	bat_info = &pdata->battery_info;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		gab_get_status(adc_bat);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN:
+		val->intval = 0;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = pdata->cal_charge(result);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+	case POWER_SUPPLY_PROP_POWER_NOW:
+		ret = read_channel(adc_bat, psp, &result);
+		if (ret < 0)
+			goto err;
+		val->intval = result;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = bat_info->technology;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = bat_info->voltage_min_design;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = bat_info->voltage_max_design;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = bat_info->charge_full_design;
+		break;
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = bat_info->name;
+		break;
+	default:
+		return -EINVAL;
+	}
+err:
+	return ret;
+}
+
+static void gab_work(struct work_struct *work)
+{
+	struct gab *adc_bat;
+	struct gab_platform_data *pdata;
+	struct delayed_work *delayed_work;
+	bool is_plugged;
+	int status;
+
+	delayed_work = container_of(work, struct delayed_work, work);
+	adc_bat = container_of(delayed_work, struct gab, bat_work);
+	pdata = adc_bat->pdata;
+	status = adc_bat->status;
+
+	is_plugged = power_supply_am_i_supplied(&adc_bat->psy);
+	adc_bat->cable_plugged = is_plugged;
+
+	if (!is_plugged)
+		adc_bat->status =  POWER_SUPPLY_STATUS_DISCHARGING;
+	else if (gab_charge_finished(adc_bat))
+		adc_bat->status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+	else
+		adc_bat->status = POWER_SUPPLY_STATUS_CHARGING;
+
+	if (status != adc_bat->status)
+		power_supply_changed(&adc_bat->psy);
+}
+
+static irqreturn_t gab_charged(int irq, void *dev_id)
+{
+	struct gab *adc_bat = dev_id;
+	struct gab_platform_data *pdata = adc_bat->pdata;
+	int delay;
+
+	delay = pdata->jitter_delay ? pdata->jitter_delay : JITTER_DEFAULT;
+	schedule_delayed_work(&adc_bat->bat_work,
+			msecs_to_jiffies(delay));
+	return IRQ_HANDLED;
+}
+
+static int __devinit gab_probe(struct platform_device *pdev)
+{
+	struct gab *adc_bat;
+	struct power_supply *psy;
+	struct gab_platform_data *pdata = pdev->dev.platform_data;
+	enum power_supply_property *properties;
+	int ret = 0;
+	int chan;
+	int index = 0;
+
+	adc_bat = devm_kzalloc(&pdev->dev, sizeof(*adc_bat), GFP_KERNEL);
+	if (!adc_bat) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	psy = &adc_bat->psy;
+	psy->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;
+	adc_bat->pdata = pdata;
+
+	/* calculate the total number of channels */
+	chan = ARRAY_SIZE(gab_chan_name);
+
+	/*
+	 * 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) +
+					ARRAY_SIZE(gab_chan_name),
+					sizeof(*psy->properties), GFP_KERNEL);
+	if (!psy->properties) {
+		ret = -ENOMEM;
+		goto first_mem_fail;
+	}
+
+	memcpy(psy->properties, gab_props, sizeof(gab_props));
+	properties = psy->properties + sizeof(gab_props);
+
+	/*
+	 * getting channel from iio and copying the battery properties
+	 * based on the channel supported by consumer device.
+	 */
+	for (chan = 0; chan < ARRAY_SIZE(gab_chan_name); chan++) {
+		adc_bat->channel[chan] = iio_channel_get(dev_name(&pdev->dev),
+						gab_chan_name[chan]);
+		if (IS_ERR(adc_bat->channel[chan])) {
+			ret = PTR_ERR(adc_bat->channel[chan]);
+		} else {
+			/* copying properties for supported channels only */
+			memcpy(properties + sizeof(*(psy->properties)) * index,
+					&gab_dyn_props[chan],
+					sizeof(gab_dyn_props[chan]));
+			index++;
+		}
+	}
+
+	/* none of the channels are supported so let's bail out */
+	if (index == ARRAY_SIZE(gab_chan_name))
+		goto second_mem_fail;
+
+	/*
+	 * Total number of properties is equal to static properties
+	 * plus the dynamic properties.Some properties may not be set
+	 * 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;
+
+	ret = power_supply_register(&pdev->dev, psy);
+	if (ret)
+		goto err_reg_fail;
+
+	INIT_DELAYED_WORK(&adc_bat->bat_work, gab_work);
+
+	if (gpio_is_valid(pdata->gpio_charge_finished)) {
+		int irq;
+		ret = gpio_request(pdata->gpio_charge_finished, "charged");
+		if (ret)
+			goto gpio_req_fail;
+
+		irq = gpio_to_irq(pdata->gpio_charge_finished);
+		ret = request_any_context_irq(irq, gab_charged,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				"battery charged", adc_bat);
+		if (ret)
+			goto err_gpio;
+	}
+
+	platform_set_drvdata(pdev, adc_bat);
+
+	/* Schedule timer to check current status */
+	schedule_delayed_work(&adc_bat->bat_work,
+			msecs_to_jiffies(0));
+	return 0;
+
+err_gpio:
+	gpio_free(pdata->gpio_charge_finished);
+gpio_req_fail:
+	power_supply_unregister(psy);
+err_reg_fail:
+	for (chan = 0; ARRAY_SIZE(gab_chan_name); chan++)
+		iio_channel_release(adc_bat->channel[chan]);
+second_mem_fail:
+	kfree(psy->properties);
+first_mem_fail:
+	return ret;
+}
+
+static int __devexit gab_remove(struct platform_device *pdev)
+{
+	int chan;
+	struct gab *adc_bat = platform_get_drvdata(pdev);
+	struct gab_platform_data *pdata = adc_bat->pdata;
+
+	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);
+		gpio_free(pdata->gpio_charge_finished);
+	}
+
+	for (chan = 0; ARRAY_SIZE(gab_chan_name); chan++)
+		iio_channel_release(adc_bat->channel[chan]);
+
+	kfree(adc_bat->psy.properties);
+	cancel_delayed_work(&adc_bat->bat_work);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int gab_suspend(struct device *dev)
+{
+	struct gab *adc_bat = dev_get_drvdata(dev);
+
+	cancel_delayed_work_sync(&adc_bat->bat_work);
+	adc_bat->status = POWER_SUPPLY_STATUS_UNKNOWN;
+	return 0;
+}
+
+static int gab_resume(struct device *dev)
+{
+	struct gab *adc_bat = dev_get_drvdata(dev);
+	struct gab_platform_data *pdata = adc_bat->pdata;
+	int delay;
+
+	delay = pdata->jitter_delay ? pdata->jitter_delay : JITTER_DEFAULT;
+
+	/* Schedule timer to check current status */
+	schedule_delayed_work(&adc_bat->bat_work,
+			msecs_to_jiffies(delay));
+	return 0;
+}
+
+static const struct dev_pm_ops gab_pm_ops = {
+	.suspend        = gab_suspend,
+	.resume         = gab_resume,
+};
+
+#define GAB_PM_OPS       (&gab_pm_ops)
+#else
+#define GAB_PM_OPS       (NULL)
+#endif
+
+static struct platform_driver gab_driver = {
+	.driver		= {
+		.name	= "generic-adc-battery",
+		.owner	= THIS_MODULE,
+		.pm	= GAB_PM_OPS
+	},
+	.probe		= gab_probe,
+	.remove		= __devexit_p(gab_remove),
+};
+module_platform_driver(gab_driver);
+
+MODULE_AUTHOR("anish kumar <anish198519851985@gmail.com>");
+MODULE_DESCRIPTION("generic battery driver using IIO");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rapidio/devices/tsi721.c b/drivers/rapidio/devices/tsi721.c
index 5d44252b7342..d5e1625bbac2 100644
--- a/drivers/rapidio/devices/tsi721.c
+++ b/drivers/rapidio/devices/tsi721.c
@@ -2219,9 +2219,7 @@ static int __devinit tsi721_probe(struct pci_dev *pdev,
 				  const struct pci_device_id *id)
 {
 	struct tsi721_device *priv;
-	int cap;
 	int err;
-	u32 regval;
 
 	priv = kzalloc(sizeof(struct tsi721_device), GFP_KERNEL);
 	if (priv == NULL) {
@@ -2330,20 +2328,16 @@ static int __devinit tsi721_probe(struct pci_dev *pdev,
 			dev_info(&pdev->dev, "Unable to set consistent DMA mask\n");
 	}
 
-	cap = pci_pcie_cap(pdev);
-	BUG_ON(cap == 0);
+	BUG_ON(!pci_is_pcie(pdev));
 
 	/* Clear "no snoop" and "relaxed ordering" bits, use default MRRS. */
-	pci_read_config_dword(pdev, cap + PCI_EXP_DEVCTL, &regval);
-	regval &= ~(PCI_EXP_DEVCTL_READRQ | PCI_EXP_DEVCTL_RELAX_EN |
-		    PCI_EXP_DEVCTL_NOSNOOP_EN);
-	regval |= 0x2 << MAX_READ_REQUEST_SZ_SHIFT;
-	pci_write_config_dword(pdev, cap + PCI_EXP_DEVCTL, regval);
+	pcie_capability_clear_and_set_word(pdev, PCI_EXP_DEVCTL,
+		PCI_EXP_DEVCTL_READRQ | PCI_EXP_DEVCTL_RELAX_EN |
+		PCI_EXP_DEVCTL_NOSNOOP_EN,
+		0x2 << MAX_READ_REQUEST_SZ_SHIFT);
 
 	/* Adjust PCIe completion timeout. */
-	pci_read_config_dword(pdev, cap + PCI_EXP_DEVCTL2, &regval);
-	regval &= ~(0x0f);
-	pci_write_config_dword(pdev, cap + PCI_EXP_DEVCTL2, regval | 0x2);
+	pcie_capability_clear_and_set_word(pdev, PCI_EXP_DEVCTL2, 0xf, 0x2);
 
 	/*
 	 * FIXUP: correct offsets of MSI-X tables in the MSI-X Capability Block
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 4e932cc695e9..e98a5e7827df 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -33,9 +33,8 @@ config REGULATOR_DUMMY
 	help
 	  If this option is enabled then when a regulator lookup fails
 	  and the board has not specified that it has provided full
-	  constraints then the regulator core will provide an always
-	  enabled dummy regulator will be provided, allowing consumer
-	  drivers to continue.
+	  constraints the regulator core will provide an always
+	  enabled dummy regulator, allowing consumer drivers to continue.
 
 	  A warning will be generated when this substitution is done.
 
@@ -50,11 +49,11 @@ config REGULATOR_VIRTUAL_CONSUMER
 	tristate "Virtual regulator consumer support"
 	help
 	  This driver provides a virtual consumer for the voltage and
-          current regulator API which provides sysfs controls for
-          configuring the supplies requested.  This is mainly useful
-          for test purposes.
+	  current regulator API which provides sysfs controls for
+	  configuring the supplies requested.  This is mainly useful
+	  for test purposes.
 
-          If unsure, say no.
+	  If unsure, say no.
 
 config REGULATOR_USERSPACE_CONSUMER
 	tristate "Userspace regulator consumer support"
@@ -63,7 +62,7 @@ config REGULATOR_USERSPACE_CONSUMER
 	  from user space. Userspace consumer driver provides ability to
 	  control power supplies for such devices.
 
-          If unsure, say no.
+	  If unsure, say no.
 
 config REGULATOR_GPIO
 	tristate "GPIO regulator support"
@@ -110,6 +109,17 @@ config REGULATOR_DA9052
 	  This driver supports the voltage regulators of DA9052-BC and
 	  DA9053-AA/Bx PMIC.
 
+config REGULATOR_FAN53555
+	tristate "Fairchild FAN53555 Regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports Fairchild FAN53555 Digitally Programmable
+	  TinyBuck Regulator. The FAN53555 is a step-down switching voltage
+	  regulator that delivers a digitally programmable output from an
+	  input voltage supply of 2.5V to 5.5V. The output voltage is
+	  programmed through an I2C interface.
+
 config REGULATOR_ANATOP
 	tristate "Freescale i.MX on-chip ANATOP LDO regulators"
 	depends on MFD_ANATOP
@@ -172,6 +182,14 @@ config REGULATOR_MAX8660
 	  This driver controls a Maxim 8660/8661 voltage output
 	  regulator via I2C bus.
 
+config REGULATOR_MAX8907
+	tristate "Maxim 8907 voltage regulator"
+	depends on MFD_MAX8907
+	help
+	  This driver controls a Maxim 8907 voltage output regulator
+	  via I2C bus. The provided regulator is suitable for Tegra
+	  chip to control Step-Down DC-DC and LDOs.
+
 config REGULATOR_MAX8925
 	tristate "Maxim MAX8925 Power Management IC"
 	depends on MFD_MAX8925
@@ -247,7 +265,7 @@ config REGULATOR_LP8788
 
 config REGULATOR_PCF50633
 	tristate "NXP PCF50633 regulator driver"
-        depends on MFD_PCF50633
+	depends on MFD_PCF50633
 	help
 	 Say Y here to support the voltage regulators and convertors
 	 on PCF50633
@@ -416,7 +434,7 @@ config REGULATOR_WM8350
 	depends on MFD_WM8350
 	help
 	  This driver provides support for the voltage and current regulators
-          of the WM8350 AudioPlus PMIC.
+	  of the WM8350 AudioPlus PMIC.
 
 config REGULATOR_WM8400
 	tristate "Wolfson Microelectronics WM8400 AudioPlus PMIC"
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 3342615cf25e..e431eed8a878 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
 obj-$(CONFIG_REGULATOR_DBX500_PRCMU) += dbx500-prcmu.o
 obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
+obj-$(CONFIG_REGULATOR_FAN53555) += fan53555.o
 obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
@@ -30,6 +31,7 @@ obj-$(CONFIG_REGULATOR_LP8788) += lp8788-ldo.o
 obj-$(CONFIG_REGULATOR_MAX1586) += max1586.o
 obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
 obj-$(CONFIG_REGULATOR_MAX8660) += max8660.o
+obj-$(CONFIG_REGULATOR_MAX8907) += max8907-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8925) += max8925-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8952) += max8952.o
 obj-$(CONFIG_REGULATOR_MAX8997) += max8997.o
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index 6f45bfd22e83..167c93f21981 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -162,7 +162,7 @@ static struct aat2870_regulator *aat2870_get_regulator(int id)
 static int aat2870_regulator_probe(struct platform_device *pdev)
 {
 	struct aat2870_regulator *ri;
-	struct regulator_config config = { 0 };
+	struct regulator_config config = { };
 	struct regulator_dev *rdev;
 
 	ri = aat2870_get_regulator(pdev->id);
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index c151fd5d8c97..65ad2b36ce36 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -347,17 +347,11 @@ static int ab3100_get_voltage_regulator_external(struct regulator_dev *reg)
 	return abreg->plfdata->external_voltage;
 }
 
-static int ab3100_get_fixed_voltage_regulator(struct regulator_dev *reg)
-{
-	return reg->desc->min_uV;
-}
-
 static struct regulator_ops regulator_ops_fixed = {
 	.list_voltage = regulator_list_voltage_linear,
 	.enable      = ab3100_enable_regulator,
 	.disable     = ab3100_disable_regulator,
 	.is_enabled  = ab3100_is_enabled_regulator,
-	.get_voltage = ab3100_get_fixed_voltage_regulator,
 };
 
 static struct regulator_ops regulator_ops_variable = {
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 10f2f4d4d190..e3d1d063025a 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -37,6 +37,7 @@
  * @voltage_bank: bank to control regulator voltage
  * @voltage_reg: register to control regulator voltage
  * @voltage_mask: mask to control regulator voltage
+ * @voltage_shift: shift to control regulator voltage
  * @delay: startup/set voltage delay in us
  */
 struct ab8500_regulator_info {
@@ -50,6 +51,7 @@ struct ab8500_regulator_info {
 	u8 voltage_bank;
 	u8 voltage_reg;
 	u8 voltage_mask;
+	u8 voltage_shift;
 	unsigned int delay;
 };
 
@@ -195,17 +197,14 @@ static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 	}
 
 	dev_vdbg(rdev_get_dev(rdev),
-		"%s-get_voltage (bank, reg, mask, value): 0x%x, 0x%x, 0x%x,"
-		" 0x%x\n",
-		info->desc.name, info->voltage_bank, info->voltage_reg,
-		info->voltage_mask, regval);
+		"%s-get_voltage (bank, reg, mask, shift, value): "
+		"0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n",
+		info->desc.name, info->voltage_bank,
+		info->voltage_reg, info->voltage_mask,
+		info->voltage_shift, regval);
 
-	/* vintcore has a different layout */
 	val = regval & info->voltage_mask;
-	if (info->desc.id == AB8500_LDO_INTCORE)
-		return val >> 0x3;
-	else
-		return val;
+	return val >> info->voltage_shift;
 }
 
 static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
@@ -221,7 +220,7 @@ static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	}
 
 	/* set the registers for the request */
-	regval = (u8)selector;
+	regval = (u8)selector << info->voltage_shift;
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg,
 			info->voltage_mask, regval);
@@ -238,13 +237,6 @@ static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int ab8500_regulator_enable_time(struct regulator_dev *rdev)
-{
-	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
-
-	return info->delay;
-}
-
 static int ab8500_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 					     unsigned int old_sel,
 					     unsigned int new_sel)
@@ -261,22 +253,14 @@ static struct regulator_ops ab8500_regulator_ops = {
 	.get_voltage_sel = ab8500_regulator_get_voltage_sel,
 	.set_voltage_sel = ab8500_regulator_set_voltage_sel,
 	.list_voltage	= regulator_list_voltage_table,
-	.enable_time	= ab8500_regulator_enable_time,
 	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
-static int ab8500_fixed_get_voltage(struct regulator_dev *rdev)
-{
-	return rdev->desc->min_uV;
-}
-
 static struct regulator_ops ab8500_regulator_fixed_ops = {
 	.enable		= ab8500_regulator_enable,
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
-	.get_voltage	= ab8500_fixed_get_voltage,
 	.list_voltage	= regulator_list_voltage_linear,
-	.enable_time	= ab8500_regulator_enable_time,
 };
 
 static struct ab8500_regulator_info
@@ -358,6 +342,7 @@ static struct ab8500_regulator_info
 		.voltage_bank		= 0x03,
 		.voltage_reg		= 0x80,
 		.voltage_mask		= 0x38,
+		.voltage_shift		= 3,
 	},
 
 	/*
@@ -374,6 +359,7 @@ static struct ab8500_regulator_info
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
 			.min_uV		= 2000000,
+			.enable_time	= 10000,
 		},
 		.delay			= 10000,
 		.update_bank		= 0x03,
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index c8f95c07adb6..d184aa35abcb 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -39,6 +39,8 @@ static struct regulator_ops arizona_ldo1_ops = {
 	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_bypass = regulator_get_bypass_regmap,
+	.set_bypass = regulator_set_bypass_regmap,
 };
 
 static const struct regulator_desc arizona_ldo1 = {
@@ -49,9 +51,11 @@ static const struct regulator_desc arizona_ldo1 = {
 
 	.vsel_reg = ARIZONA_LDO1_CONTROL_1,
 	.vsel_mask = ARIZONA_LDO1_VSEL_MASK,
+	.bypass_reg = ARIZONA_LDO1_CONTROL_1,
+	.bypass_mask = ARIZONA_LDO1_BYPASS,
 	.min_uV = 900000,
 	.uV_step = 50000,
-	.n_voltages = 7,
+	.n_voltages = 6,
 
 	.owner = THIS_MODULE,
 };
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
index 450a069aa9b6..d9b1f82cc5bd 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -82,6 +82,9 @@ static struct regulator_ops arizona_micsupp_ops = {
 
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+
+	.get_bypass = regulator_get_bypass_regmap,
+	.set_bypass = regulator_set_bypass_regmap,
 };
 
 static const struct regulator_desc arizona_micsupp = {
@@ -95,6 +98,8 @@ static const struct regulator_desc arizona_micsupp = {
 	.vsel_mask = ARIZONA_LDO2_VSEL_MASK,
 	.enable_reg = ARIZONA_MIC_CHARGE_PUMP_1,
 	.enable_mask = ARIZONA_CPMIC_ENA,
+	.bypass_reg = ARIZONA_MIC_CHARGE_PUMP_1,
+	.bypass_mask = ARIZONA_CPMIC_BYPASS,
 
 	.owner = THIS_MODULE,
 };
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 48385318175a..2e0352dc26bd 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -77,6 +77,7 @@ struct regulator {
 	struct device *dev;
 	struct list_head list;
 	unsigned int always_on:1;
+	unsigned int bypass:1;
 	int uA_load;
 	int min_uV;
 	int max_uV;
@@ -394,6 +395,9 @@ static ssize_t regulator_status_show(struct device *dev,
 	case REGULATOR_STATUS_STANDBY:
 		label = "standby";
 		break;
+	case REGULATOR_STATUS_BYPASS:
+		label = "bypass";
+		break;
 	case REGULATOR_STATUS_UNDEFINED:
 		label = "undefined";
 		break;
@@ -585,6 +589,27 @@ static ssize_t regulator_suspend_standby_state_show(struct device *dev,
 static DEVICE_ATTR(suspend_standby_state, 0444,
 		regulator_suspend_standby_state_show, NULL);
 
+static ssize_t regulator_bypass_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct regulator_dev *rdev = dev_get_drvdata(dev);
+	const char *report;
+	bool bypass;
+	int ret;
+
+	ret = rdev->desc->ops->get_bypass(rdev, &bypass);
+
+	if (ret != 0)
+		report = "unknown";
+	else if (bypass)
+		report = "enabled";
+	else
+		report = "disabled";
+
+	return sprintf(buf, "%s\n", report);
+}
+static DEVICE_ATTR(bypass, 0444,
+		   regulator_bypass_show, NULL);
 
 /*
  * These are the only attributes are present for all regulators.
@@ -778,6 +803,9 @@ static void print_constraints(struct regulator_dev *rdev)
 	if (constraints->valid_modes_mask & REGULATOR_MODE_STANDBY)
 		count += sprintf(buf + count, "standby");
 
+	if (!count)
+		sprintf(buf, "no parameters");
+
 	rdev_info(rdev, "%s\n", buf);
 
 	if ((constraints->min_uV != constraints->max_uV) &&
@@ -974,6 +1002,7 @@ static int set_supply(struct regulator_dev *rdev,
 		err = -ENOMEM;
 		return err;
 	}
+	supply_rdev->open_count++;
 
 	return 0;
 }
@@ -1720,6 +1749,9 @@ int regulator_disable_deferred(struct regulator *regulator, int ms)
 	if (regulator->always_on)
 		return 0;
 
+	if (!ms)
+		return regulator_disable(regulator);
+
 	mutex_lock(&rdev->mutex);
 	rdev->deferred_disables++;
 	mutex_unlock(&rdev->mutex);
@@ -2178,9 +2210,12 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 		}
 	}
 
-	if (ret == 0 && best_val >= 0)
+	if (ret == 0 && best_val >= 0) {
+		unsigned long data = best_val;
+
 		_notifier_call_chain(rdev, REGULATOR_EVENT_VOLTAGE_CHANGE,
-				     (void *)best_val);
+				     (void *)data);
+	}
 
 	trace_regulator_set_voltage_complete(rdev_get_name(rdev), best_val);
 
@@ -2291,8 +2326,8 @@ int regulator_set_voltage_time(struct regulator *regulator,
 EXPORT_SYMBOL_GPL(regulator_set_voltage_time);
 
 /**
- *regulator_set_voltage_time_sel - get raise/fall time
- * @regulator: regulator source
+ * regulator_set_voltage_time_sel - get raise/fall time
+ * @rdev: regulator source device
  * @old_selector: selector for starting voltage
  * @new_selector: selector for target voltage
  *
@@ -2388,6 +2423,8 @@ static int _regulator_get_voltage(struct regulator_dev *rdev)
 		ret = rdev->desc->ops->list_voltage(rdev, sel);
 	} else if (rdev->desc->ops->get_voltage) {
 		ret = rdev->desc->ops->get_voltage(rdev);
+	} else if (rdev->desc->ops->list_voltage) {
+		ret = rdev->desc->ops->list_voltage(rdev, 0);
 	} else {
 		return -EINVAL;
 	}
@@ -2674,6 +2711,100 @@ out:
 EXPORT_SYMBOL_GPL(regulator_set_optimum_mode);
 
 /**
+ * regulator_set_bypass_regmap - Default set_bypass() using regmap
+ *
+ * @rdev: device to operate on.
+ * @enable: state to set.
+ */
+int regulator_set_bypass_regmap(struct regulator_dev *rdev, bool enable)
+{
+	unsigned int val;
+
+	if (enable)
+		val = rdev->desc->bypass_mask;
+	else
+		val = 0;
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->bypass_reg,
+				  rdev->desc->bypass_mask, val);
+}
+EXPORT_SYMBOL_GPL(regulator_set_bypass_regmap);
+
+/**
+ * regulator_get_bypass_regmap - Default get_bypass() using regmap
+ *
+ * @rdev: device to operate on.
+ * @enable: current state.
+ */
+int regulator_get_bypass_regmap(struct regulator_dev *rdev, bool *enable)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(rdev->regmap, rdev->desc->bypass_reg, &val);
+	if (ret != 0)
+		return ret;
+
+	*enable = val & rdev->desc->bypass_mask;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(regulator_get_bypass_regmap);
+
+/**
+ * regulator_allow_bypass - allow the regulator to go into bypass mode
+ *
+ * @regulator: Regulator to configure
+ * @allow: enable or disable bypass mode
+ *
+ * Allow the regulator to go into bypass mode if all other consumers
+ * for the regulator also enable bypass mode and the machine
+ * constraints allow this.  Bypass mode means that the regulator is
+ * simply passing the input directly to the output with no regulation.
+ */
+int regulator_allow_bypass(struct regulator *regulator, bool enable)
+{
+	struct regulator_dev *rdev = regulator->rdev;
+	int ret = 0;
+
+	if (!rdev->desc->ops->set_bypass)
+		return 0;
+
+	if (rdev->constraints &&
+	    !(rdev->constraints->valid_ops_mask & REGULATOR_CHANGE_BYPASS))
+		return 0;
+
+	mutex_lock(&rdev->mutex);
+
+	if (enable && !regulator->bypass) {
+		rdev->bypass_count++;
+
+		if (rdev->bypass_count == rdev->open_count) {
+			ret = rdev->desc->ops->set_bypass(rdev, enable);
+			if (ret != 0)
+				rdev->bypass_count--;
+		}
+
+	} else if (!enable && regulator->bypass) {
+		rdev->bypass_count--;
+
+		if (rdev->bypass_count != rdev->open_count) {
+			ret = rdev->desc->ops->set_bypass(rdev, enable);
+			if (ret != 0)
+				rdev->bypass_count++;
+		}
+	}
+
+	if (ret == 0)
+		regulator->bypass = enable;
+
+	mutex_unlock(&rdev->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regulator_allow_bypass);
+
+/**
  * regulator_register_notifier - register regulator event notifier
  * @regulator: regulator source
  * @nb: notifier block
@@ -3011,7 +3142,8 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 
 	/* some attributes need specific methods to be displayed */
 	if ((ops->get_voltage && ops->get_voltage(rdev) >= 0) ||
-	    (ops->get_voltage_sel && ops->get_voltage_sel(rdev) >= 0)) {
+	    (ops->get_voltage_sel && ops->get_voltage_sel(rdev) >= 0) ||
+	    (ops->list_voltage && ops->list_voltage(rdev, 0) >= 0)) {
 		status = device_create_file(dev, &dev_attr_microvolts);
 		if (status < 0)
 			return status;
@@ -3036,6 +3168,11 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 		if (status < 0)
 			return status;
 	}
+	if (ops->get_bypass) {
+		status = device_create_file(dev, &dev_attr_bypass);
+		if (status < 0)
+			return status;
+	}
 
 	/* some attributes are type-specific */
 	if (rdev->desc->type == REGULATOR_CURRENT) {
@@ -3124,6 +3261,8 @@ static void rdev_init_debugfs(struct regulator_dev *rdev)
 			   &rdev->use_count);
 	debugfs_create_u32("open_count", 0444, rdev->debugfs,
 			   &rdev->open_count);
+	debugfs_create_u32("bypass_count", 0444, rdev->debugfs,
+			   &rdev->bypass_count);
 }
 
 /**
@@ -3189,8 +3328,10 @@ regulator_register(const struct regulator_desc *regulator_desc,
 	rdev->desc = regulator_desc;
 	if (config->regmap)
 		rdev->regmap = config->regmap;
-	else
+	else if (dev_get_regmap(dev, NULL))
 		rdev->regmap = dev_get_regmap(dev, NULL);
+	else if (dev->parent)
+		rdev->regmap = dev_get_regmap(dev->parent, NULL);
 	INIT_LIST_HEAD(&rdev->consumer_list);
 	INIT_LIST_HEAD(&rdev->list);
 	BLOCKING_INIT_NOTIFIER_HEAD(&rdev->notifier);
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index 903299cf15cf..27355b1199e5 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -133,8 +133,8 @@ static int da9052_dcdc_set_current_limit(struct regulator_dev *rdev, int min_uA,
 	    max_uA < da9052_current_limits[row][DA9052_MIN_UA])
 		return -EINVAL;
 
-	for (i = 0; i < DA9052_CURRENT_RANGE; i++) {
-		if (min_uA <= da9052_current_limits[row][i]) {
+	for (i = DA9052_CURRENT_RANGE - 1; i >= 0; i--) {
+		if (da9052_current_limits[row][i] <= max_uA) {
 			reg_val = i;
 			break;
 		}
diff --git a/drivers/regulator/dummy.c b/drivers/regulator/dummy.c
index 86f655c7f7a1..03a1d7c11ef2 100644
--- a/drivers/regulator/dummy.c
+++ b/drivers/regulator/dummy.c
@@ -30,7 +30,7 @@ static struct regulator_init_data dummy_initdata;
 static struct regulator_ops dummy_ops;
 
 static struct regulator_desc dummy_desc = {
-	.name = "dummy",
+	.name = "regulator-dummy",
 	.id = -1,
 	.type = REGULATOR_VOLTAGE,
 	.owner = THIS_MODULE,
diff --git a/drivers/regulator/fan53555.c b/drivers/regulator/fan53555.c
new file mode 100644
index 000000000000..339f4d732e97
--- /dev/null
+++ b/drivers/regulator/fan53555.c
@@ -0,0 +1,322 @@
+/*
+ * FAN53555 Fairchild Digitally Programmable TinyBuck Regulator Driver.
+ *
+ * Supported Part Numbers:
+ * FAN53555UC00X/01X/03X/04X/05X
+ *
+ * Copyright (c) 2012 Marvell Technology Ltd.
+ * Yunfan Zhang <yfzhang@marvell.com>
+ *
+ * This package 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/param.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/regulator/fan53555.h>
+
+/* Voltage setting */
+#define FAN53555_VSEL0		0x00
+#define FAN53555_VSEL1		0x01
+/* Control register */
+#define FAN53555_CONTROL	0x02
+/* IC Type */
+#define FAN53555_ID1		0x03
+/* IC mask version */
+#define FAN53555_ID2		0x04
+/* Monitor register */
+#define FAN53555_MONITOR	0x05
+
+/* VSEL bit definitions */
+#define VSEL_BUCK_EN	(1 << 7)
+#define VSEL_MODE		(1 << 6)
+#define VSEL_NSEL_MASK	0x3F
+/* Chip ID and Verison */
+#define DIE_ID		0x0F	/* ID1 */
+#define DIE_REV		0x0F	/* ID2 */
+/* Control bit definitions */
+#define CTL_OUTPUT_DISCHG	(1 << 7)
+#define CTL_SLEW_MASK		(0x7 << 4)
+#define CTL_SLEW_SHIFT		4
+#define CTL_RESET			(1 << 2)
+
+#define FAN53555_NVOLTAGES	64	/* Numbers of voltages */
+
+/* IC Type */
+enum {
+	FAN53555_CHIP_ID_00 = 0,
+	FAN53555_CHIP_ID_01,
+	FAN53555_CHIP_ID_02,
+	FAN53555_CHIP_ID_03,
+	FAN53555_CHIP_ID_04,
+	FAN53555_CHIP_ID_05,
+};
+
+struct fan53555_device_info {
+	struct regmap *regmap;
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct regulator_init_data *regulator;
+	/* IC Type and Rev */
+	int chip_id;
+	int chip_rev;
+	/* Voltage setting register */
+	unsigned int vol_reg;
+	unsigned int sleep_reg;
+	/* Voltage range and step(linear) */
+	unsigned int vsel_min;
+	unsigned int vsel_step;
+	/* Voltage slew rate limiting */
+	unsigned int slew_rate;
+	/* Sleep voltage cache */
+	unsigned int sleep_vol_cache;
+};
+
+static int fan53555_set_suspend_voltage(struct regulator_dev *rdev, int uV)
+{
+	struct fan53555_device_info *di = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (di->sleep_vol_cache == uV)
+		return 0;
+	ret = regulator_map_voltage_linear(rdev, uV, uV);
+	if (ret < 0)
+		return -EINVAL;
+	ret = regmap_update_bits(di->regmap, di->sleep_reg,
+					VSEL_NSEL_MASK, ret);
+	if (ret < 0)
+		return -EINVAL;
+	/* Cache the sleep voltage setting.
+	 * Might not be the real voltage which is rounded */
+	di->sleep_vol_cache = uV;
+
+	return 0;
+}
+
+static int fan53555_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct fan53555_device_info *di = rdev_get_drvdata(rdev);
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		regmap_update_bits(di->regmap, di->vol_reg,
+				VSEL_MODE, VSEL_MODE);
+		break;
+	case REGULATOR_MODE_NORMAL:
+		regmap_update_bits(di->regmap, di->vol_reg, VSEL_MODE, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static unsigned int fan53555_get_mode(struct regulator_dev *rdev)
+{
+	struct fan53555_device_info *di = rdev_get_drvdata(rdev);
+	unsigned int val;
+	int ret = 0;
+
+	ret = regmap_read(di->regmap, di->vol_reg, &val);
+	if (ret < 0)
+		return ret;
+	if (val & VSEL_MODE)
+		return REGULATOR_MODE_FAST;
+	else
+		return REGULATOR_MODE_NORMAL;
+}
+
+static struct regulator_ops fan53555_regulator_ops = {
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.map_voltage = regulator_map_voltage_linear,
+	.list_voltage = regulator_list_voltage_linear,
+	.set_suspend_voltage = fan53555_set_suspend_voltage,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.set_mode = fan53555_set_mode,
+	.get_mode = fan53555_get_mode,
+};
+
+/* For 00,01,03,05 options:
+ * VOUT = 0.60V + NSELx * 10mV, from 0.60 to 1.23V.
+ * For 04 option:
+ * VOUT = 0.603V + NSELx * 12.826mV, from 0.603 to 1.411V.
+ * */
+static int fan53555_device_setup(struct fan53555_device_info *di,
+				struct fan53555_platform_data *pdata)
+{
+	unsigned int reg, data, mask;
+
+	/* Setup voltage control register */
+	switch (pdata->sleep_vsel_id) {
+	case FAN53555_VSEL_ID_0:
+		di->sleep_reg = FAN53555_VSEL0;
+		di->vol_reg = FAN53555_VSEL1;
+		break;
+	case FAN53555_VSEL_ID_1:
+		di->sleep_reg = FAN53555_VSEL1;
+		di->vol_reg = FAN53555_VSEL0;
+		break;
+	default:
+		dev_err(di->dev, "Invalid VSEL ID!\n");
+		return -EINVAL;
+	}
+	/* Init voltage range and step */
+	switch (di->chip_id) {
+	case FAN53555_CHIP_ID_00:
+	case FAN53555_CHIP_ID_01:
+	case FAN53555_CHIP_ID_03:
+	case FAN53555_CHIP_ID_05:
+		di->vsel_min = 600000;
+		di->vsel_step = 10000;
+		break;
+	case FAN53555_CHIP_ID_04:
+		di->vsel_min = 603000;
+		di->vsel_step = 12826;
+		break;
+	default:
+		dev_err(di->dev,
+			"Chip ID[%d]\n not supported!\n", di->chip_id);
+		return -EINVAL;
+	}
+	/* Init slew rate */
+	if (pdata->slew_rate & 0x7)
+		di->slew_rate = pdata->slew_rate;
+	else
+		di->slew_rate = FAN53555_SLEW_RATE_64MV;
+	reg = FAN53555_CONTROL;
+	data = di->slew_rate << CTL_SLEW_SHIFT;
+	mask = CTL_SLEW_MASK;
+	return regmap_update_bits(di->regmap, reg, mask, data);
+}
+
+static int fan53555_regulator_register(struct fan53555_device_info *di,
+			struct regulator_config *config)
+{
+	struct regulator_desc *rdesc = &di->desc;
+
+	rdesc->name = "fan53555-reg";
+	rdesc->ops = &fan53555_regulator_ops;
+	rdesc->type = REGULATOR_VOLTAGE;
+	rdesc->n_voltages = FAN53555_NVOLTAGES;
+	rdesc->enable_reg = di->vol_reg;
+	rdesc->enable_mask = VSEL_BUCK_EN;
+	rdesc->min_uV = di->vsel_min;
+	rdesc->uV_step = di->vsel_step;
+	rdesc->vsel_reg = di->vol_reg;
+	rdesc->vsel_mask = VSEL_NSEL_MASK;
+	rdesc->owner = THIS_MODULE;
+
+	di->rdev = regulator_register(&di->desc, config);
+	if (IS_ERR(di->rdev))
+		return PTR_ERR(di->rdev);
+	return 0;
+
+}
+
+static struct regmap_config fan53555_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static int __devinit fan53555_regulator_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct fan53555_device_info *di;
+	struct fan53555_platform_data *pdata;
+	struct regulator_config config = { };
+	unsigned int val;
+	int ret;
+
+	pdata = client->dev.platform_data;
+	if (!pdata || !pdata->regulator) {
+		dev_err(&client->dev, "Platform data not found!\n");
+		return -ENODEV;
+	}
+
+	di = devm_kzalloc(&client->dev, sizeof(struct fan53555_device_info),
+					GFP_KERNEL);
+	if (!di) {
+		dev_err(&client->dev, "Failed to allocate device info data!\n");
+		return -ENOMEM;
+	}
+	di->regmap = devm_regmap_init_i2c(client, &fan53555_regmap_config);
+	if (IS_ERR(di->regmap)) {
+		dev_err(&client->dev, "Failed to allocate regmap!\n");
+		return PTR_ERR(di->regmap);
+	}
+	di->dev = &client->dev;
+	di->regulator = pdata->regulator;
+	i2c_set_clientdata(client, di);
+	/* Get chip ID */
+	ret = regmap_read(di->regmap, FAN53555_ID1, &val);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to get chip ID!\n");
+		return -ENODEV;
+	}
+	di->chip_id = val & DIE_ID;
+	/* Get chip revision */
+	ret = regmap_read(di->regmap, FAN53555_ID2, &val);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to get chip Rev!\n");
+		return -ENODEV;
+	}
+	di->chip_rev = val & DIE_REV;
+	dev_info(&client->dev, "FAN53555 Option[%d] Rev[%d] Detected!\n",
+				di->chip_id, di->chip_rev);
+	/* Device init */
+	ret = fan53555_device_setup(di, pdata);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to setup device!\n");
+		return ret;
+	}
+	/* Register regulator */
+	config.dev = di->dev;
+	config.init_data = di->regulator;
+	config.regmap = di->regmap;
+	config.driver_data = di;
+	ret = fan53555_regulator_register(di, &config);
+	if (ret < 0)
+		dev_err(&client->dev, "Failed to register regulator!\n");
+	return ret;
+
+}
+
+static int __devexit fan53555_regulator_remove(struct i2c_client *client)
+{
+	struct fan53555_device_info *di = i2c_get_clientdata(client);
+
+	regulator_unregister(di->rdev);
+	return 0;
+}
+
+static const struct i2c_device_id fan53555_id[] = {
+	{"fan53555", -1},
+	{ },
+};
+
+static struct i2c_driver fan53555_regulator_driver = {
+	.driver = {
+		.name = "fan53555-regulator",
+	},
+	.probe = fan53555_regulator_probe,
+	.remove = __devexit_p(fan53555_regulator_remove),
+	.id_table = fan53555_id,
+};
+
+module_i2c_driver(fan53555_regulator_driver);
+
+MODULE_AUTHOR("Yunfan Zhang <yfzhang@marvell.com>");
+MODULE_DESCRIPTION("FAN53555 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/isl6271a-regulator.c b/drivers/regulator/isl6271a-regulator.c
index 1d145a07ada9..d8ecf49a5777 100644
--- a/drivers/regulator/isl6271a-regulator.c
+++ b/drivers/regulator/isl6271a-regulator.c
@@ -73,13 +73,7 @@ static struct regulator_ops isl_core_ops = {
 	.map_voltage	= regulator_map_voltage_linear,
 };
 
-static int isl6271a_get_fixed_voltage(struct regulator_dev *dev)
-{
-	return dev->desc->min_uV;
-}
-
 static struct regulator_ops isl_fixed_ops = {
-	.get_voltage	= isl6271a_get_fixed_voltage,
 	.list_voltage	= regulator_list_voltage_linear,
 };
 
diff --git a/drivers/regulator/lp872x.c b/drivers/regulator/lp872x.c
index 212c38eaba70..708f4b6a17dc 100644
--- a/drivers/regulator/lp872x.c
+++ b/drivers/regulator/lp872x.c
@@ -86,6 +86,10 @@
 #define EXTERN_DVS_USED			0
 #define MAX_DELAY			6
 
+/* Default DVS Mode */
+#define LP8720_DEFAULT_DVS		0
+#define LP8725_DEFAULT_DVS		BIT(2)
+
 /* dump registers in regmap-debugfs */
 #define MAX_REGISTERS			0x0F
 
@@ -269,9 +273,9 @@ static int lp872x_regulator_enable_time(struct regulator_dev *rdev)
 	return val > MAX_DELAY ? 0 : val * time_step_us;
 }
 
-static void lp872x_set_dvs(struct lp872x *lp, int gpio)
+static void lp872x_set_dvs(struct lp872x *lp, enum lp872x_dvs_sel dvs_sel,
+			int gpio)
 {
-	enum lp872x_dvs_sel dvs_sel = lp->pdata->dvs->vsel;
 	enum lp872x_dvs_state state;
 
 	state = dvs_sel == SEL_V1 ? DVS_HIGH : DVS_LOW;
@@ -339,10 +343,10 @@ static int lp872x_buck_set_voltage_sel(struct regulator_dev *rdev,
 	struct lp872x *lp = rdev_get_drvdata(rdev);
 	enum lp872x_regulator_id buck = rdev_get_id(rdev);
 	u8 addr, mask = LP872X_VOUT_M;
-	struct lp872x_dvs *dvs = lp->pdata->dvs;
+	struct lp872x_dvs *dvs = lp->pdata ? lp->pdata->dvs : NULL;
 
 	if (dvs && gpio_is_valid(dvs->gpio))
-		lp872x_set_dvs(lp, dvs->gpio);
+		lp872x_set_dvs(lp, dvs->vsel, dvs->gpio);
 
 	addr = lp872x_select_buck_vout_addr(lp, buck);
 	if (!lp872x_is_valid_buck_addr(addr))
@@ -374,8 +378,8 @@ static int lp8725_buck_set_current_limit(struct regulator_dev *rdev,
 {
 	struct lp872x *lp = rdev_get_drvdata(rdev);
 	enum lp872x_regulator_id buck = rdev_get_id(rdev);
-	int i, max = ARRAY_SIZE(lp8725_buck_uA);
-	u8 addr, val;
+	int i;
+	u8 addr;
 
 	switch (buck) {
 	case LP8725_ID_BUCK1:
@@ -388,17 +392,15 @@ static int lp8725_buck_set_current_limit(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	for (i = 0 ; i < max ; i++)
+	for (i = ARRAY_SIZE(lp8725_buck_uA) - 1 ; i >= 0; i--) {
 		if (lp8725_buck_uA[i] >= min_uA &&
 			lp8725_buck_uA[i] <= max_uA)
-			break;
-
-	if (i == max)
-		return -EINVAL;
-
-	val = i << LP8725_BUCK_CL_S;
+			return lp872x_update_bits(lp, addr,
+						  LP8725_BUCK_CL_M,
+						  i << LP8725_BUCK_CL_S);
+	}
 
-	return lp872x_update_bits(lp, addr, LP8725_BUCK_CL_M, val);
+	return -EINVAL;
 }
 
 static int lp8725_buck_get_current_limit(struct regulator_dev *rdev)
@@ -727,39 +729,16 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 };
 
-static int lp872x_check_dvs_validity(struct lp872x *lp)
-{
-	struct lp872x_dvs *dvs = lp->pdata->dvs;
-	u8 val = 0;
-	int ret;
-
-	ret = lp872x_read_byte(lp, LP872X_GENERAL_CFG, &val);
-	if (ret)
-		return ret;
-
-	ret = 0;
-	if (lp->chipid == LP8720) {
-		if (val & LP8720_EXT_DVS_M)
-			ret = dvs ? 0 : -EINVAL;
-	} else {
-		if ((val & LP8725_DVS1_M) == EXTERN_DVS_USED)
-			ret = dvs ? 0 : -EINVAL;
-	}
-
-	return ret;
-}
-
 static int lp872x_init_dvs(struct lp872x *lp)
 {
 	int ret, gpio;
-	struct lp872x_dvs *dvs = lp->pdata->dvs;
+	struct lp872x_dvs *dvs = lp->pdata ? lp->pdata->dvs : NULL;
 	enum lp872x_dvs_state pinstate;
+	u8 mask[] = { LP8720_EXT_DVS_M, LP8725_DVS1_M | LP8725_DVS2_M };
+	u8 default_dvs_mode[] = { LP8720_DEFAULT_DVS, LP8725_DEFAULT_DVS };
 
-	ret = lp872x_check_dvs_validity(lp);
-	if (ret) {
-		dev_warn(lp->dev, "invalid dvs data: %d\n", ret);
-		return ret;
-	}
+	if (!dvs)
+		goto set_default_dvs_mode;
 
 	gpio = dvs->gpio;
 	if (!gpio_is_valid(gpio)) {
@@ -778,6 +757,10 @@ static int lp872x_init_dvs(struct lp872x *lp)
 	lp->dvs_gpio = gpio;
 
 	return 0;
+
+set_default_dvs_mode:
+	return lp872x_update_bits(lp, LP872X_GENERAL_CFG, mask[lp->chipid],
+				default_dvs_mode[lp->chipid]);
 }
 
 static int lp872x_config(struct lp872x *lp)
@@ -785,24 +768,29 @@ static int lp872x_config(struct lp872x *lp)
 	struct lp872x_platform_data *pdata = lp->pdata;
 	int ret;
 
-	if (!pdata->update_config)
-		return 0;
+	if (!pdata || !pdata->update_config)
+		goto init_dvs;
 
 	ret = lp872x_write_byte(lp, LP872X_GENERAL_CFG, pdata->general_config);
 	if (ret)
 		return ret;
 
+init_dvs:
 	return lp872x_init_dvs(lp);
 }
 
 static struct regulator_init_data
 *lp872x_find_regulator_init_data(int id, struct lp872x *lp)
 {
+	struct lp872x_platform_data *pdata = lp->pdata;
 	int i;
 
+	if (!pdata)
+		return NULL;
+
 	for (i = 0; i < lp->num_regulators; i++) {
-		if (lp->pdata->regulator_data[i].id == id)
-			return lp->pdata->regulator_data[i].init_data;
+		if (pdata->regulator_data[i].id == id)
+			return pdata->regulator_data[i].init_data;
 	}
 
 	return NULL;
@@ -863,18 +851,12 @@ static const struct regmap_config lp872x_regmap_config = {
 static int lp872x_probe(struct i2c_client *cl, const struct i2c_device_id *id)
 {
 	struct lp872x *lp;
-	struct lp872x_platform_data *pdata = cl->dev.platform_data;
 	int ret, size, num_regulators;
 	const int lp872x_num_regulators[] = {
 		[LP8720] = LP8720_NUM_REGULATORS,
 		[LP8725] = LP8725_NUM_REGULATORS,
 	};
 
-	if (!pdata) {
-		dev_err(&cl->dev, "no platform data\n");
-		return -EINVAL;
-	}
-
 	lp = devm_kzalloc(&cl->dev, sizeof(struct lp872x), GFP_KERNEL);
 	if (!lp)
 		goto err_mem;
@@ -894,7 +876,7 @@ static int lp872x_probe(struct i2c_client *cl, const struct i2c_device_id *id)
 	}
 
 	lp->dev = &cl->dev;
-	lp->pdata = pdata;
+	lp->pdata = cl->dev.platform_data;
 	lp->chipid = id->driver_data;
 	lp->num_regulators = num_regulators;
 	i2c_set_clientdata(cl, lp);
diff --git a/drivers/regulator/lp8788-buck.c b/drivers/regulator/lp8788-buck.c
index 6356e821400f..ba3e0aa402de 100644
--- a/drivers/regulator/lp8788-buck.c
+++ b/drivers/regulator/lp8788-buck.c
@@ -69,6 +69,9 @@
 #define PIN_HIGH			1
 #define ENABLE_TIME_USEC		32
 
+#define BUCK_FPWM_MASK(x)		(1 << (x))
+#define BUCK_FPWM_SHIFT(x)		(x)
+
 enum lp8788_dvs_state {
 	DVS_LOW  = GPIOF_OUT_INIT_LOW,
 	DVS_HIGH = GPIOF_OUT_INIT_HIGH,
@@ -86,15 +89,9 @@ enum lp8788_buck_id {
 	BUCK4,
 };
 
-struct lp8788_pwm_map {
-	u8 mask;
-	u8 shift;
-};
-
 struct lp8788_buck {
 	struct lp8788 *lp;
 	struct regulator_dev *regulator;
-	struct lp8788_pwm_map *pmap;
 	void *dvs;
 };
 
@@ -106,29 +103,6 @@ static const int lp8788_buck_vtbl[] = {
 	1950000, 2000000,
 };
 
-/* buck pwm mode selection : used for set/get_mode in regulator ops
- * @forced pwm : fast mode
- * @auto pwm   : normal mode
- */
-static struct lp8788_pwm_map buck_pmap[] = {
-	[BUCK1] = {
-		.mask = LP8788_FPWM_BUCK1_M,
-		.shift = LP8788_FPWM_BUCK1_S,
-	},
-	[BUCK2] = {
-		.mask = LP8788_FPWM_BUCK2_M,
-		.shift = LP8788_FPWM_BUCK2_S,
-	},
-	[BUCK3] = {
-		.mask = LP8788_FPWM_BUCK3_M,
-		.shift = LP8788_FPWM_BUCK3_S,
-	},
-	[BUCK4] = {
-		.mask = LP8788_FPWM_BUCK4_M,
-		.shift = LP8788_FPWM_BUCK4_S,
-	},
-};
-
 static const u8 buck1_vout_addr[] = {
 	LP8788_BUCK1_VOUT0, LP8788_BUCK1_VOUT1,
 	LP8788_BUCK1_VOUT2, LP8788_BUCK1_VOUT3,
@@ -347,41 +321,37 @@ static int lp8788_buck_enable_time(struct regulator_dev *rdev)
 static int lp8788_buck_set_mode(struct regulator_dev *rdev, unsigned int mode)
 {
 	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
-	struct lp8788_pwm_map *pmap = buck->pmap;
-	u8 val;
-
-	if (!pmap)
-		return -EINVAL;
+	enum lp8788_buck_id id = rdev_get_id(rdev);
+	u8 mask, val;
 
+	mask = BUCK_FPWM_MASK(id);
 	switch (mode) {
 	case REGULATOR_MODE_FAST:
-		val = LP8788_FORCE_PWM << pmap->shift;
+		val = LP8788_FORCE_PWM << BUCK_FPWM_SHIFT(id);
 		break;
 	case REGULATOR_MODE_NORMAL:
-		val = LP8788_AUTO_PWM << pmap->shift;
+		val = LP8788_AUTO_PWM << BUCK_FPWM_SHIFT(id);
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	return lp8788_update_bits(buck->lp, LP8788_BUCK_PWM, pmap->mask, val);
+	return lp8788_update_bits(buck->lp, LP8788_BUCK_PWM, mask, val);
 }
 
 static unsigned int lp8788_buck_get_mode(struct regulator_dev *rdev)
 {
 	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
-	struct lp8788_pwm_map *pmap = buck->pmap;
+	enum lp8788_buck_id id = rdev_get_id(rdev);
 	u8 val;
 	int ret;
 
-	if (!pmap)
-		return -EINVAL;
-
 	ret = lp8788_read_byte(buck->lp, LP8788_BUCK_PWM, &val);
 	if (ret)
 		return ret;
 
-	return val & pmap->mask ? REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
+	return val & BUCK_FPWM_MASK(id) ?
+				REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
 }
 
 static struct regulator_ops lp8788_buck12_ops = {
@@ -459,27 +429,6 @@ static struct regulator_desc lp8788_buck_desc[] = {
 	},
 };
 
-static int lp8788_set_default_dvs_ctrl_mode(struct lp8788 *lp,
-					enum lp8788_buck_id id)
-{
-	u8 mask, val;
-
-	switch (id) {
-	case BUCK1:
-		mask = LP8788_BUCK1_DVS_SEL_M;
-		val  = LP8788_BUCK1_DVS_I2C;
-		break;
-	case BUCK2:
-		mask = LP8788_BUCK2_DVS_SEL_M;
-		val  = LP8788_BUCK2_DVS_I2C;
-		break;
-	default:
-		return 0;
-	}
-
-	return lp8788_update_bits(lp, LP8788_BUCK_DVS_SEL, mask, val);
-}
-
 static int _gpio_request(struct lp8788_buck *buck, int gpio, char *name)
 {
 	struct device *dev = buck->lp->dev;
@@ -530,6 +479,7 @@ static int lp8788_init_dvs(struct lp8788_buck *buck, enum lp8788_buck_id id)
 	struct lp8788_platform_data *pdata = buck->lp->pdata;
 	u8 mask[] = { LP8788_BUCK1_DVS_SEL_M, LP8788_BUCK2_DVS_SEL_M };
 	u8 val[]  = { LP8788_BUCK1_DVS_PIN, LP8788_BUCK2_DVS_PIN };
+	u8 default_dvs_mode[] = { LP8788_BUCK1_DVS_I2C, LP8788_BUCK2_DVS_I2C };
 
 	/* no dvs for buck3, 4 */
 	if (id == BUCK3 || id == BUCK4)
@@ -550,7 +500,8 @@ static int lp8788_init_dvs(struct lp8788_buck *buck, enum lp8788_buck_id id)
 				val[id]);
 
 set_default_dvs_mode:
-	return lp8788_set_default_dvs_ctrl_mode(buck->lp, id);
+	return lp8788_update_bits(buck->lp, LP8788_BUCK_DVS_SEL, mask[id],
+				  default_dvs_mode[id]);
 }
 
 static __devinit int lp8788_buck_probe(struct platform_device *pdev)
@@ -567,7 +518,6 @@ static __devinit int lp8788_buck_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	buck->lp = lp;
-	buck->pmap = &buck_pmap[id];
 
 	ret = lp8788_init_dvs(buck, id);
 	if (ret)
diff --git a/drivers/regulator/lp8788-ldo.c b/drivers/regulator/lp8788-ldo.c
index d2122e41a96d..6796eeb47dc6 100644
--- a/drivers/regulator/lp8788-ldo.c
+++ b/drivers/regulator/lp8788-ldo.c
@@ -496,6 +496,7 @@ static struct regulator_desc lp8788_dldo_desc[] = {
 		.name = "dldo12",
 		.id = DLDO12,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
@@ -521,6 +522,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo2",
 		.id = ALDO2,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
@@ -530,6 +532,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo3",
 		.id = ALDO3,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
@@ -539,6 +542,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo4",
 		.id = ALDO4,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_B,
@@ -548,6 +552,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo5",
 		.id = ALDO5,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
@@ -583,6 +588,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo8",
 		.id = ALDO8,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
@@ -592,6 +598,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo9",
 		.id = ALDO9,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
@@ -601,6 +608,7 @@ static struct regulator_desc lp8788_aldo_desc[] = {
 		.name = "aldo10",
 		.id = ALDO10,
 		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 		.enable_reg = LP8788_EN_LDO_C,
diff --git a/drivers/regulator/max77686.c b/drivers/regulator/max77686.c
index c564af6f05a3..2a67d08658ad 100644
--- a/drivers/regulator/max77686.c
+++ b/drivers/regulator/max77686.c
@@ -66,7 +66,7 @@ enum max77686_ramp_rate {
 };
 
 struct max77686_data {
-	struct regulator_dev **rdev;
+	struct regulator_dev *rdev[MAX77686_REGULATORS];
 };
 
 static int max77686_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
@@ -265,6 +265,7 @@ static int max77686_pmic_dt_parse_pdata(struct max77686_dev *iodev,
 		rmatch.of_node = NULL;
 		of_regulator_match(iodev->dev, regulators_np, &rmatch, 1);
 		rdata[i].initdata = rmatch.init_data;
+		rdata[i].of_node = rmatch.of_node;
 	}
 
 	pdata->regulators = rdata;
@@ -283,10 +284,8 @@ static __devinit int max77686_pmic_probe(struct platform_device *pdev)
 {
 	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct max77686_platform_data *pdata = dev_get_platdata(iodev->dev);
-	struct regulator_dev **rdev;
 	struct max77686_data *max77686;
-	int i,  size;
-	int ret = 0;
+	int i, ret = 0;
 	struct regulator_config config = { };
 
 	dev_dbg(&pdev->dev, "%s\n", __func__);
@@ -313,45 +312,38 @@ static __devinit int max77686_pmic_probe(struct platform_device *pdev)
 	if (!max77686)
 		return -ENOMEM;
 
-	size = sizeof(struct regulator_dev *) * MAX77686_REGULATORS;
-	max77686->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
-	if (!max77686->rdev)
-		return -ENOMEM;
-
-	rdev = max77686->rdev;
 	config.dev = &pdev->dev;
 	config.regmap = iodev->regmap;
 	platform_set_drvdata(pdev, max77686);
 
 	for (i = 0; i < MAX77686_REGULATORS; i++) {
 		config.init_data = pdata->regulators[i].initdata;
+		config.of_node = pdata->regulators[i].of_node;
 
-		rdev[i] = regulator_register(&regulators[i], &config);
-		if (IS_ERR(rdev[i])) {
-			ret = PTR_ERR(rdev[i]);
+		max77686->rdev[i] = regulator_register(&regulators[i], &config);
+		if (IS_ERR(max77686->rdev[i])) {
+			ret = PTR_ERR(max77686->rdev[i]);
 			dev_err(&pdev->dev,
 				"regulator init failed for %d\n", i);
-				rdev[i] = NULL;
-				goto err;
+			max77686->rdev[i] = NULL;
+			goto err;
 		}
 	}
 
 	return 0;
 err:
 	while (--i >= 0)
-		regulator_unregister(rdev[i]);
+		regulator_unregister(max77686->rdev[i]);
 	return ret;
 }
 
 static int __devexit max77686_pmic_remove(struct platform_device *pdev)
 {
 	struct max77686_data *max77686 = platform_get_drvdata(pdev);
-	struct regulator_dev **rdev = max77686->rdev;
 	int i;
 
 	for (i = 0; i < MAX77686_REGULATORS; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
+		regulator_unregister(max77686->rdev[i]);
 
 	return 0;
 }
diff --git a/drivers/regulator/max8907-regulator.c b/drivers/regulator/max8907-regulator.c
new file mode 100644
index 000000000000..af7607515ab9
--- /dev/null
+++ b/drivers/regulator/max8907-regulator.c
@@ -0,0 +1,408 @@
+/*
+ * max8907-regulator.c -- support regulators in max8907
+ *
+ * Copyright (C) 2010 Gyungoh Yoo <jack.yoo@maxim-ic.com>
+ * Copyright (C) 2010-2012, NVIDIA CORPORATION. All rights reserved.
+ *
+ * Portions based on drivers/regulator/tps65910-regulator.c,
+ *     Copyright 2010 Texas Instruments Inc.
+ *     Author: Graeme Gregory <gg@slimlogic.co.uk>
+ *     Author: Jorge Eduardo Candelaria <jedu@slimlogic.co.uk>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max8907.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#define MAX8907_II2RR_VERSION_MASK	0xF0
+#define MAX8907_II2RR_VERSION_REV_A	0x00
+#define MAX8907_II2RR_VERSION_REV_B	0x10
+#define MAX8907_II2RR_VERSION_REV_C	0x30
+
+struct max8907_regulator {
+	struct regulator_desc desc[MAX8907_NUM_REGULATORS];
+	struct regulator_dev *rdev[MAX8907_NUM_REGULATORS];
+};
+
+#define REG_MBATT() \
+	[MAX8907_MBATT] = { \
+		.name = "MBATT", \
+		.supply_name = "mbatt", \
+		.id = MAX8907_MBATT, \
+		.ops = &max8907_mbatt_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+	}
+
+#define REG_LDO(ids, supply, base, min, max, step) \
+	[MAX8907_##ids] = { \
+		.name = #ids, \
+		.supply_name = supply, \
+		.id = MAX8907_##ids, \
+		.n_voltages = ((max) - (min)) / (step) + 1, \
+		.ops = &max8907_ldo_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		.min_uV = (min), \
+		.uV_step = (step), \
+		.vsel_reg = (base) + MAX8907_VOUT, \
+		.vsel_mask = 0x3f, \
+		.enable_reg = (base) + MAX8907_CTL, \
+		.enable_mask = MAX8907_MASK_LDO_EN, \
+	}
+
+#define REG_FIXED(ids, supply, voltage) \
+	[MAX8907_##ids] = { \
+		.name = #ids, \
+		.supply_name = supply, \
+		.id = MAX8907_##ids, \
+		.n_voltages = 1, \
+		.ops = &max8907_fixed_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		.min_uV = (voltage), \
+	}
+
+#define REG_OUT5V(ids, supply, base, voltage) \
+	[MAX8907_##ids] = { \
+		.name = #ids, \
+		.supply_name = supply, \
+		.id = MAX8907_##ids, \
+		.n_voltages = 1, \
+		.ops = &max8907_out5v_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		.min_uV = (voltage), \
+		.enable_reg = (base), \
+		.enable_mask = MAX8907_MASK_OUT5V_EN, \
+	}
+
+#define REG_BBAT(ids, supply, base, min, max, step) \
+	[MAX8907_##ids] = { \
+		.name = #ids, \
+		.supply_name = supply, \
+		.id = MAX8907_##ids, \
+		.n_voltages = ((max) - (min)) / (step) + 1, \
+		.ops = &max8907_bbat_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		.min_uV = (min), \
+		.uV_step = (step), \
+		.vsel_reg = (base), \
+		.vsel_mask = MAX8907_MASK_VBBATTCV, \
+	}
+
+#define LDO_750_50(id, supply, base) REG_LDO(id, supply, (base), \
+			750000, 3900000, 50000)
+#define LDO_650_25(id, supply, base) REG_LDO(id, supply, (base), \
+			650000, 2225000, 25000)
+
+static struct regulator_ops max8907_mbatt_ops = {
+};
+
+static struct regulator_ops max8907_ldo_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+static struct regulator_ops max8907_ldo_hwctl_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+};
+
+static struct regulator_ops max8907_fixed_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+};
+
+static struct regulator_ops max8907_out5v_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+static struct regulator_ops max8907_out5v_hwctl_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+};
+
+static struct regulator_ops max8907_bbat_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+};
+
+static struct regulator_desc max8907_regulators[] = {
+	REG_MBATT(),
+	REG_LDO(SD1, "in-v1", MAX8907_REG_SDCTL1, 650000, 2225000, 25000),
+	REG_LDO(SD2, "in-v2", MAX8907_REG_SDCTL2, 637500, 1425000, 12500),
+	REG_LDO(SD3, "in-v3", MAX8907_REG_SDCTL3, 750000, 3900000, 50000),
+	LDO_750_50(LDO1, "in1", MAX8907_REG_LDOCTL1),
+	LDO_650_25(LDO2, "in2", MAX8907_REG_LDOCTL2),
+	LDO_650_25(LDO3, "in3", MAX8907_REG_LDOCTL3),
+	LDO_750_50(LDO4, "in4", MAX8907_REG_LDOCTL4),
+	LDO_750_50(LDO5, "in5", MAX8907_REG_LDOCTL5),
+	LDO_750_50(LDO6, "in6", MAX8907_REG_LDOCTL6),
+	LDO_750_50(LDO7, "in7", MAX8907_REG_LDOCTL7),
+	LDO_750_50(LDO8, "in8", MAX8907_REG_LDOCTL8),
+	LDO_750_50(LDO9, "in9", MAX8907_REG_LDOCTL9),
+	LDO_750_50(LDO10, "in10", MAX8907_REG_LDOCTL10),
+	LDO_750_50(LDO11, "in11", MAX8907_REG_LDOCTL11),
+	LDO_750_50(LDO12, "in12", MAX8907_REG_LDOCTL12),
+	LDO_750_50(LDO13, "in13", MAX8907_REG_LDOCTL13),
+	LDO_750_50(LDO14, "in14", MAX8907_REG_LDOCTL14),
+	LDO_750_50(LDO15, "in15", MAX8907_REG_LDOCTL15),
+	LDO_750_50(LDO16, "in16", MAX8907_REG_LDOCTL16),
+	LDO_650_25(LDO17, "in17", MAX8907_REG_LDOCTL17),
+	LDO_650_25(LDO18, "in18", MAX8907_REG_LDOCTL18),
+	LDO_750_50(LDO19, "in19", MAX8907_REG_LDOCTL19),
+	LDO_750_50(LDO20, "in20", MAX8907_REG_LDOCTL20),
+	REG_OUT5V(OUT5V, "mbatt", MAX8907_REG_OUT5VEN, 5000000),
+	REG_OUT5V(OUT33V, "mbatt",  MAX8907_REG_OUT33VEN, 3300000),
+	REG_BBAT(BBAT, "MBATT", MAX8907_REG_BBAT_CNFG,
+						2400000, 3000000, 200000),
+	REG_FIXED(SDBY, "MBATT", 1200000),
+	REG_FIXED(VRTC, "MBATT", 3300000),
+};
+
+#ifdef CONFIG_OF
+
+#define MATCH(_name, _id) \
+	[MAX8907_##_id] = { \
+		.name = #_name, \
+		.driver_data = (void *)&max8907_regulators[MAX8907_##_id], \
+	}
+
+static struct of_regulator_match max8907_matches[] = {
+	MATCH(mbatt, MBATT),
+	MATCH(sd1, SD1),
+	MATCH(sd2, SD2),
+	MATCH(sd3, SD3),
+	MATCH(ldo1, LDO1),
+	MATCH(ldo2, LDO2),
+	MATCH(ldo3, LDO3),
+	MATCH(ldo4, LDO4),
+	MATCH(ldo5, LDO5),
+	MATCH(ldo6, LDO6),
+	MATCH(ldo7, LDO7),
+	MATCH(ldo8, LDO8),
+	MATCH(ldo9, LDO9),
+	MATCH(ldo10, LDO10),
+	MATCH(ldo11, LDO11),
+	MATCH(ldo12, LDO12),
+	MATCH(ldo13, LDO13),
+	MATCH(ldo14, LDO14),
+	MATCH(ldo15, LDO15),
+	MATCH(ldo16, LDO16),
+	MATCH(ldo17, LDO17),
+	MATCH(ldo18, LDO18),
+	MATCH(ldo19, LDO19),
+	MATCH(ldo20, LDO20),
+	MATCH(out5v, OUT5V),
+	MATCH(out33v, OUT33V),
+	MATCH(bbat, BBAT),
+	MATCH(sdby, SDBY),
+	MATCH(vrtc, VRTC),
+};
+
+static int max8907_regulator_parse_dt(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.parent->of_node;
+	struct device_node *regulators;
+	int ret;
+
+	if (!pdev->dev.parent->of_node)
+		return 0;
+
+	regulators = of_find_node_by_name(np, "regulators");
+	if (!regulators) {
+		dev_err(&pdev->dev, "regulators node not found\n");
+		return -EINVAL;
+	}
+
+	ret = of_regulator_match(pdev->dev.parent, regulators,
+				 max8907_matches,
+				 ARRAY_SIZE(max8907_matches));
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error parsing regulator init data: %d\n",
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return max8907_matches[index].init_data;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return max8907_matches[index].of_node;
+}
+#else
+static int max8907_regulator_parse_dt(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return NULL;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return NULL;
+}
+#endif
+
+static __devinit int max8907_regulator_probe(struct platform_device *pdev)
+{
+	struct max8907 *max8907 = dev_get_drvdata(pdev->dev.parent);
+	struct max8907_platform_data *pdata = dev_get_platdata(max8907->dev);
+	int ret;
+	struct max8907_regulator *pmic;
+	unsigned int val;
+	int i;
+	struct regulator_config config = {};
+	struct regulator_init_data *idata;
+	const char *mbatt_rail_name = NULL;
+
+	ret = max8907_regulator_parse_dt(pdev);
+	if (ret)
+		return ret;
+
+	pmic = devm_kzalloc(&pdev->dev, sizeof(*pmic), GFP_KERNEL);
+	if (!pmic) {
+		dev_err(&pdev->dev, "Failed to alloc pmic\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, pmic);
+
+	memcpy(pmic->desc, max8907_regulators, sizeof(pmic->desc));
+
+	/* Backwards compatibility with MAX8907B; SD1 uses different voltages */
+	regmap_read(max8907->regmap_gen, MAX8907_REG_II2RR, &val);
+	if ((val & MAX8907_II2RR_VERSION_MASK) ==
+	    MAX8907_II2RR_VERSION_REV_B) {
+		pmic->desc[MAX8907_SD1].min_uV = 637500;
+		pmic->desc[MAX8907_SD1].uV_step = 12500;
+		pmic->desc[MAX8907_SD1].n_voltages =
+						(1425000 - 637500) / 12500 + 1;
+	}
+
+	for (i = 0; i < MAX8907_NUM_REGULATORS; i++) {
+		config.dev = pdev->dev.parent;
+		if (pdata)
+			idata = pdata->init_data[i];
+		else
+			idata = match_init_data(i);
+		config.init_data = idata;
+		config.driver_data = pmic;
+		config.regmap = max8907->regmap_gen;
+		config.of_node = match_of_node(i);
+
+		switch (pmic->desc[i].id) {
+		case MAX8907_MBATT:
+			if (idata && idata->constraints.name)
+				mbatt_rail_name = idata->constraints.name;
+			else
+				mbatt_rail_name = pmic->desc[i].name;
+			break;
+		case MAX8907_BBAT:
+		case MAX8907_SDBY:
+		case MAX8907_VRTC:
+			idata->supply_regulator = mbatt_rail_name;
+			break;
+		}
+
+		if (pmic->desc[i].ops == &max8907_ldo_ops) {
+			regmap_read(config.regmap, pmic->desc[i].enable_reg,
+				    &val);
+			if ((val & MAX8907_MASK_LDO_SEQ) !=
+			    MAX8907_MASK_LDO_SEQ)
+				pmic->desc[i].ops = &max8907_ldo_hwctl_ops;
+		} else if (pmic->desc[i].ops == &max8907_out5v_ops) {
+			regmap_read(config.regmap, pmic->desc[i].enable_reg,
+				    &val);
+			if ((val & (MAX8907_MASK_OUT5V_VINEN |
+						MAX8907_MASK_OUT5V_ENSRC)) !=
+			    MAX8907_MASK_OUT5V_ENSRC)
+				pmic->desc[i].ops = &max8907_out5v_hwctl_ops;
+		}
+
+		pmic->rdev[i] = regulator_register(&pmic->desc[i], &config);
+		if (IS_ERR(pmic->rdev[i])) {
+			dev_err(&pdev->dev,
+				"failed to register %s regulator\n",
+				pmic->desc[i].name);
+			ret = PTR_ERR(pmic->rdev[i]);
+			goto err_unregister_regulator;
+		}
+	}
+
+	return 0;
+
+err_unregister_regulator:
+	while (--i >= 0)
+		regulator_unregister(pmic->rdev[i]);
+	return ret;
+}
+
+static __devexit int max8907_regulator_remove(struct platform_device *pdev)
+{
+	struct max8907_regulator *pmic = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < MAX8907_NUM_REGULATORS; i++)
+		regulator_unregister(pmic->rdev[i]);
+
+	return 0;
+}
+
+static struct platform_driver max8907_regulator_driver = {
+	.driver = {
+		   .name = "max8907-regulator",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = max8907_regulator_probe,
+	.remove = __devexit_p(max8907_regulator_remove),
+};
+
+static int __init max8907_regulator_init(void)
+{
+	return platform_driver_register(&max8907_regulator_driver);
+}
+
+subsys_initcall(max8907_regulator_init);
+
+static void __exit max8907_reg_exit(void)
+{
+	platform_driver_unregister(&max8907_regulator_driver);
+}
+
+module_exit(max8907_reg_exit);
+
+MODULE_DESCRIPTION("MAX8907 regulator driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@maxim-ic.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:max8907-regulator");
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 4932e3449fe1..0801a6d0c122 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -21,6 +21,30 @@
 #include <linux/module.h>
 #include "mc13xxx.h"
 
+#define MC13783_REG_SWITCHERS0			24
+/* Enable does not exist for SW1A */
+#define MC13783_REG_SWITCHERS0_SW1AEN			0
+#define MC13783_REG_SWITCHERS0_SW1AVSEL			0
+#define MC13783_REG_SWITCHERS0_SW1AVSEL_M		(63 << 0)
+
+#define MC13783_REG_SWITCHERS1			25
+/* Enable does not exist for SW1B */
+#define MC13783_REG_SWITCHERS1_SW1BEN			0
+#define MC13783_REG_SWITCHERS1_SW1BVSEL			0
+#define MC13783_REG_SWITCHERS1_SW1BVSEL_M		(63 << 0)
+
+#define MC13783_REG_SWITCHERS2			26
+/* Enable does not exist for SW2A */
+#define MC13783_REG_SWITCHERS2_SW2AEN			0
+#define MC13783_REG_SWITCHERS2_SW2AVSEL			0
+#define MC13783_REG_SWITCHERS2_SW2AVSEL_M		(63 << 0)
+
+#define MC13783_REG_SWITCHERS3			27
+/* Enable does not exist for SW2B */
+#define MC13783_REG_SWITCHERS3_SW2BEN			0
+#define MC13783_REG_SWITCHERS3_SW2BVSEL			0
+#define MC13783_REG_SWITCHERS3_SW2BVSEL_M		(63 << 0)
+
 #define MC13783_REG_SWITCHERS5			29
 #define MC13783_REG_SWITCHERS5_SW3EN			(1 << 20)
 #define MC13783_REG_SWITCHERS5_SW3VSEL			18
@@ -93,6 +117,44 @@
 
 
 /* Voltage Values */
+static const int mc13783_sw1x_val[] = {
+	900000, 925000, 950000, 975000,
+	1000000, 1025000, 1050000, 1075000,
+	1100000, 1125000, 1150000, 1175000,
+	1200000, 1225000, 1250000, 1275000,
+	1300000, 1325000, 1350000, 1375000,
+	1400000, 1425000, 1450000, 1475000,
+	1500000, 1525000, 1550000, 1575000,
+	1600000, 1625000, 1650000, 1675000,
+	1700000, 1700000, 1700000, 1700000,
+	1800000, 1800000, 1800000, 1800000,
+	1850000, 1850000, 1850000, 1850000,
+	2000000, 2000000, 2000000, 2000000,
+	2100000, 2100000, 2100000, 2100000,
+	2200000, 2200000, 2200000, 2200000,
+	2200000, 2200000, 2200000, 2200000,
+	2200000, 2200000, 2200000, 2200000,
+};
+
+static const int mc13783_sw2x_val[] = {
+	900000, 925000, 950000, 975000,
+	1000000, 1025000, 1050000, 1075000,
+	1100000, 1125000, 1150000, 1175000,
+	1200000, 1225000, 1250000, 1275000,
+	1300000, 1325000, 1350000, 1375000,
+	1400000, 1425000, 1450000, 1475000,
+	1500000, 1525000, 1550000, 1575000,
+	1600000, 1625000, 1650000, 1675000,
+	1700000, 1700000, 1700000, 1700000,
+	1800000, 1800000, 1800000, 1800000,
+	1900000, 1900000, 1900000, 1900000,
+	2000000, 2000000, 2000000, 2000000,
+	2100000, 2100000, 2100000, 2100000,
+	2200000, 2200000, 2200000, 2200000,
+	2200000, 2200000, 2200000, 2200000,
+	2200000, 2200000, 2200000, 2200000,
+};
+
 static const unsigned int mc13783_sw3_val[] = {
 	5000000, 5000000, 5000000, 5500000,
 };
@@ -188,6 +250,10 @@ static struct regulator_ops mc13783_gpo_regulator_ops;
 	MC13783_DEFINE(REG, _name, _reg, _vsel_reg, _voltages)
 
 static struct mc13xxx_regulator mc13783_regulators[] = {
+	MC13783_DEFINE_SW(SW1A, SWITCHERS0, SWITCHERS0, mc13783_sw1x_val),
+	MC13783_DEFINE_SW(SW1B, SWITCHERS1, SWITCHERS1, mc13783_sw1x_val),
+	MC13783_DEFINE_SW(SW2A, SWITCHERS2, SWITCHERS2, mc13783_sw2x_val),
+	MC13783_DEFINE_SW(SW2B, SWITCHERS3, SWITCHERS3, mc13783_sw2x_val),
 	MC13783_DEFINE_SW(SW3, SWITCHERS5, SWITCHERS5, mc13783_sw3_val),
 
 	MC13783_FIXED_DEFINE(REG, VAUDIO, REGULATORMODE0, mc13783_vaudio_val),
@@ -238,9 +304,10 @@ static int mc13783_powermisc_rmw(struct mc13xxx_regulator_priv *priv, u32 mask,
 
 	BUG_ON(val & ~mask);
 
+	mc13xxx_lock(priv->mc13xxx);
 	ret = mc13xxx_reg_read(mc13783, MC13783_REG_POWERMISC, &valread);
 	if (ret)
-		return ret;
+		goto out;
 
 	/* Update the stored state for Power Gates. */
 	priv->powermisc_pwgt_state =
@@ -253,7 +320,10 @@ static int mc13783_powermisc_rmw(struct mc13xxx_regulator_priv *priv, u32 mask,
 	valread = (valread & ~MC13783_REG_POWERMISC_PWGTSPI_M) |
 						priv->powermisc_pwgt_state;
 
-	return mc13xxx_reg_write(mc13783, MC13783_REG_POWERMISC, valread);
+	ret = mc13xxx_reg_write(mc13783, MC13783_REG_POWERMISC, valread);
+out:
+	mc13xxx_unlock(priv->mc13xxx);
+	return ret;
 }
 
 static int mc13783_gpo_regulator_enable(struct regulator_dev *rdev)
@@ -261,7 +331,6 @@ static int mc13783_gpo_regulator_enable(struct regulator_dev *rdev)
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
 	int id = rdev_get_id(rdev);
-	int ret;
 	u32 en_val = mc13xxx_regulators[id].enable_bit;
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
@@ -271,12 +340,8 @@ static int mc13783_gpo_regulator_enable(struct regulator_dev *rdev)
 	    id == MC13783_REG_PWGT2SPI)
 		en_val = 0;
 
-	mc13xxx_lock(priv->mc13xxx);
-	ret = mc13783_powermisc_rmw(priv, mc13xxx_regulators[id].enable_bit,
+	return mc13783_powermisc_rmw(priv, mc13xxx_regulators[id].enable_bit,
 					en_val);
-	mc13xxx_unlock(priv->mc13xxx);
-
-	return ret;
 }
 
 static int mc13783_gpo_regulator_disable(struct regulator_dev *rdev)
@@ -284,7 +349,6 @@ static int mc13783_gpo_regulator_disable(struct regulator_dev *rdev)
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
 	int id = rdev_get_id(rdev);
-	int ret;
 	u32 dis_val = 0;
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
@@ -294,12 +358,8 @@ static int mc13783_gpo_regulator_disable(struct regulator_dev *rdev)
 	    id == MC13783_REG_PWGT2SPI)
 		dis_val = mc13xxx_regulators[id].enable_bit;
 
-	mc13xxx_lock(priv->mc13xxx);
-	ret = mc13783_powermisc_rmw(priv, mc13xxx_regulators[id].enable_bit,
+	return mc13783_powermisc_rmw(priv, mc13xxx_regulators[id].enable_bit,
 					dis_val);
-	mc13xxx_unlock(priv->mc13xxx);
-
-	return ret;
 }
 
 static int mc13783_gpo_regulator_is_enabled(struct regulator_dev *rdev)
@@ -330,7 +390,6 @@ static struct regulator_ops mc13783_gpo_regulator_ops = {
 	.is_enabled = mc13783_gpo_regulator_is_enabled,
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
-	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
 
 static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index b388b746452e..1fa63812f7ac 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -305,9 +305,10 @@ static int mc13892_powermisc_rmw(struct mc13xxx_regulator_priv *priv, u32 mask,
 
 	BUG_ON(val & ~mask);
 
+	mc13xxx_lock(priv->mc13xxx);
 	ret = mc13xxx_reg_read(mc13892, MC13892_POWERMISC, &valread);
 	if (ret)
-		return ret;
+		goto out;
 
 	/* Update the stored state for Power Gates. */
 	priv->powermisc_pwgt_state =
@@ -320,14 +321,16 @@ static int mc13892_powermisc_rmw(struct mc13xxx_regulator_priv *priv, u32 mask,
 	valread = (valread & ~MC13892_POWERMISC_PWGTSPI_M) |
 		priv->powermisc_pwgt_state;
 
-	return mc13xxx_reg_write(mc13892, MC13892_POWERMISC, valread);
+	ret = mc13xxx_reg_write(mc13892, MC13892_POWERMISC, valread);
+out:
+	mc13xxx_unlock(priv->mc13xxx);
+	return ret;
 }
 
 static int mc13892_gpo_regulator_enable(struct regulator_dev *rdev)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	int id = rdev_get_id(rdev);
-	int ret;
 	u32 en_val = mc13892_regulators[id].enable_bit;
 	u32 mask = mc13892_regulators[id].enable_bit;
 
@@ -340,18 +343,13 @@ static int mc13892_gpo_regulator_enable(struct regulator_dev *rdev)
 	if (id == MC13892_GPO4)
 		mask |= MC13892_POWERMISC_GPO4ADINEN;
 
-	mc13xxx_lock(priv->mc13xxx);
-	ret = mc13892_powermisc_rmw(priv, mask, en_val);
-	mc13xxx_unlock(priv->mc13xxx);
-
-	return ret;
+	return mc13892_powermisc_rmw(priv, mask, en_val);
 }
 
 static int mc13892_gpo_regulator_disable(struct regulator_dev *rdev)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	int id = rdev_get_id(rdev);
-	int ret;
 	u32 dis_val = 0;
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
@@ -360,12 +358,8 @@ static int mc13892_gpo_regulator_disable(struct regulator_dev *rdev)
 	if (id == MC13892_PWGT1SPI || id == MC13892_PWGT2SPI)
 		dis_val = mc13892_regulators[id].enable_bit;
 
-	mc13xxx_lock(priv->mc13xxx);
-	ret = mc13892_powermisc_rmw(priv, mc13892_regulators[id].enable_bit,
+	return mc13892_powermisc_rmw(priv, mc13892_regulators[id].enable_bit,
 		dis_val);
-	mc13xxx_unlock(priv->mc13xxx);
-
-	return ret;
 }
 
 static int mc13892_gpo_regulator_is_enabled(struct regulator_dev *rdev)
@@ -396,14 +390,13 @@ static struct regulator_ops mc13892_gpo_regulator_ops = {
 	.is_enabled = mc13892_gpo_regulator_is_enabled,
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
-	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
 
-static int mc13892_sw_regulator_get_voltage(struct regulator_dev *rdev)
+static int mc13892_sw_regulator_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	int ret, id = rdev_get_id(rdev);
-	unsigned int val, hi;
+	unsigned int val;
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
 
@@ -414,17 +407,11 @@ static int mc13892_sw_regulator_get_voltage(struct regulator_dev *rdev)
 	if (ret)
 		return ret;
 
-	hi  = val & MC13892_SWITCHERS0_SWxHI;
 	val = (val & mc13892_regulators[id].vsel_mask)
 		>> mc13892_regulators[id].vsel_shift;
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d val: %d\n", __func__, id, val);
 
-	if (hi)
-		val = (25000 * val) + 1100000;
-	else
-		val = (25000 * val) + 600000;
-
 	return val;
 }
 
@@ -432,37 +419,25 @@ static int mc13892_sw_regulator_set_voltage_sel(struct regulator_dev *rdev,
 						unsigned selector)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
-	int hi, value, mask, id = rdev_get_id(rdev);
-	u32 valread;
+	int volt, mask, id = rdev_get_id(rdev);
+	u32 reg_value;
 	int ret;
 
-	value = rdev->desc->volt_table[selector];
+	volt = rdev->desc->volt_table[selector];
+	mask = mc13892_regulators[id].vsel_mask;
+	reg_value = selector << mc13892_regulators[id].vsel_shift;
+
+	if (volt > 1375000) {
+		mask |= MC13892_SWITCHERS0_SWxHI;
+		reg_value |= MC13892_SWITCHERS0_SWxHI;
+	} else if (volt < 1100000) {
+		mask |= MC13892_SWITCHERS0_SWxHI;
+		reg_value &= ~MC13892_SWITCHERS0_SWxHI;
+	}
 
 	mc13xxx_lock(priv->mc13xxx);
-	ret = mc13xxx_reg_read(priv->mc13xxx,
-		mc13892_regulators[id].vsel_reg, &valread);
-	if (ret)
-		goto err;
-
-	if (value > 1375000)
-		hi = 1;
-	else if (value < 1100000)
-		hi = 0;
-	else
-		hi = valread & MC13892_SWITCHERS0_SWxHI;
-
-	if (hi) {
-		value = (value - 1100000) / 25000;
-		value |= MC13892_SWITCHERS0_SWxHI;
-	} else
-		value = (value - 600000) / 25000;
-
-	mask = mc13892_regulators[id].vsel_mask | MC13892_SWITCHERS0_SWxHI;
-	valread = (valread & ~mask) |
-			(value << mc13892_regulators[id].vsel_shift);
-	ret = mc13xxx_reg_write(priv->mc13xxx, mc13892_regulators[id].vsel_reg,
-			valread);
-err:
+	ret = mc13xxx_reg_rmw(priv->mc13xxx, mc13892_regulators[id].reg, mask,
+			      reg_value);
 	mc13xxx_unlock(priv->mc13xxx);
 
 	return ret;
@@ -471,7 +446,7 @@ err:
 static struct regulator_ops mc13892_sw_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = mc13892_sw_regulator_set_voltage_sel,
-	.get_voltage = mc13892_sw_regulator_get_voltage,
+	.get_voltage_sel = mc13892_sw_regulator_get_voltage_sel,
 };
 
 static int mc13892_vcam_set_mode(struct regulator_dev *rdev, unsigned int mode)
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index d6eda28ca5d0..88cbb832d555 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -143,30 +143,21 @@ int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev, int min_uV,
 		__func__, id, min_uV, max_uV);
 
 	if (min_uV <= rdev->desc->volt_table[0] &&
-	    rdev->desc->volt_table[0] <= max_uV)
+	    rdev->desc->volt_table[0] <= max_uV) {
+		*selector = 0;
 		return 0;
-	else
+	} else {
 		return -EINVAL;
+	}
 }
 EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_set_voltage);
 
-int mc13xxx_fixed_regulator_get_voltage(struct regulator_dev *rdev)
-{
-	int id = rdev_get_id(rdev);
-
-	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
-
-	return rdev->desc->volt_table[0];
-}
-EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_get_voltage);
-
 struct regulator_ops mc13xxx_fixed_regulator_ops = {
 	.enable = mc13xxx_regulator_enable,
 	.disable = mc13xxx_regulator_disable,
 	.is_enabled = mc13xxx_regulator_is_enabled,
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
-	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
 EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_ops);
 
diff --git a/drivers/regulator/mc13xxx.h b/drivers/regulator/mc13xxx.h
index eaff5510b6df..06c8903f182a 100644
--- a/drivers/regulator/mc13xxx.h
+++ b/drivers/regulator/mc13xxx.h
@@ -34,7 +34,6 @@ struct mc13xxx_regulator_priv {
 
 extern int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev,
 		int min_uV, int max_uV, unsigned *selector);
-extern int mc13xxx_fixed_regulator_get_voltage(struct regulator_dev *rdev);
 
 #ifdef CONFIG_OF
 extern int mc13xxx_get_num_regulators_dt(struct platform_device *pdev);
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index 3e4106f2bda9..6f684916fd79 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -92,16 +92,18 @@ struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
 EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
 
 /**
- * of_regulator_match - extract regulator init data when node
- * property "regulator-compatible" matches with the regulator name.
+ * of_regulator_match - extract multiple regulator init data from device tree.
  * @dev: device requesting the data
  * @node: parent device node of the regulators
  * @matches: match table for the regulators
  * @num_matches: number of entries in match table
  *
- * This function uses a match table specified by the regulator driver and
- * looks up the corresponding init data in the device tree  if
- * regulator-compatible matches. Note that the match table is modified
+ * This function uses a match table specified by the regulator driver to
+ * parse regulator init data from the device tree. @node is expected to
+ * contain a set of child nodes, each providing the init data for one
+ * regulator. The data parsed from a child node will be matched to a regulator
+ * based on either the deprecated property regulator-compatible if present,
+ * or otherwise the child node's name. Note that the match table is modified
  * in place.
  *
  * Returns the number of matches found or a negative error code on failure.
@@ -112,26 +114,23 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 {
 	unsigned int count = 0;
 	unsigned int i;
-	const char *regulator_comp;
+	const char *name;
 	struct device_node *child;
 
 	if (!dev || !node)
 		return -EINVAL;
 
 	for_each_child_of_node(node, child) {
-		regulator_comp = of_get_property(child,
+		name = of_get_property(child,
 					"regulator-compatible", NULL);
-		if (!regulator_comp) {
-			dev_err(dev, "regulator-compatible is missing for node %s\n",
-				child->name);
-			continue;
-		}
+		if (!name)
+			name = child->name;
 		for (i = 0; i < num_matches; i++) {
 			struct of_regulator_match *match = &matches[i];
 			if (match->of_node)
 				continue;
 
-			if (strcmp(match->name, regulator_comp))
+			if (strcmp(match->name, name))
 				continue;
 
 			match->init_data =
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 46c7e88f8381..2ba7502fa3b2 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -443,44 +443,6 @@ static int palmas_list_voltage_ldo(struct regulator_dev *dev,
 	return  850000 + (selector * 50000);
 }
 
-static int palmas_get_voltage_ldo_sel(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	int selector;
-	unsigned int reg;
-	unsigned int addr;
-
-	addr = palmas_regs_info[id].vsel_addr;
-
-	palmas_ldo_read(pmic->palmas, addr, &reg);
-
-	selector = reg & PALMAS_LDO1_VOLTAGE_VSEL_MASK;
-
-	/* Adjust selector to match list_voltage ranges */
-	if (selector > 49)
-		selector = 49;
-
-	return selector;
-}
-
-static int palmas_set_voltage_ldo_sel(struct regulator_dev *dev,
-		unsigned selector)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	unsigned int reg = 0;
-	unsigned int addr;
-
-	addr = palmas_regs_info[id].vsel_addr;
-
-	reg = selector;
-
-	palmas_ldo_write(pmic->palmas, addr, reg);
-
-	return 0;
-}
-
 static int palmas_map_voltage_ldo(struct regulator_dev *rdev,
 		int min_uV, int max_uV)
 {
@@ -505,8 +467,8 @@ static struct regulator_ops palmas_ops_ldo = {
 	.is_enabled		= palmas_is_enabled_ldo,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
-	.get_voltage_sel	= palmas_get_voltage_ldo_sel,
-	.set_voltage_sel	= palmas_set_voltage_ldo_sel,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
 	.list_voltage		= palmas_list_voltage_ldo,
 	.map_voltage		= palmas_map_voltage_ldo,
 };
@@ -757,6 +719,9 @@ static __devinit int palmas_probe(struct platform_device *pdev)
 
 		pmic->desc[id].type = REGULATOR_VOLTAGE;
 		pmic->desc[id].owner = THIS_MODULE;
+		pmic->desc[id].vsel_reg = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
+						palmas_regs_info[id].vsel_addr);
+		pmic->desc[id].vsel_mask = PALMAS_LDO1_VOLTAGE_VSEL_MASK;
 		pmic->desc[id].enable_reg = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE,
 						palmas_regs_info[id].ctrl_addr);
 		pmic->desc[id].enable_mask = PALMAS_LDO1_CTRL_MODE_ACTIVE;
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index 4669dc9ac74a..926f9c8f2fac 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -24,7 +24,7 @@
 #include <linux/mfd/samsung/s2mps11.h>
 
 struct s2mps11_info {
-	struct regulator_dev **rdev;
+	struct regulator_dev *rdev[S2MPS11_REGULATOR_MAX];
 
 	int ramp_delay2;
 	int ramp_delay34;
@@ -236,9 +236,8 @@ static __devinit int s2mps11_pmic_probe(struct platform_device *pdev)
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct sec_platform_data *pdata = dev_get_platdata(iodev->dev);
 	struct regulator_config config = { };
-	struct regulator_dev **rdev;
 	struct s2mps11_info *s2mps11;
-	int i, ret, size;
+	int i, ret;
 	unsigned char ramp_enable, ramp_reg = 0;
 
 	if (!pdata) {
@@ -251,13 +250,6 @@ static __devinit int s2mps11_pmic_probe(struct platform_device *pdev)
 	if (!s2mps11)
 		return -ENOMEM;
 
-	size = sizeof(struct regulator_dev *) * S2MPS11_REGULATOR_MAX;
-	s2mps11->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
-	if (!s2mps11->rdev) {
-		return -ENOMEM;
-	}
-
-	rdev = s2mps11->rdev;
 	platform_set_drvdata(pdev, s2mps11);
 
 	s2mps11->ramp_delay2 = pdata->buck2_ramp_delay;
@@ -297,12 +289,12 @@ static __devinit int s2mps11_pmic_probe(struct platform_device *pdev)
 		config.init_data = pdata->regulators[i].initdata;
 		config.driver_data = s2mps11;
 
-		rdev[i] = regulator_register(&regulators[i], &config);
-		if (IS_ERR(rdev[i])) {
-			ret = PTR_ERR(rdev[i]);
+		s2mps11->rdev[i] = regulator_register(&regulators[i], &config);
+		if (IS_ERR(s2mps11->rdev[i])) {
+			ret = PTR_ERR(s2mps11->rdev[i]);
 			dev_err(&pdev->dev, "regulator init failed for %d\n",
 				i);
-			rdev[i] = NULL;
+			s2mps11->rdev[i] = NULL;
 			goto err;
 		}
 	}
@@ -310,8 +302,7 @@ static __devinit int s2mps11_pmic_probe(struct platform_device *pdev)
 	return 0;
 err:
 	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
+		regulator_unregister(s2mps11->rdev[i]);
 
 	return ret;
 }
@@ -319,12 +310,10 @@ err:
 static int __devexit s2mps11_pmic_remove(struct platform_device *pdev)
 {
 	struct s2mps11_info *s2mps11 = platform_get_drvdata(pdev);
-	struct regulator_dev **rdev = s2mps11->rdev;
 	int i;
 
 	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
+		regulator_unregister(s2mps11->rdev[i]);
 
 	return 0;
 }
diff --git a/drivers/regulator/tps6524x-regulator.c b/drivers/regulator/tps6524x-regulator.c
index 947ece933d90..058d2f2675e9 100644
--- a/drivers/regulator/tps6524x-regulator.c
+++ b/drivers/regulator/tps6524x-regulator.c
@@ -502,15 +502,13 @@ static int set_current_limit(struct regulator_dev *rdev, int min_uA,
 	if (info->n_ilimsels == 1)
 		return -EINVAL;
 
-	for (i = 0; i < info->n_ilimsels; i++)
+	for (i = info->n_ilimsels - 1; i >= 0; i--) {
 		if (min_uA <= info->ilimsels[i] &&
 		    max_uA >= info->ilimsels[i])
-			break;
-
-	if (i >= info->n_ilimsels)
-		return -EINVAL;
+			return write_field(hw, &info->ilimsel, i);
+	}
 
-	return write_field(hw, &info->ilimsel, i);
+	return -EINVAL;
 }
 
 static int get_current_limit(struct regulator_dev *rdev)
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index 19241fc30050..ce1e7cb8d513 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -57,9 +57,6 @@
 struct tps6586x_regulator {
 	struct regulator_desc desc;
 
-	int volt_reg;
-	int volt_shift;
-	int volt_nbits;
 	int enable_bit[2];
 	int enable_reg[2];
 
@@ -81,10 +78,10 @@ static int tps6586x_set_voltage_sel(struct regulator_dev *rdev,
 	int ret, val, rid = rdev_get_id(rdev);
 	uint8_t mask;
 
-	val = selector << ri->volt_shift;
-	mask = ((1 << ri->volt_nbits) - 1) << ri->volt_shift;
+	val = selector << (ffs(rdev->desc->vsel_mask) - 1);
+	mask = rdev->desc->vsel_mask;
 
-	ret = tps6586x_update(parent, ri->volt_reg, val, mask);
+	ret = tps6586x_update(parent, rdev->desc->vsel_reg, val, mask);
 	if (ret)
 		return ret;
 
@@ -100,66 +97,17 @@ static int tps6586x_set_voltage_sel(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int tps6586x_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-	uint8_t val, mask;
-	int ret;
-
-	ret = tps6586x_read(parent, ri->volt_reg, &val);
-	if (ret)
-		return ret;
-
-	mask = ((1 << ri->volt_nbits) - 1) << ri->volt_shift;
-	val = (val & mask) >> ri->volt_shift;
-
-	if (val >= ri->desc.n_voltages)
-		BUG();
-
-	return val;
-}
-
-static int tps6586x_regulator_enable(struct regulator_dev *rdev)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-
-	return tps6586x_set_bits(parent, ri->enable_reg[0],
-				 1 << ri->enable_bit[0]);
-}
-
-static int tps6586x_regulator_disable(struct regulator_dev *rdev)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-
-	return tps6586x_clr_bits(parent, ri->enable_reg[0],
-				 1 << ri->enable_bit[0]);
-}
-
-static int tps6586x_regulator_is_enabled(struct regulator_dev *rdev)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-	uint8_t reg_val;
-	int ret;
-
-	ret = tps6586x_read(parent, ri->enable_reg[0], &reg_val);
-	if (ret)
-		return ret;
-
-	return !!(reg_val & (1 << ri->enable_bit[0]));
-}
-
 static struct regulator_ops tps6586x_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
-	.get_voltage_sel = tps6586x_get_voltage_sel,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = tps6586x_set_voltage_sel,
 
-	.is_enabled = tps6586x_regulator_is_enabled,
-	.enable = tps6586x_regulator_enable,
-	.disable = tps6586x_regulator_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+};
+
+static struct regulator_ops tps6586x_sys_regulator_ops = {
 };
 
 static const unsigned int tps6586x_ldo0_voltages[] = {
@@ -202,10 +150,11 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 		.n_voltages = ARRAY_SIZE(tps6586x_##vdata##_voltages),	\
 		.volt_table = tps6586x_##vdata##_voltages,		\
 		.owner	= THIS_MODULE,					\
+		.enable_reg = TPS6586X_SUPPLY##ereg0,			\
+		.enable_mask = 1 << (ebit0),				\
+		.vsel_reg = TPS6586X_##vreg,				\
+		.vsel_mask = ((1 << (nbits)) - 1) << (shift),		\
 	},								\
-	.volt_reg	= TPS6586X_##vreg,				\
-	.volt_shift	= (shift),					\
-	.volt_nbits	= (nbits),					\
 	.enable_reg[0]	= TPS6586X_SUPPLY##ereg0,			\
 	.enable_bit[0]	= (ebit0),					\
 	.enable_reg[1]	= TPS6586X_SUPPLY##ereg1,			\
@@ -230,15 +179,28 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 	TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 }
 
+#define TPS6586X_SYS_REGULATOR()					\
+{									\
+	.desc	= {							\
+		.supply_name = "sys",					\
+		.name	= "REG-SYS",					\
+		.ops	= &tps6586x_sys_regulator_ops,			\
+		.type	= REGULATOR_VOLTAGE,				\
+		.id	= TPS6586X_ID_SYS,				\
+		.owner	= THIS_MODULE,					\
+	},								\
+}
+
 static struct tps6586x_regulator tps6586x_regulator[] = {
+	TPS6586X_SYS_REGULATOR(),
 	TPS6586X_LDO(LDO_0, "vinldo01", ldo0, SUPPLYV1, 5, 3, ENC, 0, END, 0),
 	TPS6586X_LDO(LDO_3, "vinldo23", ldo, SUPPLYV4, 0, 3, ENC, 2, END, 2),
-	TPS6586X_LDO(LDO_5, NULL, ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6),
+	TPS6586X_LDO(LDO_5, "REG-SYS", ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6),
 	TPS6586X_LDO(LDO_6, "vinldo678", ldo, SUPPLYV3, 0, 3, ENC, 4, END, 4),
 	TPS6586X_LDO(LDO_7, "vinldo678", ldo, SUPPLYV3, 3, 3, ENC, 5, END, 5),
 	TPS6586X_LDO(LDO_8, "vinldo678", ldo, SUPPLYV2, 5, 3, ENC, 6, END, 6),
 	TPS6586X_LDO(LDO_9, "vinldo9", ldo, SUPPLYV6, 3, 3, ENE, 7, ENE, 7),
-	TPS6586X_LDO(LDO_RTC, NULL, ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7),
+	TPS6586X_LDO(LDO_RTC, "REG-SYS", ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7),
 	TPS6586X_LDO(LDO_1, "vinldo01", dvm, SUPPLYV1, 0, 5, ENC, 1, END, 1),
 	TPS6586X_LDO(SM_2, "vin-sm2", sm2, SUPPLYV2, 0, 5, ENC, 7, END, 7),
 
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index 77a71a5c17c3..7eb986a40746 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -10,6 +10,8 @@
  */
 
 #include <linux/module.h>
+#include <linux/string.h>
+#include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
 #include <linux/platform_device.h>
@@ -624,18 +626,9 @@ static int twlfixed_list_voltage(struct regulator_dev *rdev, unsigned index)
 	return info->min_mV * 1000;
 }
 
-static int twlfixed_get_voltage(struct regulator_dev *rdev)
-{
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-
-	return info->min_mV * 1000;
-}
-
 static struct regulator_ops twl4030fixed_ops = {
 	.list_voltage	= twlfixed_list_voltage,
 
-	.get_voltage	= twlfixed_get_voltage,
-
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
 	.is_enabled	= twl4030reg_is_enabled,
@@ -648,8 +641,6 @@ static struct regulator_ops twl4030fixed_ops = {
 static struct regulator_ops twl6030fixed_ops = {
 	.list_voltage	= twlfixed_list_voltage,
 
-	.get_voltage	= twlfixed_get_voltage,
-
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
@@ -659,13 +650,6 @@ static struct regulator_ops twl6030fixed_ops = {
 	.get_status	= twl6030reg_get_status,
 };
 
-static struct regulator_ops twl6030_fixed_resource = {
-	.enable		= twl6030reg_enable,
-	.disable	= twl6030reg_disable,
-	.is_enabled	= twl6030reg_is_enabled,
-	.get_status	= twl6030reg_get_status,
-};
-
 /*
  * SMPS status and control
  */
@@ -757,37 +741,32 @@ static int twl6030smps_list_voltage(struct regulator_dev *rdev, unsigned index)
 	return voltage;
 }
 
-static int
-twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
-			unsigned int *selector)
+static int twl6030smps_map_voltage(struct regulator_dev *rdev, int min_uV,
+				   int max_uV)
 {
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-	int	vsel = 0;
+	struct twlreg_info *info = rdev_get_drvdata(rdev);
+	int vsel = 0;
 
 	switch (info->flags) {
 	case 0:
 		if (min_uV == 0)
 			vsel = 0;
 		else if ((min_uV >= 600000) && (min_uV <= 1300000)) {
-			int calc_uV;
 			vsel = DIV_ROUND_UP(min_uV - 600000, 12500);
 			vsel++;
-			calc_uV = twl6030smps_list_voltage(rdev, vsel);
-			if (calc_uV > max_uV)
-				return -EINVAL;
 		}
 		/* Values 1..57 for vsel are linear and can be calculated
 		 * values 58..62 are non linear.
 		 */
-		else if ((min_uV > 1900000) && (max_uV >= 2100000))
+		else if ((min_uV > 1900000) && (min_uV <= 2100000))
 			vsel = 62;
-		else if ((min_uV > 1800000) && (max_uV >= 1900000))
+		else if ((min_uV > 1800000) && (min_uV <= 1900000))
 			vsel = 61;
-		else if ((min_uV > 1500000) && (max_uV >= 1800000))
+		else if ((min_uV > 1500000) && (min_uV <= 1800000))
 			vsel = 60;
-		else if ((min_uV > 1350000) && (max_uV >= 1500000))
+		else if ((min_uV > 1350000) && (min_uV <= 1500000))
 			vsel = 59;
-		else if ((min_uV > 1300000) && (max_uV >= 1350000))
+		else if ((min_uV > 1300000) && (min_uV <= 1350000))
 			vsel = 58;
 		else
 			return -EINVAL;
@@ -796,25 +775,21 @@ twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 		if (min_uV == 0)
 			vsel = 0;
 		else if ((min_uV >= 700000) && (min_uV <= 1420000)) {
-			int calc_uV;
 			vsel = DIV_ROUND_UP(min_uV - 700000, 12500);
 			vsel++;
-			calc_uV = twl6030smps_list_voltage(rdev, vsel);
-			if (calc_uV > max_uV)
-				return -EINVAL;
 		}
 		/* Values 1..57 for vsel are linear and can be calculated
 		 * values 58..62 are non linear.
 		 */
-		else if ((min_uV > 1900000) && (max_uV >= 2100000))
+		else if ((min_uV > 1900000) && (min_uV <= 2100000))
 			vsel = 62;
-		else if ((min_uV > 1800000) && (max_uV >= 1900000))
+		else if ((min_uV > 1800000) && (min_uV <= 1900000))
 			vsel = 61;
-		else if ((min_uV > 1350000) && (max_uV >= 1800000))
+		else if ((min_uV > 1350000) && (min_uV <= 1800000))
 			vsel = 60;
-		else if ((min_uV > 1350000) && (max_uV >= 1500000))
+		else if ((min_uV > 1350000) && (min_uV <= 1500000))
 			vsel = 59;
-		else if ((min_uV > 1300000) && (max_uV >= 1350000))
+		else if ((min_uV > 1300000) && (min_uV <= 1350000))
 			vsel = 58;
 		else
 			return -EINVAL;
@@ -830,17 +805,23 @@ twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 	case SMPS_OFFSET_EN|SMPS_EXTENDED_EN:
 		if (min_uV == 0) {
 			vsel = 0;
-		} else if ((min_uV >= 2161000) && (max_uV <= 4321000)) {
+		} else if ((min_uV >= 2161000) && (min_uV <= 4321000)) {
 			vsel = DIV_ROUND_UP(min_uV - 2161000, 38600);
 			vsel++;
 		}
 		break;
 	}
 
-	*selector = vsel;
+	return vsel;
+}
+
+static int twl6030smps_set_voltage_sel(struct regulator_dev *rdev,
+				       unsigned int selector)
+{
+	struct twlreg_info *info = rdev_get_drvdata(rdev);
 
 	return twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE_SMPS,
-							vsel);
+			    selector);
 }
 
 static int twl6030smps_get_voltage_sel(struct regulator_dev *rdev)
@@ -852,8 +833,9 @@ static int twl6030smps_get_voltage_sel(struct regulator_dev *rdev)
 
 static struct regulator_ops twlsmps_ops = {
 	.list_voltage		= twl6030smps_list_voltage,
+	.map_voltage		= twl6030smps_map_voltage,
 
-	.set_voltage		= twl6030smps_set_voltage,
+	.set_voltage_sel	= twl6030smps_set_voltage_sel,
 	.get_voltage_sel	= twl6030smps_get_voltage_sel,
 
 	.enable			= twl6030reg_enable,
@@ -876,7 +858,7 @@ static struct regulator_ops twlsmps_ops = {
 			0x0, TWL6030, twl6030fixed_ops)
 
 #define TWL4030_ADJUSTABLE_LDO(label, offset, num, turnon_delay, remap_conf) \
-static struct twlreg_info TWL4030_INFO_##label = { \
+static const struct twlreg_info TWL4030_INFO_##label = { \
 	.base = offset, \
 	.id = num, \
 	.table_len = ARRAY_SIZE(label##_VSEL_table), \
@@ -894,7 +876,7 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 	}
 
 #define TWL4030_ADJUSTABLE_SMPS(label, offset, num, turnon_delay, remap_conf) \
-static struct twlreg_info TWL4030_INFO_##label = { \
+static const struct twlreg_info TWL4030_INFO_##label = { \
 	.base = offset, \
 	.id = num, \
 	.remap = remap_conf, \
@@ -909,7 +891,7 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 	}
 
 #define TWL6030_ADJUSTABLE_SMPS(label) \
-static struct twlreg_info TWL6030_INFO_##label = { \
+static const struct twlreg_info TWL6030_INFO_##label = { \
 	.desc = { \
 		.name = #label, \
 		.id = TWL6030_REG_##label, \
@@ -920,7 +902,7 @@ static struct twlreg_info TWL6030_INFO_##label = { \
 	}
 
 #define TWL6030_ADJUSTABLE_LDO(label, offset, min_mVolts, max_mVolts) \
-static struct twlreg_info TWL6030_INFO_##label = { \
+static const struct twlreg_info TWL6030_INFO_##label = { \
 	.base = offset, \
 	.min_mV = min_mVolts, \
 	.max_mV = max_mVolts, \
@@ -935,7 +917,7 @@ static struct twlreg_info TWL6030_INFO_##label = { \
 	}
 
 #define TWL6025_ADJUSTABLE_LDO(label, offset, min_mVolts, max_mVolts) \
-static struct twlreg_info TWL6025_INFO_##label = { \
+static const struct twlreg_info TWL6025_INFO_##label = { \
 	.base = offset, \
 	.min_mV = min_mVolts, \
 	.max_mV = max_mVolts, \
@@ -951,7 +933,7 @@ static struct twlreg_info TWL6025_INFO_##label = { \
 
 #define TWL_FIXED_LDO(label, offset, mVolts, num, turnon_delay, remap_conf, \
 		family, operations) \
-static struct twlreg_info TWLFIXED_INFO_##label = { \
+static const struct twlreg_info TWLFIXED_INFO_##label = { \
 	.base = offset, \
 	.id = num, \
 	.min_mV = mVolts, \
@@ -981,7 +963,7 @@ static struct twlreg_info TWLRES_INFO_##label = { \
 	}
 
 #define TWL6025_ADJUSTABLE_SMPS(label, offset) \
-static struct twlreg_info TWLSMPS_INFO_##label = { \
+static const struct twlreg_info TWLSMPS_INFO_##label = { \
 	.base = offset, \
 	.min_mV = 600, \
 	.max_mV = 2100, \
@@ -1138,6 +1120,7 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 {
 	int				i, id;
 	struct twlreg_info		*info;
+	const struct twlreg_info	*template;
 	struct regulator_init_data	*initdata;
 	struct regulation_constraints	*c;
 	struct regulator_dev		*rdev;
@@ -1147,17 +1130,17 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 
 	match = of_match_device(twl_of_match, &pdev->dev);
 	if (match) {
-		info = match->data;
-		id = info->desc.id;
+		template = match->data;
+		id = template->desc.id;
 		initdata = of_get_regulator_init_data(&pdev->dev,
 						      pdev->dev.of_node);
 		drvdata = NULL;
 	} else {
 		id = pdev->id;
 		initdata = pdev->dev.platform_data;
-		for (i = 0, info = NULL; i < ARRAY_SIZE(twl_of_match); i++) {
-			info = twl_of_match[i].data;
-			if (info && info->desc.id == id)
+		for (i = 0, template = NULL; i < ARRAY_SIZE(twl_of_match); i++) {
+			template = twl_of_match[i].data;
+			if (template && template->desc.id == id)
 				break;
 		}
 		if (i == ARRAY_SIZE(twl_of_match))
@@ -1168,12 +1151,16 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 			return -EINVAL;
 	}
 
-	if (!info)
+	if (!template)
 		return -ENODEV;
 
 	if (!initdata)
 		return -EINVAL;
 
+	info = kmemdup(template, sizeof (*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
 	if (drvdata) {
 		/* copy the driver data into regulator data */
 		info->features = drvdata->features;
@@ -1234,6 +1221,7 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "can't register %s, %ld\n",
 				info->desc.name, PTR_ERR(rdev));
+		kfree(info);
 		return PTR_ERR(rdev);
 	}
 	platform_set_drvdata(pdev, rdev);
@@ -1255,7 +1243,11 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 
 static int __devexit twlreg_remove(struct platform_device *pdev)
 {
-	regulator_unregister(platform_get_drvdata(pdev));
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+	struct twlreg_info *info = rdev->reg_data;
+
+	regulator_unregister(rdev);
+	kfree(info);
 	return 0;
 }
 
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 7413885be01b..90cbcc683704 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -339,16 +339,15 @@ static int wm831x_buckv_set_current_limit(struct regulator_dev *rdev,
 	u16 reg = dcdc->base + WM831X_DCDC_CONTROL_2;
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(wm831x_dcdc_ilim); i++) {
+	for (i = ARRAY_SIZE(wm831x_dcdc_ilim) - 1; i >= 0; i--) {
 		if ((min_uA <= wm831x_dcdc_ilim[i]) &&
 		    (wm831x_dcdc_ilim[i] <= max_uA))
-			break;
+			return wm831x_set_bits(wm831x, reg,
+					       WM831X_DC1_HC_THR_MASK,
+						i << WM831X_DC1_HC_THR_SHIFT);
 	}
-	if (i == ARRAY_SIZE(wm831x_dcdc_ilim))
-		return -EINVAL;
 
-	return wm831x_set_bits(wm831x, reg, WM831X_DC1_HC_THR_MASK,
-			       i << WM831X_DC1_HC_THR_SHIFT);
+	return -EINVAL;
 }
 
 static int wm831x_buckv_get_current_limit(struct regulator_dev *rdev)
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index 5cb70ca1e98d..9af512672be1 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -205,6 +205,8 @@ static int wm831x_gp_ldo_get_status(struct regulator_dev *rdev)
 
 	/* Is it reporting under voltage? */
 	ret = wm831x_reg_read(wm831x, WM831X_LDO_UV_STATUS);
+	if (ret < 0)
+		return ret;
 	if (ret & mask)
 		return REGULATOR_STATUS_ERROR;
 
@@ -237,6 +239,8 @@ static struct regulator_ops wm831x_gp_ldo_ops = {
 	.set_mode = wm831x_gp_ldo_set_mode,
 	.get_status = wm831x_gp_ldo_get_status,
 	.get_optimum_mode = wm831x_gp_ldo_get_optimum_mode,
+	.get_bypass = regulator_get_bypass_regmap,
+	.set_bypass = regulator_set_bypass_regmap,
 
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
@@ -293,6 +297,8 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 	ldo->desc.vsel_mask = WM831X_LDO1_ON_VSEL_MASK;
 	ldo->desc.enable_reg = WM831X_LDO_ENABLE;
 	ldo->desc.enable_mask = 1 << id;
+	ldo->desc.bypass_reg = ldo->base;
+	ldo->desc.bypass_mask = WM831X_LDO1_SWI;
 
 	config.dev = pdev->dev.parent;
 	if (pdata)
@@ -469,6 +475,8 @@ static int wm831x_aldo_get_status(struct regulator_dev *rdev)
 
 	/* Is it reporting under voltage? */
 	ret = wm831x_reg_read(wm831x, WM831X_LDO_UV_STATUS);
+	if (ret < 0)
+		return ret;
 	if (ret & mask)
 		return REGULATOR_STATUS_ERROR;
 
@@ -488,6 +496,8 @@ static struct regulator_ops wm831x_aldo_ops = {
 	.get_mode = wm831x_aldo_get_mode,
 	.set_mode = wm831x_aldo_set_mode,
 	.get_status = wm831x_aldo_get_status,
+	.set_bypass = regulator_set_bypass_regmap,
+	.get_bypass = regulator_get_bypass_regmap,
 
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
@@ -544,6 +554,8 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 	ldo->desc.vsel_mask = WM831X_LDO7_ON_VSEL_MASK;
 	ldo->desc.enable_reg = WM831X_LDO_ENABLE;
 	ldo->desc.enable_mask = 1 << id;
+	ldo->desc.bypass_reg = ldo->base;
+	ldo->desc.bypass_mask = WM831X_LDO7_SWI;
 
 	config.dev = pdev->dev.parent;
 	if (pdata)
diff --git a/drivers/regulator/wm8400-regulator.c b/drivers/regulator/wm8400-regulator.c
index 9035dd053611..27c746ef0636 100644
--- a/drivers/regulator/wm8400-regulator.c
+++ b/drivers/regulator/wm8400-regulator.c
@@ -120,13 +120,8 @@ static int wm8400_dcdc_set_mode(struct regulator_dev *dev, unsigned int mode)
 
 	case REGULATOR_MODE_IDLE:
 		/* Datasheet: standby */
-		ret = wm8400_set_bits(wm8400, WM8400_DCDC1_CONTROL_1 + offset,
-				      WM8400_DC1_ACTIVE, 0);
-		if (ret != 0)
-			return ret;
 		return wm8400_set_bits(wm8400, WM8400_DCDC1_CONTROL_1 + offset,
-				       WM8400_DC1_SLEEP, 0);
-
+				       WM8400_DC1_ACTIVE | WM8400_DC1_SLEEP, 0);
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 590cfafc7c17..1859f71372e2 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -1008,8 +1008,8 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	return 0;
 
 free_coherent:
-	dma_free_coherent(vdev->dev.parent, RPMSG_TOTAL_BUF_SPACE, bufs_va,
-					vrp->bufs_dma);
+	dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
+					bufs_va, vrp->bufs_dma);
 vqs_del:
 	vdev->config->del_vqs(vrp->vdev);
 free_vrp:
@@ -1043,7 +1043,7 @@ static void __devexit rpmsg_remove(struct virtio_device *vdev)
 
 	vdev->config->del_vqs(vrp->vdev);
 
-	dma_free_coherent(vdev->dev.parent, RPMSG_TOTAL_BUF_SPACE,
+	dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
 					vrp->rbufs, vrp->bufs_dma);
 
 	kfree(vrp);
diff --git a/drivers/rtc/rtc-twl.c b/drivers/rtc/rtc-twl.c
index c5d06fe83bba..9277d945bf48 100644
--- a/drivers/rtc/rtc-twl.c
+++ b/drivers/rtc/rtc-twl.c
@@ -495,6 +495,11 @@ static int __devinit twl_rtc_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto out1;
 
+	/* ensure interrupts are disabled, bootloaders can be strange */
+	ret = twl_rtc_write_u8(0, REG_RTC_INTERRUPTS_REG);
+	if (ret < 0)
+		dev_warn(&pdev->dev, "unable to disable interrupt\n");
+
 	/* init cached IRQ enable bits */
 	ret = twl_rtc_read_u8(&rtc_irq_bits, REG_RTC_INTERRUPTS_REG);
 	if (ret < 0)
diff --git a/drivers/s390/block/Kconfig b/drivers/s390/block/Kconfig
index 8e477bb1f3f6..4a3b62326183 100644
--- a/drivers/s390/block/Kconfig
+++ b/drivers/s390/block/Kconfig
@@ -70,3 +70,21 @@ config DASD_EER
 	  This driver provides a character device interface to the
 	  DASD extended error reporting. This is only needed if you want to
 	  use applications written for the EER facility.
+
+config SCM_BLOCK
+	def_tristate m
+	prompt "Support for Storage Class Memory"
+	depends on S390 && BLOCK && EADM_SCH && SCM_BUS
+	help
+	  Block device driver for Storage Class Memory (SCM). This driver
+	  provides a block device interface for each available SCM increment.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called scm_block.
+
+config SCM_BLOCK_CLUSTER_WRITE
+	def_bool y
+	prompt "SCM force cluster writes"
+	depends on SCM_BLOCK
+	help
+	  Force writes to Storage Class Memory (SCM) to be in done in clusters.
diff --git a/drivers/s390/block/Makefile b/drivers/s390/block/Makefile
index 0a89e080b389..c2f4e673e031 100644
--- a/drivers/s390/block/Makefile
+++ b/drivers/s390/block/Makefile
@@ -17,3 +17,9 @@ obj-$(CONFIG_DASD_ECKD) += dasd_eckd_mod.o
 obj-$(CONFIG_DASD_FBA)  += dasd_fba_mod.o
 obj-$(CONFIG_BLK_DEV_XPRAM) += xpram.o
 obj-$(CONFIG_DCSSBLK) += dcssblk.o
+
+scm_block-objs := scm_drv.o scm_blk.o
+ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
+scm_block-objs += scm_blk_cluster.o
+endif
+obj-$(CONFIG_SCM_BLOCK) += scm_block.o
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 15370a2c5ff0..0595c763dafd 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -534,11 +534,11 @@ static void dasd_change_state(struct dasd_device *device)
 	if (rc)
 		device->target = device->state;
 
-	if (device->state == device->target)
-		wake_up(&dasd_init_waitq);
-
 	/* let user-space know that the device status changed */
 	kobject_uevent(&device->cdev->dev.kobj, KOBJ_CHANGE);
+
+	if (device->state == device->target)
+		wake_up(&dasd_init_waitq);
 }
 
 /*
@@ -2157,6 +2157,7 @@ static int _dasd_sleep_on(struct dasd_ccw_req *maincqr, int interruptible)
 		    test_bit(DASD_CQR_FLAGS_FAILFAST, &cqr->flags) &&
 		    (!dasd_eer_enabled(device))) {
 			cqr->status = DASD_CQR_FAILED;
+			cqr->intrc = -EAGAIN;
 			continue;
 		}
 		/* Don't try to start requests if device is stopped */
@@ -3270,6 +3271,16 @@ void dasd_generic_path_event(struct ccw_device *cdev, int *path_event)
 			dasd_schedule_device_bh(device);
 		}
 		if (path_event[chp] & PE_PATHGROUP_ESTABLISHED) {
+			if (!(device->path_data.opm & eventlpm) &&
+			    !(device->path_data.tbvpm & eventlpm)) {
+				/*
+				 * we can not establish a pathgroup on an
+				 * unavailable path, so trigger a path
+				 * verification first
+				 */
+				device->path_data.tbvpm |= eventlpm;
+				dasd_schedule_device_bh(device);
+			}
 			DBF_DEV_EVENT(DBF_WARNING, device, "%s",
 				      "Pathgroup re-established\n");
 			if (device->discipline->kick_validate)
diff --git a/drivers/s390/block/dasd_alias.c b/drivers/s390/block/dasd_alias.c
index 157defe5e069..6b556995bb33 100644
--- a/drivers/s390/block/dasd_alias.c
+++ b/drivers/s390/block/dasd_alias.c
@@ -384,6 +384,29 @@ static void _remove_device_from_lcu(struct alias_lcu *lcu,
 		group->next = NULL;
 };
 
+static int
+suborder_not_supported(struct dasd_ccw_req *cqr)
+{
+	char *sense;
+	char reason;
+	char msg_format;
+	char msg_no;
+
+	sense = dasd_get_sense(&cqr->irb);
+	if (!sense)
+		return 0;
+
+	reason = sense[0];
+	msg_format = (sense[7] & 0xF0);
+	msg_no = (sense[7] & 0x0F);
+
+	/* command reject, Format 0 MSG 4 - invalid parameter */
+	if ((reason == 0x80) && (msg_format == 0x00) && (msg_no == 0x04))
+		return 1;
+
+	return 0;
+}
+
 static int read_unit_address_configuration(struct dasd_device *device,
 					   struct alias_lcu *lcu)
 {
@@ -435,6 +458,8 @@ static int read_unit_address_configuration(struct dasd_device *device,
 
 	do {
 		rc = dasd_sleep_on(cqr);
+		if (rc && suborder_not_supported(cqr))
+			return -EOPNOTSUPP;
 	} while (rc && (cqr->retries > 0));
 	if (rc) {
 		spin_lock_irqsave(&lcu->lock, flags);
@@ -521,7 +546,7 @@ static void lcu_update_work(struct work_struct *work)
 	 * processing the data
 	 */
 	spin_lock_irqsave(&lcu->lock, flags);
-	if (rc || (lcu->flags & NEED_UAC_UPDATE)) {
+	if ((rc && (rc != -EOPNOTSUPP)) || (lcu->flags & NEED_UAC_UPDATE)) {
 		DBF_DEV_EVENT(DBF_WARNING, device, "could not update"
 			    " alias data in lcu (rc = %d), retry later", rc);
 		schedule_delayed_work(&lcu->ruac_data.dwork, 30*HZ);
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 2fb2b9ea97ec..108332b44d98 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -20,6 +20,7 @@
 #include <linux/compat.h>
 #include <linux/init.h>
 
+#include <asm/css_chars.h>
 #include <asm/debug.h>
 #include <asm/idals.h>
 #include <asm/ebcdic.h>
@@ -31,8 +32,6 @@
 
 #include "dasd_int.h"
 #include "dasd_eckd.h"
-#include "../cio/chsc.h"
-
 
 #ifdef PRINTK_HEADER
 #undef PRINTK_HEADER
@@ -140,6 +139,10 @@ dasd_eckd_set_online(struct ccw_device *cdev)
 static const int sizes_trk0[] = { 28, 148, 84 };
 #define LABEL_SIZE 140
 
+/* head and record addresses of count_area read in analysis ccw */
+static const int count_area_head[] = { 0, 0, 0, 0, 2 };
+static const int count_area_rec[] = { 1, 2, 3, 4, 1 };
+
 static inline unsigned int
 round_up_multiple(unsigned int no, unsigned int mult)
 {
@@ -212,7 +215,7 @@ check_XRC (struct ccw1         *de_ccw,
 
 	rc = get_sync_clock(&data->ep_sys_time);
 	/* Ignore return code if sync clock is switched off. */
-	if (rc == -ENOSYS || rc == -EACCES)
+	if (rc == -EOPNOTSUPP || rc == -EACCES)
 		rc = 0;
 
 	de_ccw->count = sizeof(struct DE_eckd_data);
@@ -323,7 +326,7 @@ static int check_XRC_on_prefix(struct PFX_eckd_data *pfxdata,
 
 	rc = get_sync_clock(&pfxdata->define_extent.ep_sys_time);
 	/* Ignore return code if sync clock is switched off. */
-	if (rc == -ENOSYS || rc == -EACCES)
+	if (rc == -EOPNOTSUPP || rc == -EACCES)
 		rc = 0;
 	return rc;
 }
@@ -1507,7 +1510,8 @@ static struct dasd_ccw_req *dasd_eckd_build_psf_ssc(struct dasd_device *device,
  * call might change behaviour of DASD devices.
  */
 static int
-dasd_eckd_psf_ssc(struct dasd_device *device, int enable_pav)
+dasd_eckd_psf_ssc(struct dasd_device *device, int enable_pav,
+		  unsigned long flags)
 {
 	struct dasd_ccw_req *cqr;
 	int rc;
@@ -1516,10 +1520,19 @@ dasd_eckd_psf_ssc(struct dasd_device *device, int enable_pav)
 	if (IS_ERR(cqr))
 		return PTR_ERR(cqr);
 
+	/*
+	 * set flags e.g. turn on failfast, to prevent blocking
+	 * the calling function should handle failed requests
+	 */
+	cqr->flags |= flags;
+
 	rc = dasd_sleep_on(cqr);
 	if (!rc)
 		/* trigger CIO to reprobe devices */
 		css_schedule_reprobe();
+	else if (cqr->intrc == -EAGAIN)
+		rc = -EAGAIN;
+
 	dasd_sfree_request(cqr, cqr->memdev);
 	return rc;
 }
@@ -1527,7 +1540,8 @@ dasd_eckd_psf_ssc(struct dasd_device *device, int enable_pav)
 /*
  * Valide storage server of current device.
  */
-static void dasd_eckd_validate_server(struct dasd_device *device)
+static int dasd_eckd_validate_server(struct dasd_device *device,
+				     unsigned long flags)
 {
 	int rc;
 	struct dasd_eckd_private *private;
@@ -1536,17 +1550,18 @@ static void dasd_eckd_validate_server(struct dasd_device *device)
 	private = (struct dasd_eckd_private *) device->private;
 	if (private->uid.type == UA_BASE_PAV_ALIAS ||
 	    private->uid.type == UA_HYPER_PAV_ALIAS)
-		return;
+		return 0;
 	if (dasd_nopav || MACHINE_IS_VM)
 		enable_pav = 0;
 	else
 		enable_pav = 1;
-	rc = dasd_eckd_psf_ssc(device, enable_pav);
+	rc = dasd_eckd_psf_ssc(device, enable_pav, flags);
 
 	/* may be requested feature is not available on server,
 	 * therefore just report error and go ahead */
 	DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "PSF-SSC for SSID %04x "
 			"returned rc=%d", private->uid.ssid, rc);
+	return rc;
 }
 
 /*
@@ -1556,7 +1571,13 @@ static void dasd_eckd_do_validate_server(struct work_struct *work)
 {
 	struct dasd_device *device = container_of(work, struct dasd_device,
 						  kick_validate);
-	dasd_eckd_validate_server(device);
+	if (dasd_eckd_validate_server(device, DASD_CQR_FLAGS_FAILFAST)
+	    == -EAGAIN) {
+		/* schedule worker again if failed */
+		schedule_work(&device->kick_validate);
+		return;
+	}
+
 	dasd_put_device(device);
 }
 
@@ -1685,7 +1706,7 @@ dasd_eckd_check_characteristics(struct dasd_device *device)
 	if (rc)
 		goto out_err2;
 
-	dasd_eckd_validate_server(device);
+	dasd_eckd_validate_server(device, 0);
 
 	/* device may report different configuration data after LCU setup */
 	rc = dasd_eckd_read_conf(device);
@@ -1922,7 +1943,10 @@ static int dasd_eckd_end_analysis(struct dasd_block *block)
 	count_area = NULL;
 	for (i = 0; i < 3; i++) {
 		if (private->count_area[i].kl != 4 ||
-		    private->count_area[i].dl != dasd_eckd_cdl_reclen(i) - 4) {
+		    private->count_area[i].dl != dasd_eckd_cdl_reclen(i) - 4 ||
+		    private->count_area[i].cyl != 0 ||
+		    private->count_area[i].head != count_area_head[i] ||
+		    private->count_area[i].record != count_area_rec[i]) {
 			private->uses_cdl = 0;
 			break;
 		}
@@ -1934,7 +1958,10 @@ static int dasd_eckd_end_analysis(struct dasd_block *block)
 		for (i = 0; i < 5; i++) {
 			if ((private->count_area[i].kl != 0) ||
 			    (private->count_area[i].dl !=
-			     private->count_area[0].dl))
+			     private->count_area[0].dl) ||
+			    private->count_area[i].cyl !=  0 ||
+			    private->count_area[i].head != count_area_head[i] ||
+			    private->count_area[i].record != count_area_rec[i])
 				break;
 		}
 		if (i == 5)
@@ -4153,7 +4180,7 @@ static int dasd_eckd_restore_device(struct dasd_device *device)
 	rc = dasd_alias_make_device_known_to_lcu(device);
 	if (rc)
 		return rc;
-	dasd_eckd_validate_server(device);
+	dasd_eckd_validate_server(device, DASD_CQR_FLAGS_FAILFAST);
 
 	/* RE-Read Configuration Data */
 	rc = dasd_eckd_read_conf(device);
diff --git a/drivers/s390/block/dasd_ioctl.c b/drivers/s390/block/dasd_ioctl.c
index 654c6921a6d4..8252f37d04ed 100644
--- a/drivers/s390/block/dasd_ioctl.c
+++ b/drivers/s390/block/dasd_ioctl.c
@@ -292,12 +292,12 @@ out:
 #else
 static int dasd_ioctl_reset_profile(struct dasd_block *block)
 {
-	return -ENOSYS;
+	return -ENOTTY;
 }
 
 static int dasd_ioctl_read_profile(struct dasd_block *block, void __user *argp)
 {
-	return -ENOSYS;
+	return -ENOTTY;
 }
 #endif
 
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
new file mode 100644
index 000000000000..9978ad4433cb
--- /dev/null
+++ b/drivers/s390/block/scm_blk.c
@@ -0,0 +1,445 @@
+/*
+ * Block driver for s390 storage class memory.
+ *
+ * Copyright IBM Corp. 2012
+ * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
+ */
+
+#define KMSG_COMPONENT "scm_block"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/genhd.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <asm/eadm.h>
+#include "scm_blk.h"
+
+debug_info_t *scm_debug;
+static int scm_major;
+static DEFINE_SPINLOCK(list_lock);
+static LIST_HEAD(inactive_requests);
+static unsigned int nr_requests = 64;
+static atomic_t nr_devices = ATOMIC_INIT(0);
+module_param(nr_requests, uint, S_IRUGO);
+MODULE_PARM_DESC(nr_requests, "Number of parallel requests.");
+
+MODULE_DESCRIPTION("Block driver for s390 storage class memory.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("scm:scmdev*");
+
+static void __scm_free_rq(struct scm_request *scmrq)
+{
+	struct aob_rq_header *aobrq = to_aobrq(scmrq);
+
+	free_page((unsigned long) scmrq->aob);
+	free_page((unsigned long) scmrq->aidaw);
+	__scm_free_rq_cluster(scmrq);
+	kfree(aobrq);
+}
+
+static void scm_free_rqs(void)
+{
+	struct list_head *iter, *safe;
+	struct scm_request *scmrq;
+
+	spin_lock_irq(&list_lock);
+	list_for_each_safe(iter, safe, &inactive_requests) {
+		scmrq = list_entry(iter, struct scm_request, list);
+		list_del(&scmrq->list);
+		__scm_free_rq(scmrq);
+	}
+	spin_unlock_irq(&list_lock);
+}
+
+static int __scm_alloc_rq(void)
+{
+	struct aob_rq_header *aobrq;
+	struct scm_request *scmrq;
+
+	aobrq = kzalloc(sizeof(*aobrq) + sizeof(*scmrq), GFP_KERNEL);
+	if (!aobrq)
+		return -ENOMEM;
+
+	scmrq = (void *) aobrq->data;
+	scmrq->aidaw = (void *) get_zeroed_page(GFP_DMA);
+	scmrq->aob = (void *) get_zeroed_page(GFP_DMA);
+	if (!scmrq->aob || !scmrq->aidaw) {
+		__scm_free_rq(scmrq);
+		return -ENOMEM;
+	}
+
+	if (__scm_alloc_rq_cluster(scmrq)) {
+		__scm_free_rq(scmrq);
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&scmrq->list);
+	spin_lock_irq(&list_lock);
+	list_add(&scmrq->list, &inactive_requests);
+	spin_unlock_irq(&list_lock);
+
+	return 0;
+}
+
+static int scm_alloc_rqs(unsigned int nrqs)
+{
+	int ret = 0;
+
+	while (nrqs-- && !ret)
+		ret = __scm_alloc_rq();
+
+	return ret;
+}
+
+static struct scm_request *scm_request_fetch(void)
+{
+	struct scm_request *scmrq = NULL;
+
+	spin_lock(&list_lock);
+	if (list_empty(&inactive_requests))
+		goto out;
+	scmrq = list_first_entry(&inactive_requests, struct scm_request, list);
+	list_del(&scmrq->list);
+out:
+	spin_unlock(&list_lock);
+	return scmrq;
+}
+
+static void scm_request_done(struct scm_request *scmrq)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&list_lock, flags);
+	list_add(&scmrq->list, &inactive_requests);
+	spin_unlock_irqrestore(&list_lock, flags);
+}
+
+static int scm_open(struct block_device *blkdev, fmode_t mode)
+{
+	return scm_get_ref();
+}
+
+static int scm_release(struct gendisk *gendisk, fmode_t mode)
+{
+	scm_put_ref();
+	return 0;
+}
+
+static const struct block_device_operations scm_blk_devops = {
+	.owner = THIS_MODULE,
+	.open = scm_open,
+	.release = scm_release,
+};
+
+static void scm_request_prepare(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	struct scm_device *scmdev = bdev->gendisk->private_data;
+	struct aidaw *aidaw = scmrq->aidaw;
+	struct msb *msb = &scmrq->aob->msb[0];
+	struct req_iterator iter;
+	struct bio_vec *bv;
+
+	msb->bs = MSB_BS_4K;
+	scmrq->aob->request.msb_count = 1;
+	msb->scm_addr = scmdev->address +
+		((u64) blk_rq_pos(scmrq->request) << 9);
+	msb->oc = (rq_data_dir(scmrq->request) == READ) ?
+		MSB_OC_READ : MSB_OC_WRITE;
+	msb->flags |= MSB_FLAG_IDA;
+	msb->data_addr = (u64) aidaw;
+
+	rq_for_each_segment(bv, scmrq->request, iter) {
+		WARN_ON(bv->bv_offset);
+		msb->blk_count += bv->bv_len >> 12;
+		aidaw->data_addr = (u64) page_address(bv->bv_page);
+		aidaw++;
+	}
+}
+
+static inline void scm_request_init(struct scm_blk_dev *bdev,
+				    struct scm_request *scmrq,
+				    struct request *req)
+{
+	struct aob_rq_header *aobrq = to_aobrq(scmrq);
+	struct aob *aob = scmrq->aob;
+
+	memset(aob, 0, sizeof(*aob));
+	memset(scmrq->aidaw, 0, PAGE_SIZE);
+	aobrq->scmdev = bdev->scmdev;
+	aob->request.cmd_code = ARQB_CMD_MOVE;
+	aob->request.data = (u64) aobrq;
+	scmrq->request = req;
+	scmrq->bdev = bdev;
+	scmrq->retries = 4;
+	scmrq->error = 0;
+	scm_request_cluster_init(scmrq);
+}
+
+static void scm_ensure_queue_restart(struct scm_blk_dev *bdev)
+{
+	if (atomic_read(&bdev->queued_reqs)) {
+		/* Queue restart is triggered by the next interrupt. */
+		return;
+	}
+	blk_delay_queue(bdev->rq, SCM_QUEUE_DELAY);
+}
+
+void scm_request_requeue(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+
+	scm_release_cluster(scmrq);
+	blk_requeue_request(bdev->rq, scmrq->request);
+	scm_request_done(scmrq);
+	scm_ensure_queue_restart(bdev);
+}
+
+void scm_request_finish(struct scm_request *scmrq)
+{
+	scm_release_cluster(scmrq);
+	blk_end_request_all(scmrq->request, scmrq->error);
+	scm_request_done(scmrq);
+}
+
+static void scm_blk_request(struct request_queue *rq)
+{
+	struct scm_device *scmdev = rq->queuedata;
+	struct scm_blk_dev *bdev = dev_get_drvdata(&scmdev->dev);
+	struct scm_request *scmrq;
+	struct request *req;
+	int ret;
+
+	while ((req = blk_peek_request(rq))) {
+		if (req->cmd_type != REQ_TYPE_FS)
+			continue;
+
+		scmrq = scm_request_fetch();
+		if (!scmrq) {
+			SCM_LOG(5, "no request");
+			scm_ensure_queue_restart(bdev);
+			return;
+		}
+		scm_request_init(bdev, scmrq, req);
+		if (!scm_reserve_cluster(scmrq)) {
+			SCM_LOG(5, "cluster busy");
+			scm_request_done(scmrq);
+			return;
+		}
+		if (scm_need_cluster_request(scmrq)) {
+			blk_start_request(req);
+			scm_initiate_cluster_request(scmrq);
+			return;
+		}
+		scm_request_prepare(scmrq);
+		blk_start_request(req);
+
+		ret = scm_start_aob(scmrq->aob);
+		if (ret) {
+			SCM_LOG(5, "no subchannel");
+			scm_request_requeue(scmrq);
+			return;
+		}
+		atomic_inc(&bdev->queued_reqs);
+	}
+}
+
+static void __scmrq_log_error(struct scm_request *scmrq)
+{
+	struct aob *aob = scmrq->aob;
+
+	if (scmrq->error == -ETIMEDOUT)
+		SCM_LOG(1, "Request timeout");
+	else {
+		SCM_LOG(1, "Request error");
+		SCM_LOG_HEX(1, &aob->response, sizeof(aob->response));
+	}
+	if (scmrq->retries)
+		SCM_LOG(1, "Retry request");
+	else
+		pr_err("An I/O operation to SCM failed with rc=%d\n",
+		       scmrq->error);
+}
+
+void scm_blk_irq(struct scm_device *scmdev, void *data, int error)
+{
+	struct scm_request *scmrq = data;
+	struct scm_blk_dev *bdev = scmrq->bdev;
+
+	scmrq->error = error;
+	if (error)
+		__scmrq_log_error(scmrq);
+
+	spin_lock(&bdev->lock);
+	list_add_tail(&scmrq->list, &bdev->finished_requests);
+	spin_unlock(&bdev->lock);
+	tasklet_hi_schedule(&bdev->tasklet);
+}
+
+static void scm_blk_tasklet(struct scm_blk_dev *bdev)
+{
+	struct scm_request *scmrq;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bdev->lock, flags);
+	while (!list_empty(&bdev->finished_requests)) {
+		scmrq = list_first_entry(&bdev->finished_requests,
+					 struct scm_request, list);
+		list_del(&scmrq->list);
+		spin_unlock_irqrestore(&bdev->lock, flags);
+
+		if (scmrq->error && scmrq->retries-- > 0) {
+			if (scm_start_aob(scmrq->aob)) {
+				spin_lock_irqsave(&bdev->rq_lock, flags);
+				scm_request_requeue(scmrq);
+				spin_unlock_irqrestore(&bdev->rq_lock, flags);
+			}
+			/* Request restarted or requeued, handle next. */
+			spin_lock_irqsave(&bdev->lock, flags);
+			continue;
+		}
+
+		if (scm_test_cluster_request(scmrq)) {
+			scm_cluster_request_irq(scmrq);
+			spin_lock_irqsave(&bdev->lock, flags);
+			continue;
+		}
+
+		scm_request_finish(scmrq);
+		atomic_dec(&bdev->queued_reqs);
+		spin_lock_irqsave(&bdev->lock, flags);
+	}
+	spin_unlock_irqrestore(&bdev->lock, flags);
+	/* Look out for more requests. */
+	blk_run_queue(bdev->rq);
+}
+
+int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
+{
+	struct request_queue *rq;
+	int len, ret = -ENOMEM;
+	unsigned int devindex, nr_max_blk;
+
+	devindex = atomic_inc_return(&nr_devices) - 1;
+	/* scma..scmz + scmaa..scmzz */
+	if (devindex > 701) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	bdev->scmdev = scmdev;
+	spin_lock_init(&bdev->rq_lock);
+	spin_lock_init(&bdev->lock);
+	INIT_LIST_HEAD(&bdev->finished_requests);
+	atomic_set(&bdev->queued_reqs, 0);
+	tasklet_init(&bdev->tasklet,
+		     (void (*)(unsigned long)) scm_blk_tasklet,
+		     (unsigned long) bdev);
+
+	rq = blk_init_queue(scm_blk_request, &bdev->rq_lock);
+	if (!rq)
+		goto out;
+
+	bdev->rq = rq;
+	nr_max_blk = min(scmdev->nr_max_block,
+			 (unsigned int) (PAGE_SIZE / sizeof(struct aidaw)));
+
+	blk_queue_logical_block_size(rq, 1 << 12);
+	blk_queue_max_hw_sectors(rq, nr_max_blk << 3); /* 8 * 512 = blk_size */
+	blk_queue_max_segments(rq, nr_max_blk);
+	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, rq);
+	scm_blk_dev_cluster_setup(bdev);
+
+	bdev->gendisk = alloc_disk(SCM_NR_PARTS);
+	if (!bdev->gendisk)
+		goto out_queue;
+
+	rq->queuedata = scmdev;
+	bdev->gendisk->driverfs_dev = &scmdev->dev;
+	bdev->gendisk->private_data = scmdev;
+	bdev->gendisk->fops = &scm_blk_devops;
+	bdev->gendisk->queue = rq;
+	bdev->gendisk->major = scm_major;
+	bdev->gendisk->first_minor = devindex * SCM_NR_PARTS;
+
+	len = snprintf(bdev->gendisk->disk_name, DISK_NAME_LEN, "scm");
+	if (devindex > 25) {
+		len += snprintf(bdev->gendisk->disk_name + len,
+				DISK_NAME_LEN - len, "%c",
+				'a' + (devindex / 26) - 1);
+		devindex = devindex % 26;
+	}
+	snprintf(bdev->gendisk->disk_name + len, DISK_NAME_LEN - len, "%c",
+		 'a' + devindex);
+
+	/* 512 byte sectors */
+	set_capacity(bdev->gendisk, scmdev->size >> 9);
+	add_disk(bdev->gendisk);
+	return 0;
+
+out_queue:
+	blk_cleanup_queue(rq);
+out:
+	atomic_dec(&nr_devices);
+	return ret;
+}
+
+void scm_blk_dev_cleanup(struct scm_blk_dev *bdev)
+{
+	tasklet_kill(&bdev->tasklet);
+	del_gendisk(bdev->gendisk);
+	blk_cleanup_queue(bdev->gendisk->queue);
+	put_disk(bdev->gendisk);
+}
+
+static int __init scm_blk_init(void)
+{
+	int ret = -EINVAL;
+
+	if (!scm_cluster_size_valid())
+		goto out;
+
+	ret = register_blkdev(0, "scm");
+	if (ret < 0)
+		goto out;
+
+	scm_major = ret;
+	if (scm_alloc_rqs(nr_requests))
+		goto out_unreg;
+
+	scm_debug = debug_register("scm_log", 16, 1, 16);
+	if (!scm_debug)
+		goto out_free;
+
+	debug_register_view(scm_debug, &debug_hex_ascii_view);
+	debug_set_level(scm_debug, 2);
+
+	ret = scm_drv_init();
+	if (ret)
+		goto out_dbf;
+
+	return ret;
+
+out_dbf:
+	debug_unregister(scm_debug);
+out_free:
+	scm_free_rqs();
+out_unreg:
+	unregister_blkdev(scm_major, "scm");
+out:
+	return ret;
+}
+module_init(scm_blk_init);
+
+static void __exit scm_blk_cleanup(void)
+{
+	scm_drv_cleanup();
+	debug_unregister(scm_debug);
+	scm_free_rqs();
+	unregister_blkdev(scm_major, "scm");
+}
+module_exit(scm_blk_cleanup);
diff --git a/drivers/s390/block/scm_blk.h b/drivers/s390/block/scm_blk.h
new file mode 100644
index 000000000000..7ac6bad919ef
--- /dev/null
+++ b/drivers/s390/block/scm_blk.h
@@ -0,0 +1,117 @@
+#ifndef SCM_BLK_H
+#define SCM_BLK_H
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/blkdev.h>
+#include <linux/genhd.h>
+#include <linux/list.h>
+
+#include <asm/debug.h>
+#include <asm/eadm.h>
+
+#define SCM_NR_PARTS 8
+#define SCM_QUEUE_DELAY 5
+
+struct scm_blk_dev {
+	struct tasklet_struct tasklet;
+	struct request_queue *rq;
+	struct gendisk *gendisk;
+	struct scm_device *scmdev;
+	spinlock_t rq_lock;	/* guard the request queue */
+	spinlock_t lock;	/* guard the rest of the blockdev */
+	atomic_t queued_reqs;
+	struct list_head finished_requests;
+#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
+	struct list_head cluster_list;
+#endif
+};
+
+struct scm_request {
+	struct scm_blk_dev *bdev;
+	struct request *request;
+	struct aidaw *aidaw;
+	struct aob *aob;
+	struct list_head list;
+	u8 retries;
+	int error;
+#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
+	struct {
+		enum {CLUSTER_NONE, CLUSTER_READ, CLUSTER_WRITE} state;
+		struct list_head list;
+		void **buf;
+	} cluster;
+#endif
+};
+
+#define to_aobrq(rq) container_of((void *) rq, struct aob_rq_header, data)
+
+int scm_blk_dev_setup(struct scm_blk_dev *, struct scm_device *);
+void scm_blk_dev_cleanup(struct scm_blk_dev *);
+void scm_blk_irq(struct scm_device *, void *, int);
+
+void scm_request_finish(struct scm_request *);
+void scm_request_requeue(struct scm_request *);
+
+int scm_drv_init(void);
+void scm_drv_cleanup(void);
+
+#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
+void __scm_free_rq_cluster(struct scm_request *);
+int __scm_alloc_rq_cluster(struct scm_request *);
+void scm_request_cluster_init(struct scm_request *);
+bool scm_reserve_cluster(struct scm_request *);
+void scm_release_cluster(struct scm_request *);
+void scm_blk_dev_cluster_setup(struct scm_blk_dev *);
+bool scm_need_cluster_request(struct scm_request *);
+void scm_initiate_cluster_request(struct scm_request *);
+void scm_cluster_request_irq(struct scm_request *);
+bool scm_test_cluster_request(struct scm_request *);
+bool scm_cluster_size_valid(void);
+#else
+#define __scm_free_rq_cluster(scmrq) {}
+#define __scm_alloc_rq_cluster(scmrq) 0
+#define scm_request_cluster_init(scmrq) {}
+#define scm_reserve_cluster(scmrq) true
+#define scm_release_cluster(scmrq) {}
+#define scm_blk_dev_cluster_setup(bdev) {}
+#define scm_need_cluster_request(scmrq) false
+#define scm_initiate_cluster_request(scmrq) {}
+#define scm_cluster_request_irq(scmrq) {}
+#define scm_test_cluster_request(scmrq) false
+#define scm_cluster_size_valid() true
+#endif
+
+extern debug_info_t *scm_debug;
+
+#define SCM_LOG(imp, txt) do {					\
+		debug_text_event(scm_debug, imp, txt);		\
+	} while (0)
+
+static inline void SCM_LOG_HEX(int level, void *data, int length)
+{
+	if (level > scm_debug->level)
+		return;
+	while (length > 0) {
+		debug_event(scm_debug, level, data, length);
+		length -= scm_debug->buf_size;
+		data += scm_debug->buf_size;
+	}
+}
+
+static inline void SCM_LOG_STATE(int level, struct scm_device *scmdev)
+{
+	struct {
+		u64 address;
+		u8 oper_state;
+		u8 rank;
+	} __packed data = {
+		.address = scmdev->address,
+		.oper_state = scmdev->attrs.oper_state,
+		.rank = scmdev->attrs.rank,
+	};
+
+	SCM_LOG_HEX(level, &data, sizeof(data));
+}
+
+#endif /* SCM_BLK_H */
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
new file mode 100644
index 000000000000..f4bb61b0cea1
--- /dev/null
+++ b/drivers/s390/block/scm_blk_cluster.c
@@ -0,0 +1,228 @@
+/*
+ * Block driver for s390 storage class memory.
+ *
+ * Copyright IBM Corp. 2012
+ * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
+ */
+
+#include <linux/spinlock.h>
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/genhd.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <asm/eadm.h>
+#include "scm_blk.h"
+
+static unsigned int write_cluster_size = 64;
+module_param(write_cluster_size, uint, S_IRUGO);
+MODULE_PARM_DESC(write_cluster_size,
+		 "Number of pages used for contiguous writes.");
+
+#define CLUSTER_SIZE (write_cluster_size * PAGE_SIZE)
+
+void __scm_free_rq_cluster(struct scm_request *scmrq)
+{
+	int i;
+
+	if (!scmrq->cluster.buf)
+		return;
+
+	for (i = 0; i < 2 * write_cluster_size; i++)
+		free_page((unsigned long) scmrq->cluster.buf[i]);
+
+	kfree(scmrq->cluster.buf);
+}
+
+int __scm_alloc_rq_cluster(struct scm_request *scmrq)
+{
+	int i;
+
+	scmrq->cluster.buf = kzalloc(sizeof(void *) * 2 * write_cluster_size,
+				 GFP_KERNEL);
+	if (!scmrq->cluster.buf)
+		return -ENOMEM;
+
+	for (i = 0; i < 2 * write_cluster_size; i++) {
+		scmrq->cluster.buf[i] = (void *) get_zeroed_page(GFP_DMA);
+		if (!scmrq->cluster.buf[i])
+			return -ENOMEM;
+	}
+	INIT_LIST_HEAD(&scmrq->cluster.list);
+	return 0;
+}
+
+void scm_request_cluster_init(struct scm_request *scmrq)
+{
+	scmrq->cluster.state = CLUSTER_NONE;
+}
+
+static bool clusters_intersect(struct scm_request *A, struct scm_request *B)
+{
+	unsigned long firstA, lastA, firstB, lastB;
+
+	firstA = ((u64) blk_rq_pos(A->request) << 9) / CLUSTER_SIZE;
+	lastA = (((u64) blk_rq_pos(A->request) << 9) +
+		    blk_rq_bytes(A->request) - 1) / CLUSTER_SIZE;
+
+	firstB = ((u64) blk_rq_pos(B->request) << 9) / CLUSTER_SIZE;
+	lastB = (((u64) blk_rq_pos(B->request) << 9) +
+		    blk_rq_bytes(B->request) - 1) / CLUSTER_SIZE;
+
+	return (firstB <= lastA && firstA <= lastB);
+}
+
+bool scm_reserve_cluster(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	struct scm_request *iter;
+
+	if (write_cluster_size == 0)
+		return true;
+
+	spin_lock(&bdev->lock);
+	list_for_each_entry(iter, &bdev->cluster_list, cluster.list) {
+		if (clusters_intersect(scmrq, iter) &&
+		    (rq_data_dir(scmrq->request) == WRITE ||
+		     rq_data_dir(iter->request) == WRITE)) {
+			spin_unlock(&bdev->lock);
+			return false;
+		}
+	}
+	list_add(&scmrq->cluster.list, &bdev->cluster_list);
+	spin_unlock(&bdev->lock);
+
+	return true;
+}
+
+void scm_release_cluster(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	unsigned long flags;
+
+	if (write_cluster_size == 0)
+		return;
+
+	spin_lock_irqsave(&bdev->lock, flags);
+	list_del(&scmrq->cluster.list);
+	spin_unlock_irqrestore(&bdev->lock, flags);
+}
+
+void scm_blk_dev_cluster_setup(struct scm_blk_dev *bdev)
+{
+	INIT_LIST_HEAD(&bdev->cluster_list);
+	blk_queue_io_opt(bdev->rq, CLUSTER_SIZE);
+}
+
+static void scm_prepare_cluster_request(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	struct scm_device *scmdev = bdev->gendisk->private_data;
+	struct request *req = scmrq->request;
+	struct aidaw *aidaw = scmrq->aidaw;
+	struct msb *msb = &scmrq->aob->msb[0];
+	struct req_iterator iter;
+	struct bio_vec *bv;
+	int i = 0;
+	u64 addr;
+
+	switch (scmrq->cluster.state) {
+	case CLUSTER_NONE:
+		scmrq->cluster.state = CLUSTER_READ;
+		/* fall through */
+	case CLUSTER_READ:
+		scmrq->aob->request.msb_count = 1;
+		msb->bs = MSB_BS_4K;
+		msb->oc = MSB_OC_READ;
+		msb->flags = MSB_FLAG_IDA;
+		msb->data_addr = (u64) aidaw;
+		msb->blk_count = write_cluster_size;
+
+		addr = scmdev->address + ((u64) blk_rq_pos(req) << 9);
+		msb->scm_addr = round_down(addr, CLUSTER_SIZE);
+
+		if (msb->scm_addr !=
+		    round_down(addr + (u64) blk_rq_bytes(req) - 1,
+			       CLUSTER_SIZE))
+			msb->blk_count = 2 * write_cluster_size;
+
+		for (i = 0; i < msb->blk_count; i++) {
+			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
+			aidaw++;
+		}
+
+		break;
+	case CLUSTER_WRITE:
+		msb->oc = MSB_OC_WRITE;
+
+		for (addr = msb->scm_addr;
+		     addr < scmdev->address + ((u64) blk_rq_pos(req) << 9);
+		     addr += PAGE_SIZE) {
+			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
+			aidaw++;
+			i++;
+		}
+		rq_for_each_segment(bv, req, iter) {
+			aidaw->data_addr = (u64) page_address(bv->bv_page);
+			aidaw++;
+			i++;
+		}
+		for (; i < msb->blk_count; i++) {
+			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
+			aidaw++;
+		}
+		break;
+	}
+}
+
+bool scm_need_cluster_request(struct scm_request *scmrq)
+{
+	if (rq_data_dir(scmrq->request) == READ)
+		return false;
+
+	return blk_rq_bytes(scmrq->request) < CLUSTER_SIZE;
+}
+
+/* Called with queue lock held. */
+void scm_initiate_cluster_request(struct scm_request *scmrq)
+{
+	scm_prepare_cluster_request(scmrq);
+	if (scm_start_aob(scmrq->aob))
+		scm_request_requeue(scmrq);
+}
+
+bool scm_test_cluster_request(struct scm_request *scmrq)
+{
+	return scmrq->cluster.state != CLUSTER_NONE;
+}
+
+void scm_cluster_request_irq(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	unsigned long flags;
+
+	switch (scmrq->cluster.state) {
+	case CLUSTER_NONE:
+		BUG();
+		break;
+	case CLUSTER_READ:
+		if (scmrq->error) {
+			scm_request_finish(scmrq);
+			break;
+		}
+		scmrq->cluster.state = CLUSTER_WRITE;
+		spin_lock_irqsave(&bdev->rq_lock, flags);
+		scm_initiate_cluster_request(scmrq);
+		spin_unlock_irqrestore(&bdev->rq_lock, flags);
+		break;
+	case CLUSTER_WRITE:
+		scm_request_finish(scmrq);
+		break;
+	}
+}
+
+bool scm_cluster_size_valid(void)
+{
+	return write_cluster_size == 0 || write_cluster_size == 32 ||
+		write_cluster_size == 64 || write_cluster_size == 128;
+}
diff --git a/drivers/s390/block/scm_drv.c b/drivers/s390/block/scm_drv.c
new file mode 100644
index 000000000000..9fa0a908607b
--- /dev/null
+++ b/drivers/s390/block/scm_drv.c
@@ -0,0 +1,81 @@
+/*
+ * Device driver for s390 storage class memory.
+ *
+ * Copyright IBM Corp. 2012
+ * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
+ */
+
+#define KMSG_COMPONENT "scm_block"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <asm/eadm.h>
+#include "scm_blk.h"
+
+static void notify(struct scm_device *scmdev)
+{
+	pr_info("%lu: The capabilities of the SCM increment changed\n",
+		(unsigned long) scmdev->address);
+	SCM_LOG(2, "State changed");
+	SCM_LOG_STATE(2, scmdev);
+}
+
+static int scm_probe(struct scm_device *scmdev)
+{
+	struct scm_blk_dev *bdev;
+	int ret;
+
+	SCM_LOG(2, "probe");
+	SCM_LOG_STATE(2, scmdev);
+
+	if (scmdev->attrs.oper_state != OP_STATE_GOOD)
+		return -EINVAL;
+
+	bdev = kzalloc(sizeof(*bdev), GFP_KERNEL);
+	if (!bdev)
+		return -ENOMEM;
+
+	dev_set_drvdata(&scmdev->dev, bdev);
+	ret = scm_blk_dev_setup(bdev, scmdev);
+	if (ret) {
+		dev_set_drvdata(&scmdev->dev, NULL);
+		kfree(bdev);
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static int scm_remove(struct scm_device *scmdev)
+{
+	struct scm_blk_dev *bdev = dev_get_drvdata(&scmdev->dev);
+
+	scm_blk_dev_cleanup(bdev);
+	dev_set_drvdata(&scmdev->dev, NULL);
+	kfree(bdev);
+
+	return 0;
+}
+
+static struct scm_driver scm_drv = {
+	.drv = {
+		.name = "scm_block",
+		.owner = THIS_MODULE,
+	},
+	.notify = notify,
+	.probe = scm_probe,
+	.remove = scm_remove,
+	.handler = scm_blk_irq,
+};
+
+int __init scm_drv_init(void)
+{
+	return scm_driver_register(&scm_drv);
+}
+
+void scm_drv_cleanup(void)
+{
+	scm_driver_unregister(&scm_drv);
+}
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index 6c0116d48c74..9ffb6d5f17aa 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -716,10 +716,17 @@ static int raw3215_probe (struct ccw_device *cdev)
 static void raw3215_remove (struct ccw_device *cdev)
 {
 	struct raw3215_info *raw;
+	unsigned int line;
 
 	ccw_device_set_offline(cdev);
 	raw = dev_get_drvdata(&cdev->dev);
 	if (raw) {
+		spin_lock(&raw3215_device_lock);
+		for (line = 0; line < NR_3215; line++)
+			if (raw3215[line] == raw)
+				break;
+		raw3215[line] = NULL;
+		spin_unlock(&raw3215_device_lock);
 		dev_set_drvdata(&cdev->dev, NULL);
 		raw3215_free_info(raw);
 	}
@@ -935,6 +942,19 @@ static int __init con3215_init(void)
 console_initcall(con3215_init);
 #endif
 
+static int tty3215_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct raw3215_info *raw;
+
+	raw = raw3215[tty->index];
+	if (raw == NULL)
+		return -ENODEV;
+
+	tty->driver_data = raw;
+
+	return tty_port_install(&raw->port, driver, tty);
+}
+
 /*
  * tty3215_open
  *
@@ -942,14 +962,9 @@ console_initcall(con3215_init);
  */
 static int tty3215_open(struct tty_struct *tty, struct file * filp)
 {
-	struct raw3215_info *raw;
+	struct raw3215_info *raw = tty->driver_data;
 	int retval;
 
-	raw = raw3215[tty->index];
-	if (raw == NULL)
-		return -ENODEV;
-
-	tty->driver_data = raw;
 	tty_port_tty_set(&raw->port, tty);
 
 	tty->low_latency = 0;  /* don't use bottom half for pushing chars */
@@ -1110,6 +1125,7 @@ static void tty3215_start(struct tty_struct *tty)
 }
 
 static const struct tty_operations tty3215_ops = {
+	.install = tty3215_install,
 	.open = tty3215_open,
 	.close = tty3215_close,
 	.write = tty3215_write,
diff --git a/drivers/s390/char/con3270.c b/drivers/s390/char/con3270.c
index bb07577e8fd4..699fd3e363df 100644
--- a/drivers/s390/char/con3270.c
+++ b/drivers/s390/char/con3270.c
@@ -35,7 +35,6 @@ static struct raw3270_fn con3270_fn;
  */
 struct con3270 {
 	struct raw3270_view view;
-	spinlock_t lock;
 	struct list_head freemem;	/* list of free memory for strings. */
 
 	/* Output stuff. */
diff --git a/drivers/s390/char/monreader.c b/drivers/s390/char/monreader.c
index 5b8b8592d311..f4ff515db251 100644
--- a/drivers/s390/char/monreader.c
+++ b/drivers/s390/char/monreader.c
@@ -571,8 +571,11 @@ static int __init mon_init(void)
 	if (rc)
 		goto out_iucv;
 	monreader_device = kzalloc(sizeof(struct device), GFP_KERNEL);
-	if (!monreader_device)
+	if (!monreader_device) {
+		rc = -ENOMEM;
 		goto out_driver;
+	}
+
 	dev_set_name(monreader_device, "monreader-dev");
 	monreader_device->bus = &iucv_bus;
 	monreader_device->parent = iucv_root;
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index 3fcc000efc53..4fa21f7e2308 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -334,7 +334,7 @@ sclp_dispatch_evbufs(struct sccb_header *sccb)
 			reg->receiver_fn(evbuf);
 			spin_lock_irqsave(&sclp_lock, flags);
 		} else if (reg == NULL)
-			rc = -ENOSYS;
+			rc = -EOPNOTSUPP;
 	}
 	spin_unlock_irqrestore(&sclp_lock, flags);
 	return rc;
diff --git a/drivers/s390/char/sclp_rw.c b/drivers/s390/char/sclp_rw.c
index 4be63be73445..3b13d58fe87b 100644
--- a/drivers/s390/char/sclp_rw.c
+++ b/drivers/s390/char/sclp_rw.c
@@ -463,7 +463,7 @@ sclp_emit_buffer(struct sclp_buffer *buffer,
 		/* Use write priority message */
 		sccb->msg_buf.header.type = EVTYP_PMSGCMD;
 	else
-		return -ENOSYS;
+		return -EOPNOTSUPP;
 	buffer->request.command = SCLP_CMDW_WRITE_EVENT_DATA;
 	buffer->request.status = SCLP_REQ_FILLED;
 	buffer->request.callback = sclp_writedata_callback;
diff --git a/drivers/s390/char/sclp_tty.c b/drivers/s390/char/sclp_tty.c
index 0792c85baafe..30ec09e3d037 100644
--- a/drivers/s390/char/sclp_tty.c
+++ b/drivers/s390/char/sclp_tty.c
@@ -567,6 +567,7 @@ sclp_tty_init(void)
 	driver->init_termios.c_lflag = ISIG | ECHO;
 	driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(driver, &sclp_ops);
+	tty_port_link_device(&sclp_port, driver, 0);
 	rc = tty_register_driver(driver);
 	if (rc) {
 		put_tty_driver(driver);
diff --git a/drivers/s390/char/sclp_vt220.c b/drivers/s390/char/sclp_vt220.c
index edfc0fd73dc6..7e60f3d2f3f9 100644
--- a/drivers/s390/char/sclp_vt220.c
+++ b/drivers/s390/char/sclp_vt220.c
@@ -691,6 +691,7 @@ static int __init sclp_vt220_tty_init(void)
 	driver->init_termios = tty_std_termios;
 	driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(driver, &sclp_vt220_ops);
+	tty_port_link_device(&sclp_vt220_port, driver, 0);
 
 	rc = tty_register_driver(driver);
 	if (rc)
diff --git a/drivers/s390/char/tape.h b/drivers/s390/char/tape.h
index c06be6cc2fc3..ea664dd4f56d 100644
--- a/drivers/s390/char/tape.h
+++ b/drivers/s390/char/tape.h
@@ -15,7 +15,6 @@
 #include <asm/ccwdev.h>
 #include <asm/debug.h>
 #include <asm/idals.h>
-#include <linux/blkdev.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/mtio.h>
diff --git a/drivers/s390/char/tape_std.h b/drivers/s390/char/tape_std.h
index c5816ad9ed7d..8c760c036832 100644
--- a/drivers/s390/char/tape_std.h
+++ b/drivers/s390/char/tape_std.h
@@ -100,11 +100,7 @@ struct tape_request *tape_std_read_block(struct tape_device *, size_t);
 void tape_std_read_backward(struct tape_device *device,
 			    struct tape_request *request);
 struct tape_request *tape_std_write_block(struct tape_device *, size_t);
-struct tape_request *tape_std_bread(struct tape_device *, struct request *);
-void tape_std_free_bread(struct tape_request *);
 void tape_std_check_locate(struct tape_device *, struct tape_request *);
-struct tape_request *tape_std_bwrite(struct request *,
-				     struct tape_device *, int);
 
 /* Some non-mtop commands. */
 int tape_std_assign(struct tape_device *);
diff --git a/drivers/s390/char/tty3270.c b/drivers/s390/char/tty3270.c
index 1928f3458d10..482ee028f842 100644
--- a/drivers/s390/char/tty3270.c
+++ b/drivers/s390/char/tty3270.c
@@ -842,17 +842,14 @@ static struct raw3270_fn tty3270_fn = {
 };
 
 /*
- * This routine is called whenever a 3270 tty is opened.
+ * This routine is called whenever a 3270 tty is opened first time.
  */
-static int
-tty3270_open(struct tty_struct *tty, struct file * filp)
+static int tty3270_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	struct raw3270_view *view;
 	struct tty3270 *tp;
 	int i, rc;
 
-	if (tty->count > 1)
-		return 0;
 	/* Check if the tty3270 is already there. */
 	view = raw3270_find_view(&tty3270_fn,
 				  tty->index + RAW3270_FIRSTMINOR);
@@ -865,7 +862,7 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 		/* why to reassign? */
 		tty_port_tty_set(&tp->port, tty);
 		tp->inattr = TF_INPUT;
-		return 0;
+		return tty_port_install(&tp->port, driver, tty);
 	}
 	if (tty3270_max_index < tty->index + 1)
 		tty3270_max_index = tty->index + 1;
@@ -895,7 +892,6 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 
 	tty_port_tty_set(&tp->port, tty);
 	tty->low_latency = 0;
-	tty->driver_data = tp;
 	tty->winsize.ws_row = tp->view.rows - 2;
 	tty->winsize.ws_col = tp->view.cols;
 
@@ -915,6 +911,15 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 	kbd_ascebc(tp->kbd, tp->view.ascebc);
 
 	raw3270_activate_view(&tp->view);
+
+	rc = tty_port_install(&tp->port, driver, tty);
+	if (rc) {
+		raw3270_put_view(&tp->view);
+		return rc;
+	}
+
+	tty->driver_data = tp;
+
 	return 0;
 }
 
@@ -932,10 +937,17 @@ tty3270_close(struct tty_struct *tty, struct file * filp)
 	if (tp) {
 		tty->driver_data = NULL;
 		tty_port_tty_set(&tp->port, NULL);
-		raw3270_put_view(&tp->view);
 	}
 }
 
+static void tty3270_cleanup(struct tty_struct *tty)
+{
+	struct tty3270 *tp = tty->driver_data;
+
+	if (tp)
+		raw3270_put_view(&tp->view);
+}
+
 /*
  * We always have room.
  */
@@ -1737,7 +1749,8 @@ static long tty3270_compat_ioctl(struct tty_struct *tty,
 #endif
 
 static const struct tty_operations tty3270_ops = {
-	.open = tty3270_open,
+	.install = tty3270_install,
+	.cleanup = tty3270_cleanup,
 	.close = tty3270_close,
 	.write = tty3270_write,
 	.put_char = tty3270_put_char,
@@ -1781,7 +1794,7 @@ static int __init tty3270_init(void)
 	driver->type = TTY_DRIVER_TYPE_SYSTEM;
 	driver->subtype = SYSTEM_TYPE_TTY;
 	driver->init_termios = tty_std_termios;
-	driver->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_DYNAMIC_DEV;
+	driver->flags = TTY_DRIVER_RESET_TERMIOS;
 	tty_set_operations(driver, &tty3270_ops);
 	ret = tty_register_driver(driver);
 	if (ret) {
@@ -1800,6 +1813,7 @@ tty3270_exit(void)
 	driver = tty3270_driver;
 	tty3270_driver = NULL;
 	tty_unregister_driver(driver);
+	put_tty_driver(driver);
 	tty3270_del_views();
 }
 
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index c131bc40f962..9b3a24e8d3a0 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -321,7 +321,7 @@ static int vmlogrdr_open (struct inode *inode, struct file *filp)
 	 * only allow for blocking reads to be open
 	 */
 	if (filp->f_flags & O_NONBLOCK)
-		return -ENOSYS;
+		return -EOPNOTSUPP;
 
 	/* Besure this device hasn't already been opened */
 	spin_lock_bh(&logptr->priv_lock);
diff --git a/drivers/s390/cio/Makefile b/drivers/s390/cio/Makefile
index e1b700a19648..8c4a386e97f6 100644
--- a/drivers/s390/cio/Makefile
+++ b/drivers/s390/cio/Makefile
@@ -8,6 +8,8 @@ ccw_device-objs += device.o device_fsm.o device_ops.o
 ccw_device-objs += device_id.o device_pgid.o device_status.o
 obj-y += ccw_device.o cmf.o
 obj-$(CONFIG_CHSC_SCH) += chsc_sch.o
+obj-$(CONFIG_EADM_SCH) += eadm_sch.o
+obj-$(CONFIG_SCM_BUS) += scm.o
 obj-$(CONFIG_CCWGROUP) += ccwgroup.o
 
 qdio-objs := qdio_main.o qdio_thinint.o qdio_debug.o qdio_setup.o
diff --git a/drivers/s390/cio/chsc.c b/drivers/s390/cio/chsc.c
index cfe0c087fe5c..4d51a7c4eb8b 100644
--- a/drivers/s390/cio/chsc.c
+++ b/drivers/s390/cio/chsc.c
@@ -52,6 +52,11 @@ int chsc_error_from_response(int response)
 		return -EINVAL;
 	case 0x0004:
 		return -EOPNOTSUPP;
+	case 0x000b:
+		return -EBUSY;
+	case 0x0100:
+	case 0x0102:
+		return -ENOMEM;
 	default:
 		return -EIO;
 	}
@@ -393,6 +398,20 @@ static void chsc_process_sei_chp_config(struct chsc_sei_area *sei_area)
 	}
 }
 
+static void chsc_process_sei_scm_change(struct chsc_sei_area *sei_area)
+{
+	int ret;
+
+	CIO_CRW_EVENT(4, "chsc: scm change notification\n");
+	if (sei_area->rs != 7)
+		return;
+
+	ret = scm_update_information();
+	if (ret)
+		CIO_CRW_EVENT(0, "chsc: updating change notification"
+			      " failed (rc=%d).\n", ret);
+}
+
 static void chsc_process_sei(struct chsc_sei_area *sei_area)
 {
 	/* Check if we might have lost some information. */
@@ -414,6 +433,9 @@ static void chsc_process_sei(struct chsc_sei_area *sei_area)
 	case 8: /* channel-path-configuration notification */
 		chsc_process_sei_chp_config(sei_area);
 		break;
+	case 12: /* scm change notification */
+		chsc_process_sei_scm_change(sei_area);
+		break;
 	default: /* other stuff */
 		CIO_CRW_EVENT(4, "chsc: unhandled sei content code %d\n",
 			      sei_area->cc);
@@ -1047,3 +1069,33 @@ out:
 	return rc;
 }
 EXPORT_SYMBOL_GPL(chsc_siosl);
+
+/**
+ * chsc_scm_info() - store SCM information (SSI)
+ * @scm_area: request and response block for SSI
+ * @token: continuation token
+ *
+ * Returns 0 on success.
+ */
+int chsc_scm_info(struct chsc_scm_info *scm_area, u64 token)
+{
+	int ccode, ret;
+
+	memset(scm_area, 0, sizeof(*scm_area));
+	scm_area->request.length = 0x0020;
+	scm_area->request.code = 0x004C;
+	scm_area->reqtok = token;
+
+	ccode = chsc(scm_area);
+	if (ccode > 0) {
+		ret = (ccode == 3) ? -ENODEV : -EBUSY;
+		goto out;
+	}
+	ret = chsc_error_from_response(scm_area->response.code);
+	if (ret != 0)
+		CIO_MSG_EVENT(2, "chsc: scm info failed (rc=%04x)\n",
+			      scm_area->response.code);
+out:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(chsc_scm_info);
diff --git a/drivers/s390/cio/chsc.h b/drivers/s390/cio/chsc.h
index 3f15b2aaeaea..662dab4b93e6 100644
--- a/drivers/s390/cio/chsc.h
+++ b/drivers/s390/cio/chsc.h
@@ -3,6 +3,7 @@
 
 #include <linux/types.h>
 #include <linux/device.h>
+#include <asm/css_chars.h>
 #include <asm/chpid.h>
 #include <asm/chsc.h>
 #include <asm/schid.h>
@@ -118,4 +119,46 @@ int chsc_error_from_response(int response);
 
 int chsc_siosl(struct subchannel_id schid);
 
+/* Functions and definitions to query storage-class memory. */
+struct sale {
+	u64 sa;
+	u32 p:4;
+	u32 op_state:4;
+	u32 data_state:4;
+	u32 rank:4;
+	u32 r:1;
+	u32:7;
+	u32 rid:8;
+	u32:32;
+} __packed;
+
+struct chsc_scm_info {
+	struct chsc_header request;
+	u32:32;
+	u64 reqtok;
+	u32 reserved1[4];
+	struct chsc_header response;
+	u64:56;
+	u8 rq;
+	u32 mbc;
+	u64 msa;
+	u16 is;
+	u16 mmc;
+	u32 mci;
+	u64 nr_scm_ini;
+	u64 nr_scm_unini;
+	u32 reserved2[10];
+	u64 restok;
+	struct sale scmal[248];
+} __packed;
+
+int chsc_scm_info(struct chsc_scm_info *scm_area, u64 token);
+
+#ifdef CONFIG_SCM_BUS
+int scm_update_information(void);
+#else /* CONFIG_SCM_BUS */
+#define scm_update_information() 0
+#endif /* CONFIG_SCM_BUS */
+
+
 #endif
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 33d1ef703593..8e927b9f285f 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -1029,7 +1029,7 @@ extern void do_reipl_asm(__u32 schid);
 /* Make sure all subchannels are quiet before we re-ipl an lpar. */
 void reipl_ccw_dev(struct ccw_dev_id *devid)
 {
-	struct subchannel_id schid;
+	struct subchannel_id uninitialized_var(schid);
 
 	s390_reset_system(NULL, NULL);
 	if (reipl_find_schid(devid, &schid) != 0)
diff --git a/drivers/s390/cio/css.c b/drivers/s390/cio/css.c
index 21908e67bf67..b4d572f65f07 100644
--- a/drivers/s390/cio/css.c
+++ b/drivers/s390/cio/css.c
@@ -445,6 +445,7 @@ void css_sched_sch_todo(struct subchannel *sch, enum sch_todo todo)
 		put_device(&sch->dev);
 	}
 }
+EXPORT_SYMBOL_GPL(css_sched_sch_todo);
 
 static void css_sch_todo(struct work_struct *work)
 {
diff --git a/drivers/s390/cio/device.c b/drivers/s390/cio/device.c
index ed25c8740a9c..fc916f5d7314 100644
--- a/drivers/s390/cio/device.c
+++ b/drivers/s390/cio/device.c
@@ -1426,6 +1426,8 @@ static enum io_sch_action sch_get_action(struct subchannel *sch)
 		return IO_SCH_REPROBE;
 	if (cdev->online)
 		return IO_SCH_VERIFY;
+	if (cdev->private->state == DEV_STATE_NOT_OPER)
+		return IO_SCH_UNREG_ATTACH;
 	return IO_SCH_NOP;
 }
 
@@ -1519,11 +1521,14 @@ static int io_subchannel_sch_event(struct subchannel *sch, int process)
 			goto out;
 		break;
 	case IO_SCH_UNREG_ATTACH:
+		spin_lock_irqsave(sch->lock, flags);
 		if (cdev->private->flags.resuming) {
 			/* Device will be handled later. */
 			rc = 0;
-			goto out;
+			goto out_unlock;
 		}
+		sch_set_cdev(sch, NULL);
+		spin_unlock_irqrestore(sch->lock, flags);
 		/* Unregister ccw device. */
 		ccw_device_unregister(cdev);
 		break;
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
new file mode 100644
index 000000000000..6c9673400464
--- /dev/null
+++ b/drivers/s390/cio/eadm_sch.c
@@ -0,0 +1,401 @@
+/*
+ * Driver for s390 eadm subchannels
+ *
+ * Copyright IBM Corp. 2012
+ * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
+ */
+
+#include <linux/kernel_stat.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/timer.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+
+#include <asm/css_chars.h>
+#include <asm/debug.h>
+#include <asm/isc.h>
+#include <asm/cio.h>
+#include <asm/scsw.h>
+#include <asm/eadm.h>
+
+#include "eadm_sch.h"
+#include "ioasm.h"
+#include "cio.h"
+#include "css.h"
+#include "orb.h"
+
+MODULE_DESCRIPTION("driver for s390 eadm subchannels");
+MODULE_LICENSE("GPL");
+
+#define EADM_TIMEOUT (5 * HZ)
+static DEFINE_SPINLOCK(list_lock);
+static LIST_HEAD(eadm_list);
+
+static debug_info_t *eadm_debug;
+
+#define EADM_LOG(imp, txt) do {					\
+		debug_text_event(eadm_debug, imp, txt);		\
+	} while (0)
+
+static void EADM_LOG_HEX(int level, void *data, int length)
+{
+	if (level > eadm_debug->level)
+		return;
+	while (length > 0) {
+		debug_event(eadm_debug, level, data, length);
+		length -= eadm_debug->buf_size;
+		data += eadm_debug->buf_size;
+	}
+}
+
+static void orb_init(union orb *orb)
+{
+	memset(orb, 0, sizeof(union orb));
+	orb->eadm.compat1 = 1;
+	orb->eadm.compat2 = 1;
+	orb->eadm.fmt = 1;
+	orb->eadm.x = 1;
+}
+
+static int eadm_subchannel_start(struct subchannel *sch, struct aob *aob)
+{
+	union orb *orb = &get_eadm_private(sch)->orb;
+	int cc;
+
+	orb_init(orb);
+	orb->eadm.aob = (u32)__pa(aob);
+	orb->eadm.intparm = (u32)(addr_t)sch;
+	orb->eadm.key = PAGE_DEFAULT_KEY >> 4;
+
+	EADM_LOG(6, "start");
+	EADM_LOG_HEX(6, &sch->schid, sizeof(sch->schid));
+
+	cc = ssch(sch->schid, orb);
+	switch (cc) {
+	case 0:
+		sch->schib.scsw.eadm.actl |= SCSW_ACTL_START_PEND;
+		break;
+	case 1:		/* status pending */
+	case 2:		/* busy */
+		return -EBUSY;
+	case 3:		/* not operational */
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static int eadm_subchannel_clear(struct subchannel *sch)
+{
+	int cc;
+
+	cc = csch(sch->schid);
+	if (cc)
+		return -ENODEV;
+
+	sch->schib.scsw.eadm.actl |= SCSW_ACTL_CLEAR_PEND;
+	return 0;
+}
+
+static void eadm_subchannel_timeout(unsigned long data)
+{
+	struct subchannel *sch = (struct subchannel *) data;
+
+	spin_lock_irq(sch->lock);
+	EADM_LOG(1, "timeout");
+	EADM_LOG_HEX(1, &sch->schid, sizeof(sch->schid));
+	if (eadm_subchannel_clear(sch))
+		EADM_LOG(0, "clear failed");
+	spin_unlock_irq(sch->lock);
+}
+
+static void eadm_subchannel_set_timeout(struct subchannel *sch, int expires)
+{
+	struct eadm_private *private = get_eadm_private(sch);
+
+	if (expires == 0) {
+		del_timer(&private->timer);
+		return;
+	}
+	if (timer_pending(&private->timer)) {
+		if (mod_timer(&private->timer, jiffies + expires))
+			return;
+	}
+	private->timer.function = eadm_subchannel_timeout;
+	private->timer.data = (unsigned long) sch;
+	private->timer.expires = jiffies + expires;
+	add_timer(&private->timer);
+}
+
+static void eadm_subchannel_irq(struct subchannel *sch)
+{
+	struct eadm_private *private = get_eadm_private(sch);
+	struct eadm_scsw *scsw = &sch->schib.scsw.eadm;
+	struct irb *irb = (struct irb *)&S390_lowcore.irb;
+	int error = 0;
+
+	EADM_LOG(6, "irq");
+	EADM_LOG_HEX(6, irb, sizeof(*irb));
+
+	kstat_cpu(smp_processor_id()).irqs[IOINT_ADM]++;
+
+	if ((scsw->stctl & (SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND))
+	    && scsw->eswf == 1 && irb->esw.eadm.erw.r)
+		error = -EIO;
+
+	if (scsw->fctl & SCSW_FCTL_CLEAR_FUNC)
+		error = -ETIMEDOUT;
+
+	eadm_subchannel_set_timeout(sch, 0);
+
+	if (private->state != EADM_BUSY) {
+		EADM_LOG(1, "irq unsol");
+		EADM_LOG_HEX(1, irb, sizeof(*irb));
+		private->state = EADM_NOT_OPER;
+		css_sched_sch_todo(sch, SCH_TODO_EVAL);
+		return;
+	}
+	scm_irq_handler((struct aob *)(unsigned long)scsw->aob, error);
+	private->state = EADM_IDLE;
+}
+
+static struct subchannel *eadm_get_idle_sch(void)
+{
+	struct eadm_private *private;
+	struct subchannel *sch;
+	unsigned long flags;
+
+	spin_lock_irqsave(&list_lock, flags);
+	list_for_each_entry(private, &eadm_list, head) {
+		sch = private->sch;
+		spin_lock(sch->lock);
+		if (private->state == EADM_IDLE) {
+			private->state = EADM_BUSY;
+			list_move_tail(&private->head, &eadm_list);
+			spin_unlock(sch->lock);
+			spin_unlock_irqrestore(&list_lock, flags);
+
+			return sch;
+		}
+		spin_unlock(sch->lock);
+	}
+	spin_unlock_irqrestore(&list_lock, flags);
+
+	return NULL;
+}
+
+static int eadm_start_aob(struct aob *aob)
+{
+	struct eadm_private *private;
+	struct subchannel *sch;
+	unsigned long flags;
+	int ret;
+
+	sch = eadm_get_idle_sch();
+	if (!sch)
+		return -EBUSY;
+
+	spin_lock_irqsave(sch->lock, flags);
+	eadm_subchannel_set_timeout(sch, EADM_TIMEOUT);
+	ret = eadm_subchannel_start(sch, aob);
+	if (!ret)
+		goto out_unlock;
+
+	/* Handle start subchannel failure. */
+	eadm_subchannel_set_timeout(sch, 0);
+	private = get_eadm_private(sch);
+	private->state = EADM_NOT_OPER;
+	css_sched_sch_todo(sch, SCH_TODO_EVAL);
+
+out_unlock:
+	spin_unlock_irqrestore(sch->lock, flags);
+
+	return ret;
+}
+
+static int eadm_subchannel_probe(struct subchannel *sch)
+{
+	struct eadm_private *private;
+	int ret;
+
+	private = kzalloc(sizeof(*private), GFP_KERNEL | GFP_DMA);
+	if (!private)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&private->head);
+	init_timer(&private->timer);
+
+	spin_lock_irq(sch->lock);
+	set_eadm_private(sch, private);
+	private->state = EADM_IDLE;
+	private->sch = sch;
+	sch->isc = EADM_SCH_ISC;
+	ret = cio_enable_subchannel(sch, (u32)(unsigned long)sch);
+	if (ret) {
+		set_eadm_private(sch, NULL);
+		spin_unlock_irq(sch->lock);
+		kfree(private);
+		goto out;
+	}
+	spin_unlock_irq(sch->lock);
+
+	spin_lock_irq(&list_lock);
+	list_add(&private->head, &eadm_list);
+	spin_unlock_irq(&list_lock);
+
+	if (dev_get_uevent_suppress(&sch->dev)) {
+		dev_set_uevent_suppress(&sch->dev, 0);
+		kobject_uevent(&sch->dev.kobj, KOBJ_ADD);
+	}
+out:
+	return ret;
+}
+
+static void eadm_quiesce(struct subchannel *sch)
+{
+	int ret;
+
+	do {
+		spin_lock_irq(sch->lock);
+		ret = cio_disable_subchannel(sch);
+		spin_unlock_irq(sch->lock);
+	} while (ret == -EBUSY);
+}
+
+static int eadm_subchannel_remove(struct subchannel *sch)
+{
+	struct eadm_private *private = get_eadm_private(sch);
+
+	spin_lock_irq(&list_lock);
+	list_del(&private->head);
+	spin_unlock_irq(&list_lock);
+
+	eadm_quiesce(sch);
+
+	spin_lock_irq(sch->lock);
+	set_eadm_private(sch, NULL);
+	spin_unlock_irq(sch->lock);
+
+	kfree(private);
+
+	return 0;
+}
+
+static void eadm_subchannel_shutdown(struct subchannel *sch)
+{
+	eadm_quiesce(sch);
+}
+
+static int eadm_subchannel_freeze(struct subchannel *sch)
+{
+	return cio_disable_subchannel(sch);
+}
+
+static int eadm_subchannel_restore(struct subchannel *sch)
+{
+	return cio_enable_subchannel(sch, (u32)(unsigned long)sch);
+}
+
+/**
+ * eadm_subchannel_sch_event - process subchannel event
+ * @sch: subchannel
+ * @process: non-zero if function is called in process context
+ *
+ * An unspecified event occurred for this subchannel. Adjust data according
+ * to the current operational state of the subchannel. Return zero when the
+ * event has been handled sufficiently or -EAGAIN when this function should
+ * be called again in process context.
+ */
+static int eadm_subchannel_sch_event(struct subchannel *sch, int process)
+{
+	struct eadm_private *private;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(sch->lock, flags);
+	if (!device_is_registered(&sch->dev))
+		goto out_unlock;
+
+	if (work_pending(&sch->todo_work))
+		goto out_unlock;
+
+	if (cio_update_schib(sch)) {
+		css_sched_sch_todo(sch, SCH_TODO_UNREG);
+		goto out_unlock;
+	}
+	private = get_eadm_private(sch);
+	if (private->state == EADM_NOT_OPER)
+		private->state = EADM_IDLE;
+
+out_unlock:
+	spin_unlock_irqrestore(sch->lock, flags);
+
+	return ret;
+}
+
+static struct css_device_id eadm_subchannel_ids[] = {
+	{ .match_flags = 0x1, .type = SUBCHANNEL_TYPE_ADM, },
+	{ /* end of list */ },
+};
+MODULE_DEVICE_TABLE(css, eadm_subchannel_ids);
+
+static struct css_driver eadm_subchannel_driver = {
+	.drv = {
+		.name = "eadm_subchannel",
+		.owner = THIS_MODULE,
+	},
+	.subchannel_type = eadm_subchannel_ids,
+	.irq = eadm_subchannel_irq,
+	.probe = eadm_subchannel_probe,
+	.remove = eadm_subchannel_remove,
+	.shutdown = eadm_subchannel_shutdown,
+	.sch_event = eadm_subchannel_sch_event,
+	.freeze = eadm_subchannel_freeze,
+	.thaw = eadm_subchannel_restore,
+	.restore = eadm_subchannel_restore,
+};
+
+static struct eadm_ops eadm_ops = {
+	.eadm_start = eadm_start_aob,
+	.owner = THIS_MODULE,
+};
+
+static int __init eadm_sch_init(void)
+{
+	int ret;
+
+	if (!css_general_characteristics.eadm)
+		return -ENXIO;
+
+	eadm_debug = debug_register("eadm_log", 16, 1, 16);
+	if (!eadm_debug)
+		return -ENOMEM;
+
+	debug_register_view(eadm_debug, &debug_hex_ascii_view);
+	debug_set_level(eadm_debug, 2);
+
+	isc_register(EADM_SCH_ISC);
+	ret = css_driver_register(&eadm_subchannel_driver);
+	if (ret)
+		goto cleanup;
+
+	register_eadm_ops(&eadm_ops);
+	return ret;
+
+cleanup:
+	isc_unregister(EADM_SCH_ISC);
+	debug_unregister(eadm_debug);
+	return ret;
+}
+
+static void __exit eadm_sch_exit(void)
+{
+	unregister_eadm_ops(&eadm_ops);
+	css_driver_unregister(&eadm_subchannel_driver);
+	isc_unregister(EADM_SCH_ISC);
+	debug_unregister(eadm_debug);
+}
+module_init(eadm_sch_init);
+module_exit(eadm_sch_exit);
diff --git a/drivers/s390/cio/eadm_sch.h b/drivers/s390/cio/eadm_sch.h
new file mode 100644
index 000000000000..2779be093982
--- /dev/null
+++ b/drivers/s390/cio/eadm_sch.h
@@ -0,0 +1,20 @@
+#ifndef EADM_SCH_H
+#define EADM_SCH_H
+
+#include <linux/device.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include "orb.h"
+
+struct eadm_private {
+	union orb orb;
+	enum {EADM_IDLE, EADM_BUSY, EADM_NOT_OPER} state;
+	struct timer_list timer;
+	struct list_head head;
+	struct subchannel *sch;
+} __aligned(8);
+
+#define get_eadm_private(n) ((struct eadm_private *)dev_get_drvdata(&n->dev))
+#define set_eadm_private(n, p) (dev_set_drvdata(&n->dev, p))
+
+#endif
diff --git a/drivers/s390/cio/orb.h b/drivers/s390/cio/orb.h
index 45a9865c2b36..7a640530e7f5 100644
--- a/drivers/s390/cio/orb.h
+++ b/drivers/s390/cio/orb.h
@@ -59,9 +59,33 @@ struct tm_orb {
 	u32:32;
 }  __packed __aligned(4);
 
+/*
+ * eadm operation request block
+ */
+struct eadm_orb {
+	u32 intparm;
+	u32 key:4;
+	u32:4;
+	u32 compat1:1;
+	u32 compat2:1;
+	u32:21;
+	u32 x:1;
+	u32 aob;
+	u32 css_prio:8;
+	u32:8;
+	u32 scm_prio:8;
+	u32:8;
+	u32:29;
+	u32 fmt:3;
+	u32:32;
+	u32:32;
+	u32:32;
+}  __packed __aligned(4);
+
 union orb {
 	struct cmd_orb cmd;
 	struct tm_orb tm;
+	struct eadm_orb eadm;
 }  __packed __aligned(4);
 
 #endif /* S390_ORB_H */
diff --git a/drivers/s390/cio/qdio_debug.h b/drivers/s390/cio/qdio_debug.h
index e1f646800ddb..7f8b973da298 100644
--- a/drivers/s390/cio/qdio_debug.h
+++ b/drivers/s390/cio/qdio_debug.h
@@ -37,10 +37,14 @@ static inline int qdio_dbf_passes(debug_info_t *dbf_grp, int level)
 		debug_text_event(qdio_dbf_setup, DBF_ERR, debug_buffer); \
 	} while (0)
 
-#define DBF_HEX(addr, len) \
-	do { \
-		debug_event(qdio_dbf_setup, DBF_ERR, (void*)(addr), len); \
-	} while (0)
+static inline void DBF_HEX(void *addr, int len)
+{
+	while (len > 0) {
+		debug_event(qdio_dbf_setup, DBF_ERR, addr, len);
+		len -= qdio_dbf_setup->buf_size;
+		addr += qdio_dbf_setup->buf_size;
+	}
+}
 
 #define DBF_ERROR(text...) \
 	do { \
@@ -49,11 +53,14 @@ static inline int qdio_dbf_passes(debug_info_t *dbf_grp, int level)
 		debug_text_event(qdio_dbf_error, DBF_ERR, debug_buffer); \
 	} while (0)
 
-#define DBF_ERROR_HEX(addr, len) \
-	do { \
-		debug_event(qdio_dbf_error, DBF_ERR, (void*)(addr), len); \
-	} while (0)
-
+static inline void DBF_ERROR_HEX(void *addr, int len)
+{
+	while (len > 0) {
+		debug_event(qdio_dbf_error, DBF_ERR, addr, len);
+		len -= qdio_dbf_error->buf_size;
+		addr += qdio_dbf_error->buf_size;
+	}
+}
 
 #define DBF_DEV_EVENT(level, device, text...) \
 	do { \
@@ -64,10 +71,15 @@ static inline int qdio_dbf_passes(debug_info_t *dbf_grp, int level)
 		} \
 	} while (0)
 
-#define DBF_DEV_HEX(level, device, addr, len) \
-	do { \
-		debug_event(device->debug_area, level, (void*)(addr), len); \
-	} while (0)
+static inline void DBF_DEV_HEX(struct qdio_irq *dev, void *addr,
+			       int len, int level)
+{
+	while (len > 0) {
+		debug_event(dev->debug_area, level, addr, len);
+		len -= dev->debug_area->buf_size;
+		addr += dev->debug_area->buf_size;
+	}
+}
 
 void qdio_allocate_dbf(struct qdio_initialize *init_data,
 		       struct qdio_irq *irq_ptr);
diff --git a/drivers/s390/cio/scm.c b/drivers/s390/cio/scm.c
new file mode 100644
index 000000000000..bcf20f3aa51b
--- /dev/null
+++ b/drivers/s390/cio/scm.c
@@ -0,0 +1,317 @@
+/*
+ * Recognize and maintain s390 storage class memory.
+ *
+ * Copyright IBM Corp. 2012
+ * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <asm/eadm.h>
+#include "chsc.h"
+
+static struct device *scm_root;
+static struct eadm_ops *eadm_ops;
+static DEFINE_MUTEX(eadm_ops_mutex);
+
+#define to_scm_dev(n) container_of(n, struct scm_device, dev)
+#define	to_scm_drv(d) container_of(d, struct scm_driver, drv)
+
+static int scmdev_probe(struct device *dev)
+{
+	struct scm_device *scmdev = to_scm_dev(dev);
+	struct scm_driver *scmdrv = to_scm_drv(dev->driver);
+
+	return scmdrv->probe ? scmdrv->probe(scmdev) : -ENODEV;
+}
+
+static int scmdev_remove(struct device *dev)
+{
+	struct scm_device *scmdev = to_scm_dev(dev);
+	struct scm_driver *scmdrv = to_scm_drv(dev->driver);
+
+	return scmdrv->remove ? scmdrv->remove(scmdev) : -ENODEV;
+}
+
+static int scmdev_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	return add_uevent_var(env, "MODALIAS=scm:scmdev");
+}
+
+static struct bus_type scm_bus_type = {
+	.name  = "scm",
+	.probe = scmdev_probe,
+	.remove = scmdev_remove,
+	.uevent = scmdev_uevent,
+};
+
+/**
+ * scm_driver_register() - register a scm driver
+ * @scmdrv: driver to be registered
+ */
+int scm_driver_register(struct scm_driver *scmdrv)
+{
+	struct device_driver *drv = &scmdrv->drv;
+
+	drv->bus = &scm_bus_type;
+
+	return driver_register(drv);
+}
+EXPORT_SYMBOL_GPL(scm_driver_register);
+
+/**
+ * scm_driver_unregister() - deregister a scm driver
+ * @scmdrv: driver to be deregistered
+ */
+void scm_driver_unregister(struct scm_driver *scmdrv)
+{
+	driver_unregister(&scmdrv->drv);
+}
+EXPORT_SYMBOL_GPL(scm_driver_unregister);
+
+int scm_get_ref(void)
+{
+	int ret = 0;
+
+	mutex_lock(&eadm_ops_mutex);
+	if (!eadm_ops || !try_module_get(eadm_ops->owner))
+		ret = -ENOENT;
+	mutex_unlock(&eadm_ops_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(scm_get_ref);
+
+void scm_put_ref(void)
+{
+	mutex_lock(&eadm_ops_mutex);
+	module_put(eadm_ops->owner);
+	mutex_unlock(&eadm_ops_mutex);
+}
+EXPORT_SYMBOL_GPL(scm_put_ref);
+
+void register_eadm_ops(struct eadm_ops *ops)
+{
+	mutex_lock(&eadm_ops_mutex);
+	eadm_ops = ops;
+	mutex_unlock(&eadm_ops_mutex);
+}
+EXPORT_SYMBOL_GPL(register_eadm_ops);
+
+void unregister_eadm_ops(struct eadm_ops *ops)
+{
+	mutex_lock(&eadm_ops_mutex);
+	eadm_ops = NULL;
+	mutex_unlock(&eadm_ops_mutex);
+}
+EXPORT_SYMBOL_GPL(unregister_eadm_ops);
+
+int scm_start_aob(struct aob *aob)
+{
+	return eadm_ops->eadm_start(aob);
+}
+EXPORT_SYMBOL_GPL(scm_start_aob);
+
+void scm_irq_handler(struct aob *aob, int error)
+{
+	struct aob_rq_header *aobrq = (void *) aob->request.data;
+	struct scm_device *scmdev = aobrq->scmdev;
+	struct scm_driver *scmdrv = to_scm_drv(scmdev->dev.driver);
+
+	scmdrv->handler(scmdev, aobrq->data, error);
+}
+EXPORT_SYMBOL_GPL(scm_irq_handler);
+
+#define scm_attr(name)							\
+static ssize_t show_##name(struct device *dev,				\
+	       struct device_attribute *attr, char *buf)		\
+{									\
+	struct scm_device *scmdev = to_scm_dev(dev);			\
+	int ret;							\
+									\
+	device_lock(dev);						\
+	ret = sprintf(buf, "%u\n", scmdev->attrs.name);			\
+	device_unlock(dev);						\
+									\
+	return ret;							\
+}									\
+static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL);
+
+scm_attr(persistence);
+scm_attr(oper_state);
+scm_attr(data_state);
+scm_attr(rank);
+scm_attr(release);
+scm_attr(res_id);
+
+static struct attribute *scmdev_attrs[] = {
+	&dev_attr_persistence.attr,
+	&dev_attr_oper_state.attr,
+	&dev_attr_data_state.attr,
+	&dev_attr_rank.attr,
+	&dev_attr_release.attr,
+	&dev_attr_res_id.attr,
+	NULL,
+};
+
+static struct attribute_group scmdev_attr_group = {
+	.attrs = scmdev_attrs,
+};
+
+static const struct attribute_group *scmdev_attr_groups[] = {
+	&scmdev_attr_group,
+	NULL,
+};
+
+static void scmdev_release(struct device *dev)
+{
+	struct scm_device *scmdev = to_scm_dev(dev);
+
+	kfree(scmdev);
+}
+
+static void scmdev_setup(struct scm_device *scmdev, struct sale *sale,
+			 unsigned int size, unsigned int max_blk_count)
+{
+	dev_set_name(&scmdev->dev, "%016llx", (unsigned long long) sale->sa);
+	scmdev->nr_max_block = max_blk_count;
+	scmdev->address = sale->sa;
+	scmdev->size = 1UL << size;
+	scmdev->attrs.rank = sale->rank;
+	scmdev->attrs.persistence = sale->p;
+	scmdev->attrs.oper_state = sale->op_state;
+	scmdev->attrs.data_state = sale->data_state;
+	scmdev->attrs.rank = sale->rank;
+	scmdev->attrs.release = sale->r;
+	scmdev->attrs.res_id = sale->rid;
+	scmdev->dev.parent = scm_root;
+	scmdev->dev.bus = &scm_bus_type;
+	scmdev->dev.release = scmdev_release;
+	scmdev->dev.groups = scmdev_attr_groups;
+}
+
+/*
+ * Check for state-changes, notify the driver and userspace.
+ */
+static void scmdev_update(struct scm_device *scmdev, struct sale *sale)
+{
+	struct scm_driver *scmdrv;
+	bool changed;
+
+	device_lock(&scmdev->dev);
+	changed = scmdev->attrs.rank != sale->rank ||
+		  scmdev->attrs.oper_state != sale->op_state;
+	scmdev->attrs.rank = sale->rank;
+	scmdev->attrs.oper_state = sale->op_state;
+	if (!scmdev->dev.driver)
+		goto out;
+	scmdrv = to_scm_drv(scmdev->dev.driver);
+	if (changed && scmdrv->notify)
+		scmdrv->notify(scmdev);
+out:
+	device_unlock(&scmdev->dev);
+	if (changed)
+		kobject_uevent(&scmdev->dev.kobj, KOBJ_CHANGE);
+}
+
+static int check_address(struct device *dev, void *data)
+{
+	struct scm_device *scmdev = to_scm_dev(dev);
+	struct sale *sale = data;
+
+	return scmdev->address == sale->sa;
+}
+
+static struct scm_device *scmdev_find(struct sale *sale)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&scm_bus_type, NULL, sale, check_address);
+
+	return dev ? to_scm_dev(dev) : NULL;
+}
+
+static int scm_add(struct chsc_scm_info *scm_info, size_t num)
+{
+	struct sale *sale, *scmal = scm_info->scmal;
+	struct scm_device *scmdev;
+	int ret;
+
+	for (sale = scmal; sale < scmal + num; sale++) {
+		scmdev = scmdev_find(sale);
+		if (scmdev) {
+			scmdev_update(scmdev, sale);
+			/* Release reference from scm_find(). */
+			put_device(&scmdev->dev);
+			continue;
+		}
+		scmdev = kzalloc(sizeof(*scmdev), GFP_KERNEL);
+		if (!scmdev)
+			return -ENODEV;
+		scmdev_setup(scmdev, sale, scm_info->is, scm_info->mbc);
+		ret = device_register(&scmdev->dev);
+		if (ret) {
+			/* Release reference from device_initialize(). */
+			put_device(&scmdev->dev);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int scm_update_information(void)
+{
+	struct chsc_scm_info *scm_info;
+	u64 token = 0;
+	size_t num;
+	int ret;
+
+	scm_info = (void *)__get_free_page(GFP_KERNEL | GFP_DMA);
+	if (!scm_info)
+		return -ENOMEM;
+
+	do {
+		ret = chsc_scm_info(scm_info, token);
+		if (ret)
+			break;
+
+		num = (scm_info->response.length -
+		       (offsetof(struct chsc_scm_info, scmal) -
+			offsetof(struct chsc_scm_info, response))
+		      ) / sizeof(struct sale);
+
+		ret = scm_add(scm_info, num);
+		if (ret)
+			break;
+
+		token = scm_info->restok;
+	} while (token);
+
+	free_page((unsigned long)scm_info);
+
+	return ret;
+}
+
+static int __init scm_init(void)
+{
+	int ret;
+
+	ret = bus_register(&scm_bus_type);
+	if (ret)
+		return ret;
+
+	scm_root = root_device_register("scm");
+	if (IS_ERR(scm_root)) {
+		bus_unregister(&scm_bus_type);
+		return PTR_ERR(scm_root);
+	}
+
+	scm_update_information();
+	return 0;
+}
+subsys_initcall_sync(scm_init);
diff --git a/drivers/s390/crypto/Makefile b/drivers/s390/crypto/Makefile
index af3c7f16ea88..771faf7094d6 100644
--- a/drivers/s390/crypto/Makefile
+++ b/drivers/s390/crypto/Makefile
@@ -4,4 +4,5 @@
 
 ap-objs := ap_bus.o
 obj-$(CONFIG_ZCRYPT) += ap.o zcrypt_api.o zcrypt_pcicc.o zcrypt_pcixcc.o
-obj-$(CONFIG_ZCRYPT) += zcrypt_pcica.o zcrypt_cex2a.o
+obj-$(CONFIG_ZCRYPT) += zcrypt_pcica.o zcrypt_cex2a.o zcrypt_cex4.o
+obj-$(CONFIG_ZCRYPT) += zcrypt_msgtype6.o zcrypt_msgtype50.o
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index ae258a4b4e5e..7b865a7300e6 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -1,5 +1,5 @@
 /*
- * Copyright IBM Corp. 2006
+ * Copyright IBM Corp. 2006, 2012
  * Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *	      Martin Schwidefsky <schwidefsky@de.ibm.com>
  *	      Ralph Wuerthner <rwuerthn@de.ibm.com>
@@ -62,13 +62,14 @@ static void ap_interrupt_handler(void *unused1, void *unused2);
 static void ap_reset(struct ap_device *ap_dev);
 static void ap_config_timeout(unsigned long ptr);
 static int ap_select_domain(void);
+static void ap_query_configuration(void);
 
 /*
  * Module description.
  */
 MODULE_AUTHOR("IBM Corporation");
-MODULE_DESCRIPTION("Adjunct Processor Bus driver, "
-		   "Copyright IBM Corp. 2006");
+MODULE_DESCRIPTION("Adjunct Processor Bus driver, " \
+		   "Copyright IBM Corp. 2006, 2012");
 MODULE_LICENSE("GPL");
 
 /*
@@ -84,6 +85,7 @@ module_param_named(poll_thread, ap_thread_flag, int, 0000);
 MODULE_PARM_DESC(poll_thread, "Turn on/off poll thread, default is 0 (off).");
 
 static struct device *ap_root_device = NULL;
+static struct ap_config_info *ap_configuration;
 static DEFINE_SPINLOCK(ap_device_list_lock);
 static LIST_HEAD(ap_device_list);
 
@@ -158,6 +160,19 @@ static int ap_interrupts_available(void)
 }
 
 /**
+ * ap_configuration_available(): Test if AP configuration
+ * information is available.
+ *
+ * Returns 1 if AP configuration information is available.
+ */
+#ifdef CONFIG_64BIT
+static int ap_configuration_available(void)
+{
+	return test_facility(2) && test_facility(12);
+}
+#endif
+
+/**
  * ap_test_queue(): Test adjunct processor queue.
  * @qid: The AP queue number
  * @queue_depth: Pointer to queue depth value
@@ -242,6 +257,26 @@ __ap_query_functions(ap_qid_t qid, unsigned int *functions)
 }
 #endif
 
+#ifdef CONFIG_64BIT
+static inline int __ap_query_configuration(struct ap_config_info *config)
+{
+	register unsigned long reg0 asm ("0") = 0x04000000UL;
+	register unsigned long reg1 asm ("1") = -EINVAL;
+	register unsigned char *reg2 asm ("2") = (unsigned char *)config;
+
+	asm volatile(
+		".long 0xb2af0000\n"		/* PQAP(QCI) */
+		"0: la    %1,0\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: "+d" (reg0), "+d" (reg1), "+d" (reg2)
+		:
+		: "cc");
+
+	return reg1;
+}
+#endif
+
 /**
  * ap_query_functions(): Query supported functions.
  * @qid: The AP queue number
@@ -292,25 +327,6 @@ static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
 }
 
 /**
- * ap_4096_commands_availablen(): Check for availability of 4096 bit RSA
- * support.
- * @qid: The AP queue number
- *
- * Returns 1 if 4096 bit RSA keys are support fo the AP, returns 0 if not.
- */
-int ap_4096_commands_available(ap_qid_t qid)
-{
-	unsigned int functions;
-
-	if (ap_query_functions(qid, &functions))
-		return 0;
-
-	return test_ap_facility(functions, 1) &&
-	       test_ap_facility(functions, 2);
-}
-EXPORT_SYMBOL(ap_4096_commands_available);
-
-/**
  * ap_queue_enable_interruption(): Enable interruption on an AP.
  * @qid: The AP queue number
  * @ind: the notification indicator byte
@@ -657,6 +673,34 @@ static ssize_t ap_request_count_show(struct device *dev,
 
 static DEVICE_ATTR(request_count, 0444, ap_request_count_show, NULL);
 
+static ssize_t ap_requestq_count_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+	int rc;
+
+	spin_lock_bh(&ap_dev->lock);
+	rc = snprintf(buf, PAGE_SIZE, "%d\n", ap_dev->requestq_count);
+	spin_unlock_bh(&ap_dev->lock);
+	return rc;
+}
+
+static DEVICE_ATTR(requestq_count, 0444, ap_requestq_count_show, NULL);
+
+static ssize_t ap_pendingq_count_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+	int rc;
+
+	spin_lock_bh(&ap_dev->lock);
+	rc = snprintf(buf, PAGE_SIZE, "%d\n", ap_dev->pendingq_count);
+	spin_unlock_bh(&ap_dev->lock);
+	return rc;
+}
+
+static DEVICE_ATTR(pendingq_count, 0444, ap_pendingq_count_show, NULL);
+
 static ssize_t ap_modalias_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -665,11 +709,23 @@ static ssize_t ap_modalias_show(struct device *dev,
 
 static DEVICE_ATTR(modalias, 0444, ap_modalias_show, NULL);
 
+static ssize_t ap_functions_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+	return snprintf(buf, PAGE_SIZE, "0x%08X\n", ap_dev->functions);
+}
+
+static DEVICE_ATTR(ap_functions, 0444, ap_functions_show, NULL);
+
 static struct attribute *ap_dev_attrs[] = {
 	&dev_attr_hwtype.attr,
 	&dev_attr_depth.attr,
 	&dev_attr_request_count.attr,
+	&dev_attr_requestq_count.attr,
+	&dev_attr_pendingq_count.attr,
 	&dev_attr_modalias.attr,
+	&dev_attr_ap_functions.attr,
 	NULL
 };
 static struct attribute_group ap_dev_attr_group = {
@@ -772,6 +828,7 @@ static int ap_bus_resume(struct device *dev)
 		ap_suspend_flag = 0;
 		if (!ap_interrupts_available())
 			ap_interrupt_indicator = NULL;
+		ap_query_configuration();
 		if (!user_set_domain) {
 			ap_domain_index = -1;
 			ap_select_domain();
@@ -895,6 +952,20 @@ void ap_driver_unregister(struct ap_driver *ap_drv)
 }
 EXPORT_SYMBOL(ap_driver_unregister);
 
+void ap_bus_force_rescan(void)
+{
+	/* Delete the AP bus rescan timer. */
+	del_timer(&ap_config_timer);
+
+	/* processing a synchonuous bus rescan */
+	ap_scan_bus(NULL);
+
+	/* Setup the AP bus rescan timer again. */
+	ap_config_timer.expires = jiffies + ap_config_time * HZ;
+	add_timer(&ap_config_timer);
+}
+EXPORT_SYMBOL(ap_bus_force_rescan);
+
 /*
  * AP bus attributes.
  */
@@ -997,6 +1068,65 @@ static struct bus_attribute *const ap_bus_attrs[] = {
 	NULL,
 };
 
+static inline int ap_test_config(unsigned int *field, unsigned int nr)
+{
+	if (nr > 0xFFu)
+		return 0;
+	return ap_test_bit((field + (nr >> 5)), (nr & 0x1f));
+}
+
+/*
+ * ap_test_config_card_id(): Test, whether an AP card ID is configured.
+ * @id AP card ID
+ *
+ * Returns 0 if the card is not configured
+ *	   1 if the card is configured or
+ *	     if the configuration information is not available
+ */
+static inline int ap_test_config_card_id(unsigned int id)
+{
+	if (!ap_configuration)
+		return 1;
+	return ap_test_config(ap_configuration->apm, id);
+}
+
+/*
+ * ap_test_config_domain(): Test, whether an AP usage domain is configured.
+ * @domain AP usage domain ID
+ *
+ * Returns 0 if the usage domain is not configured
+ *	   1 if the usage domain is configured or
+ *	     if the configuration information is not available
+ */
+static inline int ap_test_config_domain(unsigned int domain)
+{
+	if (!ap_configuration)
+		return 1;
+	return ap_test_config(ap_configuration->aqm, domain);
+}
+
+/**
+ * ap_query_configuration(): Query AP configuration information.
+ *
+ * Query information of installed cards and configured domains from AP.
+ */
+static void ap_query_configuration(void)
+{
+#ifdef CONFIG_64BIT
+	if (ap_configuration_available()) {
+		if (!ap_configuration)
+			ap_configuration =
+				kzalloc(sizeof(struct ap_config_info),
+					GFP_KERNEL);
+		if (ap_configuration)
+			__ap_query_configuration(ap_configuration);
+	} else
+		ap_configuration = NULL;
+#else
+	ap_configuration = NULL;
+#endif
+}
+
 /**
  * ap_select_domain(): Select an AP domain.
  *
@@ -1005,6 +1135,7 @@ static struct bus_attribute *const ap_bus_attrs[] = {
 static int ap_select_domain(void)
 {
 	int queue_depth, device_type, count, max_count, best_domain;
+	ap_qid_t qid;
 	int rc, i, j;
 
 	/*
@@ -1018,9 +1149,13 @@ static int ap_select_domain(void)
 	best_domain = -1;
 	max_count = 0;
 	for (i = 0; i < AP_DOMAINS; i++) {
+		if (!ap_test_config_domain(i))
+			continue;
 		count = 0;
 		for (j = 0; j < AP_DEVICES; j++) {
-			ap_qid_t qid = AP_MKQID(j, i);
+			if (!ap_test_config_card_id(j))
+				continue;
+			qid = AP_MKQID(j, i);
 			rc = ap_query_queue(qid, &queue_depth, &device_type);
 			if (rc)
 				continue;
@@ -1169,6 +1304,7 @@ static void ap_scan_bus(struct work_struct *unused)
 	unsigned int device_functions;
 	int rc, i;
 
+	ap_query_configuration();
 	if (ap_select_domain() != 0)
 		return;
 	for (i = 0; i < AP_DEVICES; i++) {
@@ -1176,7 +1312,10 @@ static void ap_scan_bus(struct work_struct *unused)
 		dev = bus_find_device(&ap_bus_type, NULL,
 				      (void *)(unsigned long)qid,
 				      __ap_scan_bus);
-		rc = ap_query_queue(qid, &queue_depth, &device_type);
+		if (ap_test_config_card_id(i))
+			rc = ap_query_queue(qid, &queue_depth, &device_type);
+		else
+			rc = -ENODEV;
 		if (dev) {
 			if (rc == -EBUSY) {
 				set_current_state(TASK_UNINTERRUPTIBLE);
@@ -1217,29 +1356,22 @@ static void ap_scan_bus(struct work_struct *unused)
 			    (unsigned long) ap_dev);
 		switch (device_type) {
 		case 0:
+			/* device type probing for old cards */
 			if (ap_probe_device_type(ap_dev)) {
 				kfree(ap_dev);
 				continue;
 			}
 			break;
-		case 10:
-			if (ap_query_functions(qid, &device_functions)) {
-				kfree(ap_dev);
-				continue;
-			}
-			if (test_ap_facility(device_functions, 3))
-				ap_dev->device_type = AP_DEVICE_TYPE_CEX3C;
-			else if (test_ap_facility(device_functions, 4))
-				ap_dev->device_type = AP_DEVICE_TYPE_CEX3A;
-			else {
-				kfree(ap_dev);
-				continue;
-			}
-			break;
 		default:
 			ap_dev->device_type = device_type;
 		}
 
+		rc = ap_query_functions(qid, &device_functions);
+		if (!rc)
+			ap_dev->functions = device_functions;
+		else
+			ap_dev->functions = 0u;
+
 		ap_dev->device.bus = &ap_bus_type;
 		ap_dev->device.parent = ap_root_device;
 		if (dev_set_name(&ap_dev->device, "card%02x",
@@ -1785,6 +1917,7 @@ int __init ap_module_init(void)
 		goto out_root;
 	}
 
+	ap_query_configuration();
 	if (ap_select_domain() == 0)
 		ap_scan_bus(NULL);
 
diff --git a/drivers/s390/crypto/ap_bus.h b/drivers/s390/crypto/ap_bus.h
index 52d61995af88..685f6cc022f9 100644
--- a/drivers/s390/crypto/ap_bus.h
+++ b/drivers/s390/crypto/ap_bus.h
@@ -1,5 +1,5 @@
 /*
- * Copyright IBM Corp. 2006
+ * Copyright IBM Corp. 2006, 2012
  * Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *	      Martin Schwidefsky <schwidefsky@de.ibm.com>
  *	      Ralph Wuerthner <rwuerthn@de.ibm.com>
@@ -83,13 +83,12 @@ int ap_queue_status_invalid_test(struct ap_queue_status *status)
 	return !(memcmp(status, &invalid, sizeof(struct ap_queue_status)));
 }
 
-#define MAX_AP_FACILITY 31
-
-static inline int test_ap_facility(unsigned int function, unsigned int nr)
+#define AP_MAX_BITS 31
+static inline int ap_test_bit(unsigned int *ptr, unsigned int nr)
 {
-	if (nr > MAX_AP_FACILITY)
+	if (nr > AP_MAX_BITS)
 		return 0;
-	return function & (unsigned int)(0x80000000 >> nr);
+	return (*ptr & (0x80000000u >> nr)) != 0;
 }
 
 #define AP_RESPONSE_NORMAL		0x00
@@ -117,6 +116,15 @@ static inline int test_ap_facility(unsigned int function, unsigned int nr)
 #define AP_DEVICE_TYPE_CEX2C	7
 #define AP_DEVICE_TYPE_CEX3A	8
 #define AP_DEVICE_TYPE_CEX3C	9
+#define AP_DEVICE_TYPE_CEX4	10
+
+/*
+ * Known function facilities
+ */
+#define AP_FUNC_MEX4K 1
+#define AP_FUNC_CRT4K 2
+#define AP_FUNC_COPRO 3
+#define AP_FUNC_ACCEL 4
 
 /*
  * AP reset flag states
@@ -151,6 +159,7 @@ struct ap_device {
 	ap_qid_t qid;			/* AP queue id. */
 	int queue_depth;		/* AP queue depth.*/
 	int device_type;		/* AP device type. */
+	unsigned int functions;		/* AP device function bitfield. */
 	int unregistered;		/* marks AP device as unregistered */
 	struct timer_list timeout;	/* Timer for request timeouts. */
 	int reset;			/* Reset required after req. timeout. */
@@ -183,6 +192,17 @@ struct ap_message {
 			struct ap_message *);
 };
 
+struct ap_config_info {
+	unsigned int special_command:1;
+	unsigned int ap_extended:1;
+	unsigned char reserved1:6;
+	unsigned char reserved2[15];
+	unsigned int apm[8];		/* AP ID mask */
+	unsigned int aqm[8];		/* AP queue mask */
+	unsigned int adm[8];		/* AP domain mask */
+	unsigned char reserved4[16];
+} __packed;
+
 #define AP_DEVICE(dt)					\
 	.dev_type=(dt),					\
 	.match_flags=AP_DEVICE_ID_MATCH_DEVICE_TYPE,
@@ -211,10 +231,9 @@ int ap_recv(ap_qid_t, unsigned long long *, void *, size_t);
 void ap_queue_message(struct ap_device *ap_dev, struct ap_message *ap_msg);
 void ap_cancel_message(struct ap_device *ap_dev, struct ap_message *ap_msg);
 void ap_flush_queue(struct ap_device *ap_dev);
+void ap_bus_force_rescan(void);
 
 int ap_module_init(void);
 void ap_module_exit(void);
 
-int ap_4096_commands_available(ap_qid_t qid);
-
 #endif /* _AP_BUS_H_ */
diff --git a/drivers/s390/crypto/zcrypt_api.c b/drivers/s390/crypto/zcrypt_api.c
index 2f94132246a1..31cfaa556072 100644
--- a/drivers/s390/crypto/zcrypt_api.c
+++ b/drivers/s390/crypto/zcrypt_api.c
@@ -1,7 +1,7 @@
 /*
  *  zcrypt 2.1.0
  *
- *  Copyright IBM Corp. 2001, 2006
+ *  Copyright IBM Corp. 2001, 2012
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *	       Cornelia Huck <cornelia.huck@de.ibm.com>
@@ -9,6 +9,7 @@
  *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
  *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
  *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.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
@@ -37,25 +38,39 @@
 #include <linux/atomic.h>
 #include <asm/uaccess.h>
 #include <linux/hw_random.h>
+#include <linux/debugfs.h>
+#include <asm/debug.h>
 
+#include "zcrypt_debug.h"
 #include "zcrypt_api.h"
 
 /*
  * Module description.
  */
 MODULE_AUTHOR("IBM Corporation");
-MODULE_DESCRIPTION("Cryptographic Coprocessor interface, "
-		   "Copyright IBM Corp. 2001, 2006");
+MODULE_DESCRIPTION("Cryptographic Coprocessor interface, " \
+		   "Copyright IBM Corp. 2001, 2012");
 MODULE_LICENSE("GPL");
 
 static DEFINE_SPINLOCK(zcrypt_device_lock);
 static LIST_HEAD(zcrypt_device_list);
 static int zcrypt_device_count = 0;
 static atomic_t zcrypt_open_count = ATOMIC_INIT(0);
+static atomic_t zcrypt_rescan_count = ATOMIC_INIT(0);
+
+atomic_t zcrypt_rescan_req = ATOMIC_INIT(0);
+EXPORT_SYMBOL(zcrypt_rescan_req);
 
 static int zcrypt_rng_device_add(void);
 static void zcrypt_rng_device_remove(void);
 
+static DEFINE_SPINLOCK(zcrypt_ops_list_lock);
+static LIST_HEAD(zcrypt_ops_list);
+
+static debug_info_t *zcrypt_dbf_common;
+static debug_info_t *zcrypt_dbf_devices;
+static struct dentry *debugfs_root;
+
 /*
  * Device attributes common for all crypto devices.
  */
@@ -85,6 +100,8 @@ static ssize_t zcrypt_online_store(struct device *dev,
 	if (sscanf(buf, "%d\n", &online) != 1 || online < 0 || online > 1)
 		return -EINVAL;
 	zdev->online = online;
+	ZCRYPT_DBF_DEV(DBF_INFO, zdev, "dev%04xo%dman", zdev->ap_dev->qid,
+		       zdev->online);
 	if (!online)
 		ap_flush_queue(zdev->ap_dev);
 	return count;
@@ -103,6 +120,24 @@ static struct attribute_group zcrypt_device_attr_group = {
 };
 
 /**
+ * Process a rescan of the transport layer.
+ *
+ * Returns 1, if the rescan has been processed, otherwise 0.
+ */
+static inline int zcrypt_process_rescan(void)
+{
+	if (atomic_read(&zcrypt_rescan_req)) {
+		atomic_set(&zcrypt_rescan_req, 0);
+		atomic_inc(&zcrypt_rescan_count);
+		ap_bus_force_rescan();
+		ZCRYPT_DBF_COMMON(DBF_INFO, "rescan%07d",
+				  atomic_inc_return(&zcrypt_rescan_count));
+		return 1;
+	}
+	return 0;
+}
+
+/**
  * __zcrypt_increase_preference(): Increase preference of a crypto device.
  * @zdev: Pointer the crypto device
  *
@@ -190,6 +225,7 @@ struct zcrypt_device *zcrypt_device_alloc(size_t max_response_size)
 	zdev->reply.length = max_response_size;
 	spin_lock_init(&zdev->lock);
 	INIT_LIST_HEAD(&zdev->list);
+	zdev->dbf_area = zcrypt_dbf_devices;
 	return zdev;
 
 out_free:
@@ -215,6 +251,8 @@ int zcrypt_device_register(struct zcrypt_device *zdev)
 {
 	int rc;
 
+	if (!zdev->ops)
+		return -ENODEV;
 	rc = sysfs_create_group(&zdev->ap_dev->device.kobj,
 				&zcrypt_device_attr_group);
 	if (rc)
@@ -223,6 +261,8 @@ int zcrypt_device_register(struct zcrypt_device *zdev)
 	kref_init(&zdev->refcount);
 	spin_lock_bh(&zcrypt_device_lock);
 	zdev->online = 1;	/* New devices are online by default. */
+	ZCRYPT_DBF_DEV(DBF_INFO, zdev, "dev%04xo%dreg", zdev->ap_dev->qid,
+		       zdev->online);
 	list_add_tail(&zdev->list, &zcrypt_device_list);
 	__zcrypt_increase_preference(zdev);
 	zcrypt_device_count++;
@@ -269,6 +309,67 @@ void zcrypt_device_unregister(struct zcrypt_device *zdev)
 }
 EXPORT_SYMBOL(zcrypt_device_unregister);
 
+void zcrypt_msgtype_register(struct zcrypt_ops *zops)
+{
+	if (zops->owner) {
+		spin_lock_bh(&zcrypt_ops_list_lock);
+		list_add_tail(&zops->list, &zcrypt_ops_list);
+		spin_unlock_bh(&zcrypt_ops_list_lock);
+	}
+}
+EXPORT_SYMBOL(zcrypt_msgtype_register);
+
+void zcrypt_msgtype_unregister(struct zcrypt_ops *zops)
+{
+	spin_lock_bh(&zcrypt_ops_list_lock);
+	list_del_init(&zops->list);
+	spin_unlock_bh(&zcrypt_ops_list_lock);
+}
+EXPORT_SYMBOL(zcrypt_msgtype_unregister);
+
+static inline
+struct zcrypt_ops *__ops_lookup(unsigned char *name, int variant)
+{
+	struct zcrypt_ops *zops;
+	int found = 0;
+
+	spin_lock_bh(&zcrypt_ops_list_lock);
+	list_for_each_entry(zops, &zcrypt_ops_list, list) {
+		if ((zops->variant == variant) &&
+		    (!strncmp(zops->owner->name, name, MODULE_NAME_LEN))) {
+			found = 1;
+			break;
+		}
+	}
+	spin_unlock_bh(&zcrypt_ops_list_lock);
+
+	if (!found)
+		return NULL;
+	return zops;
+}
+
+struct zcrypt_ops *zcrypt_msgtype_request(unsigned char *name, int variant)
+{
+	struct zcrypt_ops *zops = NULL;
+
+	zops = __ops_lookup(name, variant);
+	if (!zops) {
+		request_module(name);
+		zops = __ops_lookup(name, variant);
+	}
+	if ((!zops) || (!try_module_get(zops->owner)))
+		return NULL;
+	return zops;
+}
+EXPORT_SYMBOL(zcrypt_msgtype_request);
+
+void zcrypt_msgtype_release(struct zcrypt_ops *zops)
+{
+	if (zops)
+		module_put(zops->owner);
+}
+EXPORT_SYMBOL(zcrypt_msgtype_release);
+
 /**
  * zcrypt_read (): Not supported beyond zcrypt 1.3.1.
  *
@@ -640,6 +741,11 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 		do {
 			rc = zcrypt_rsa_modexpo(&mex);
 		} while (rc == -EAGAIN);
+		/* on failure: retry once again after a requested rescan */
+		if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+			do {
+				rc = zcrypt_rsa_modexpo(&mex);
+			} while (rc == -EAGAIN);
 		if (rc)
 			return rc;
 		return put_user(mex.outputdatalength, &umex->outputdatalength);
@@ -652,6 +758,11 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 		do {
 			rc = zcrypt_rsa_crt(&crt);
 		} while (rc == -EAGAIN);
+		/* on failure: retry once again after a requested rescan */
+		if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+			do {
+				rc = zcrypt_rsa_crt(&crt);
+			} while (rc == -EAGAIN);
 		if (rc)
 			return rc;
 		return put_user(crt.outputdatalength, &ucrt->outputdatalength);
@@ -664,6 +775,11 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 		do {
 			rc = zcrypt_send_cprb(&xcRB);
 		} while (rc == -EAGAIN);
+		/* on failure: retry once again after a requested rescan */
+		if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+			do {
+				rc = zcrypt_send_cprb(&xcRB);
+			} while (rc == -EAGAIN);
 		if (copy_to_user(uxcRB, &xcRB, sizeof(xcRB)))
 			return -EFAULT;
 		return rc;
@@ -770,10 +886,15 @@ static long trans_modexpo32(struct file *filp, unsigned int cmd,
 	do {
 		rc = zcrypt_rsa_modexpo(&mex64);
 	} while (rc == -EAGAIN);
-	if (!rc)
-		rc = put_user(mex64.outputdatalength,
-			      &umex32->outputdatalength);
-	return rc;
+	/* on failure: retry once again after a requested rescan */
+	if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+		do {
+			rc = zcrypt_rsa_modexpo(&mex64);
+		} while (rc == -EAGAIN);
+	if (rc)
+		return rc;
+	return put_user(mex64.outputdatalength,
+			&umex32->outputdatalength);
 }
 
 struct compat_ica_rsa_modexpo_crt {
@@ -810,10 +931,15 @@ static long trans_modexpo_crt32(struct file *filp, unsigned int cmd,
 	do {
 		rc = zcrypt_rsa_crt(&crt64);
 	} while (rc == -EAGAIN);
-	if (!rc)
-		rc = put_user(crt64.outputdatalength,
-			      &ucrt32->outputdatalength);
-	return rc;
+	/* on failure: retry once again after a requested rescan */
+	if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+		do {
+			rc = zcrypt_rsa_crt(&crt64);
+		} while (rc == -EAGAIN);
+	if (rc)
+		return rc;
+	return put_user(crt64.outputdatalength,
+			&ucrt32->outputdatalength);
 }
 
 struct compat_ica_xcRB {
@@ -869,6 +995,11 @@ static long trans_xcRB32(struct file *filp, unsigned int cmd,
 	do {
 		rc = zcrypt_send_cprb(&xcRB64);
 	} while (rc == -EAGAIN);
+	/* on failure: retry once again after a requested rescan */
+	if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+		do {
+			rc = zcrypt_send_cprb(&xcRB64);
+		} while (rc == -EAGAIN);
 	xcRB32.reply_control_blk_length = xcRB64.reply_control_blk_length;
 	xcRB32.reply_data_length = xcRB64.reply_data_length;
 	xcRB32.status = xcRB64.status;
@@ -1126,6 +1257,9 @@ static int zcrypt_rng_data_read(struct hwrng *rng, u32 *data)
 	 */
 	if (zcrypt_rng_buffer_index == 0) {
 		rc = zcrypt_rng((char *) zcrypt_rng_buffer);
+		/* on failure: retry once again after a requested rescan */
+		if ((rc == -ENODEV) && (zcrypt_process_rescan()))
+			rc = zcrypt_rng((char *) zcrypt_rng_buffer);
 		if (rc < 0)
 			return -EIO;
 		zcrypt_rng_buffer_index = rc / sizeof *data;
@@ -1178,6 +1312,30 @@ static void zcrypt_rng_device_remove(void)
 	mutex_unlock(&zcrypt_rng_mutex);
 }
 
+int __init zcrypt_debug_init(void)
+{
+	debugfs_root = debugfs_create_dir("zcrypt", NULL);
+
+	zcrypt_dbf_common = debug_register("zcrypt_common", 1, 1, 16);
+	debug_register_view(zcrypt_dbf_common, &debug_hex_ascii_view);
+	debug_set_level(zcrypt_dbf_common, DBF_ERR);
+
+	zcrypt_dbf_devices = debug_register("zcrypt_devices", 1, 1, 16);
+	debug_register_view(zcrypt_dbf_devices, &debug_hex_ascii_view);
+	debug_set_level(zcrypt_dbf_devices, DBF_ERR);
+
+	return 0;
+}
+
+void zcrypt_debug_exit(void)
+{
+	debugfs_remove(debugfs_root);
+	if (zcrypt_dbf_common)
+		debug_unregister(zcrypt_dbf_common);
+	if (zcrypt_dbf_devices)
+		debug_unregister(zcrypt_dbf_devices);
+}
+
 /**
  * zcrypt_api_init(): Module initialization.
  *
@@ -1187,6 +1345,12 @@ int __init zcrypt_api_init(void)
 {
 	int rc;
 
+	rc = zcrypt_debug_init();
+	if (rc)
+		goto out;
+
+	atomic_set(&zcrypt_rescan_req, 0);
+
 	/* Register the request sprayer. */
 	rc = misc_register(&zcrypt_misc_device);
 	if (rc < 0)
@@ -1216,6 +1380,7 @@ void zcrypt_api_exit(void)
 {
 	remove_proc_entry("driver/z90crypt", NULL);
 	misc_deregister(&zcrypt_misc_device);
+	zcrypt_debug_exit();
 }
 
 module_init(zcrypt_api_init);
diff --git a/drivers/s390/crypto/zcrypt_api.h b/drivers/s390/crypto/zcrypt_api.h
index 7a32c4bc8ef9..89632919c993 100644
--- a/drivers/s390/crypto/zcrypt_api.h
+++ b/drivers/s390/crypto/zcrypt_api.h
@@ -1,7 +1,7 @@
 /*
  *  zcrypt 2.1.0
  *
- *  Copyright IBM Corp. 2001, 2006
+ *  Copyright IBM Corp. 2001, 2012
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *	       Cornelia Huck <cornelia.huck@de.ibm.com>
@@ -9,6 +9,7 @@
  *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
  *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
  *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.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
@@ -28,8 +29,10 @@
 #ifndef _ZCRYPT_API_H_
 #define _ZCRYPT_API_H_
 
-#include "ap_bus.h"
+#include <linux/atomic.h>
+#include <asm/debug.h>
 #include <asm/zcrypt.h>
+#include "ap_bus.h"
 
 /* deprecated status calls */
 #define ICAZ90STATUS		_IOR(ZCRYPT_IOCTL_MAGIC, 0x10, struct ica_z90_status)
@@ -87,6 +90,9 @@ struct zcrypt_ops {
 				struct ica_rsa_modexpo_crt *);
 	long (*send_cprb)(struct zcrypt_device *, struct ica_xcRB *);
 	long (*rng)(struct zcrypt_device *, char *);
+	struct list_head list;		/* zcrypt ops list. */
+	struct module *owner;
+	int variant;
 };
 
 struct zcrypt_device {
@@ -108,14 +114,23 @@ struct zcrypt_device {
 
 	struct ap_message reply;	/* Per-device reply structure. */
 	int max_exp_bit_length;
+
+	debug_info_t *dbf_area;		/* debugging */
 };
 
+/* transport layer rescanning */
+extern atomic_t zcrypt_rescan_req;
+
 struct zcrypt_device *zcrypt_device_alloc(size_t);
 void zcrypt_device_free(struct zcrypt_device *);
 void zcrypt_device_get(struct zcrypt_device *);
 int zcrypt_device_put(struct zcrypt_device *);
 int zcrypt_device_register(struct zcrypt_device *);
 void zcrypt_device_unregister(struct zcrypt_device *);
+void zcrypt_msgtype_register(struct zcrypt_ops *);
+void zcrypt_msgtype_unregister(struct zcrypt_ops *);
+struct zcrypt_ops *zcrypt_msgtype_request(unsigned char *, int);
+void zcrypt_msgtype_release(struct zcrypt_ops *);
 int zcrypt_api_init(void);
 void zcrypt_api_exit(void);
 
diff --git a/drivers/s390/crypto/zcrypt_cex2a.c b/drivers/s390/crypto/zcrypt_cex2a.c
index 744c668f586c..1e849d6e1dfe 100644
--- a/drivers/s390/crypto/zcrypt_cex2a.c
+++ b/drivers/s390/crypto/zcrypt_cex2a.c
@@ -1,13 +1,14 @@
 /*
  *  zcrypt 2.1.0
  *
- *  Copyright IBM Corp. 2001, 2006
+ *  Copyright IBM Corp. 2001, 2012
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
  *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
  *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
  *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.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
@@ -35,6 +36,7 @@
 #include "zcrypt_api.h"
 #include "zcrypt_error.h"
 #include "zcrypt_cex2a.h"
+#include "zcrypt_msgtype50.h"
 
 #define CEX2A_MIN_MOD_SIZE	  1	/*    8 bits	*/
 #define CEX2A_MAX_MOD_SIZE	256	/* 2048 bits	*/
@@ -63,14 +65,12 @@ static struct ap_device_id zcrypt_cex2a_ids[] = {
 
 MODULE_DEVICE_TABLE(ap, zcrypt_cex2a_ids);
 MODULE_AUTHOR("IBM Corporation");
-MODULE_DESCRIPTION("CEX2A Cryptographic Coprocessor device driver, "
-		   "Copyright IBM Corp. 2001, 2006");
+MODULE_DESCRIPTION("CEX2A Cryptographic Coprocessor device driver, " \
+		   "Copyright IBM Corp. 2001, 2012");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_cex2a_probe(struct ap_device *ap_dev);
 static void zcrypt_cex2a_remove(struct ap_device *ap_dev);
-static void zcrypt_cex2a_receive(struct ap_device *, struct ap_message *,
-				 struct ap_message *);
 
 static struct ap_driver zcrypt_cex2a_driver = {
 	.probe = zcrypt_cex2a_probe,
@@ -80,344 +80,6 @@ static struct ap_driver zcrypt_cex2a_driver = {
 };
 
 /**
- * Convert a ICAMEX message to a type50 MEX message.
- *
- * @zdev: crypto device pointer
- * @zreq: crypto request pointer
- * @mex: pointer to user input data
- *
- * Returns 0 on success or -EFAULT.
- */
-static int ICAMEX_msg_to_type50MEX_msg(struct zcrypt_device *zdev,
-				       struct ap_message *ap_msg,
-				       struct ica_rsa_modexpo *mex)
-{
-	unsigned char *mod, *exp, *inp;
-	int mod_len;
-
-	mod_len = mex->inputdatalength;
-
-	if (mod_len <= 128) {
-		struct type50_meb1_msg *meb1 = ap_msg->message;
-		memset(meb1, 0, sizeof(*meb1));
-		ap_msg->length = sizeof(*meb1);
-		meb1->header.msg_type_code = TYPE50_TYPE_CODE;
-		meb1->header.msg_len = sizeof(*meb1);
-		meb1->keyblock_type = TYPE50_MEB1_FMT;
-		mod = meb1->modulus + sizeof(meb1->modulus) - mod_len;
-		exp = meb1->exponent + sizeof(meb1->exponent) - mod_len;
-		inp = meb1->message + sizeof(meb1->message) - mod_len;
-	} else if (mod_len <= 256) {
-		struct type50_meb2_msg *meb2 = ap_msg->message;
-		memset(meb2, 0, sizeof(*meb2));
-		ap_msg->length = sizeof(*meb2);
-		meb2->header.msg_type_code = TYPE50_TYPE_CODE;
-		meb2->header.msg_len = sizeof(*meb2);
-		meb2->keyblock_type = TYPE50_MEB2_FMT;
-		mod = meb2->modulus + sizeof(meb2->modulus) - mod_len;
-		exp = meb2->exponent + sizeof(meb2->exponent) - mod_len;
-		inp = meb2->message + sizeof(meb2->message) - mod_len;
-	} else {
-		/* mod_len > 256 = 4096 bit RSA Key */
-		struct type50_meb3_msg *meb3 = ap_msg->message;
-		memset(meb3, 0, sizeof(*meb3));
-		ap_msg->length = sizeof(*meb3);
-		meb3->header.msg_type_code = TYPE50_TYPE_CODE;
-		meb3->header.msg_len = sizeof(*meb3);
-		meb3->keyblock_type = TYPE50_MEB3_FMT;
-		mod = meb3->modulus + sizeof(meb3->modulus) - mod_len;
-		exp = meb3->exponent + sizeof(meb3->exponent) - mod_len;
-		inp = meb3->message + sizeof(meb3->message) - mod_len;
-	}
-
-	if (copy_from_user(mod, mex->n_modulus, mod_len) ||
-	    copy_from_user(exp, mex->b_key, mod_len) ||
-	    copy_from_user(inp, mex->inputdata, mod_len))
-		return -EFAULT;
-	return 0;
-}
-
-/**
- * Convert a ICACRT message to a type50 CRT message.
- *
- * @zdev: crypto device pointer
- * @zreq: crypto request pointer
- * @crt: pointer to user input data
- *
- * Returns 0 on success or -EFAULT.
- */
-static int ICACRT_msg_to_type50CRT_msg(struct zcrypt_device *zdev,
-				       struct ap_message *ap_msg,
-				       struct ica_rsa_modexpo_crt *crt)
-{
-	int mod_len, short_len, long_len, long_offset, limit;
-	unsigned char *p, *q, *dp, *dq, *u, *inp;
-
-	mod_len = crt->inputdatalength;
-	short_len = mod_len / 2;
-	long_len = mod_len / 2 + 8;
-
-	/*
-	 * CEX2A cannot handle p, dp, or U > 128 bytes.
-	 * If we have one of these, we need to do extra checking.
-	 * For CEX3A the limit is 256 bytes.
-	 */
-	if (zdev->max_mod_size == CEX3A_MAX_MOD_SIZE)
-		limit = 256;
-	else
-		limit = 128;
-
-	if (long_len > limit) {
-		/*
-		 * zcrypt_rsa_crt already checked for the leading
-		 * zeroes of np_prime, bp_key and u_mult_inc.
-		 */
-		long_offset = long_len - limit;
-		long_len = limit;
-	} else
-		long_offset = 0;
-
-	/*
-	 * Instead of doing extra work for p, dp, U > 64 bytes, we'll just use
-	 * the larger message structure.
-	 */
-	if (long_len <= 64) {
-		struct type50_crb1_msg *crb1 = ap_msg->message;
-		memset(crb1, 0, sizeof(*crb1));
-		ap_msg->length = sizeof(*crb1);
-		crb1->header.msg_type_code = TYPE50_TYPE_CODE;
-		crb1->header.msg_len = sizeof(*crb1);
-		crb1->keyblock_type = TYPE50_CRB1_FMT;
-		p = crb1->p + sizeof(crb1->p) - long_len;
-		q = crb1->q + sizeof(crb1->q) - short_len;
-		dp = crb1->dp + sizeof(crb1->dp) - long_len;
-		dq = crb1->dq + sizeof(crb1->dq) - short_len;
-		u = crb1->u + sizeof(crb1->u) - long_len;
-		inp = crb1->message + sizeof(crb1->message) - mod_len;
-	} else if (long_len <= 128) {
-		struct type50_crb2_msg *crb2 = ap_msg->message;
-		memset(crb2, 0, sizeof(*crb2));
-		ap_msg->length = sizeof(*crb2);
-		crb2->header.msg_type_code = TYPE50_TYPE_CODE;
-		crb2->header.msg_len = sizeof(*crb2);
-		crb2->keyblock_type = TYPE50_CRB2_FMT;
-		p = crb2->p + sizeof(crb2->p) - long_len;
-		q = crb2->q + sizeof(crb2->q) - short_len;
-		dp = crb2->dp + sizeof(crb2->dp) - long_len;
-		dq = crb2->dq + sizeof(crb2->dq) - short_len;
-		u = crb2->u + sizeof(crb2->u) - long_len;
-		inp = crb2->message + sizeof(crb2->message) - mod_len;
-	} else {
-		/* long_len >= 256 */
-		struct type50_crb3_msg *crb3 = ap_msg->message;
-		memset(crb3, 0, sizeof(*crb3));
-		ap_msg->length = sizeof(*crb3);
-		crb3->header.msg_type_code = TYPE50_TYPE_CODE;
-		crb3->header.msg_len = sizeof(*crb3);
-		crb3->keyblock_type = TYPE50_CRB3_FMT;
-		p = crb3->p + sizeof(crb3->p) - long_len;
-		q = crb3->q + sizeof(crb3->q) - short_len;
-		dp = crb3->dp + sizeof(crb3->dp) - long_len;
-		dq = crb3->dq + sizeof(crb3->dq) - short_len;
-		u = crb3->u + sizeof(crb3->u) - long_len;
-		inp = crb3->message + sizeof(crb3->message) - mod_len;
-	}
-
-	if (copy_from_user(p, crt->np_prime + long_offset, long_len) ||
-	    copy_from_user(q, crt->nq_prime, short_len) ||
-	    copy_from_user(dp, crt->bp_key + long_offset, long_len) ||
-	    copy_from_user(dq, crt->bq_key, short_len) ||
-	    copy_from_user(u, crt->u_mult_inv + long_offset, long_len) ||
-	    copy_from_user(inp, crt->inputdata, mod_len))
-		return -EFAULT;
-
-	return 0;
-}
-
-/**
- * Copy results from a type 80 reply message back to user space.
- *
- * @zdev: crypto device pointer
- * @reply: reply AP message.
- * @data: pointer to user output data
- * @length: size of user output data
- *
- * Returns 0 on success or -EFAULT.
- */
-static int convert_type80(struct zcrypt_device *zdev,
-			  struct ap_message *reply,
-			  char __user *outputdata,
-			  unsigned int outputdatalength)
-{
-	struct type80_hdr *t80h = reply->message;
-	unsigned char *data;
-
-	if (t80h->len < sizeof(*t80h) + outputdatalength) {
-		/* The result is too short, the CEX2A card may not do that.. */
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-	if (zdev->user_space_type == ZCRYPT_CEX2A)
-		BUG_ON(t80h->len > CEX2A_MAX_RESPONSE_SIZE);
-	else
-		BUG_ON(t80h->len > CEX3A_MAX_RESPONSE_SIZE);
-	data = reply->message + t80h->len - outputdatalength;
-	if (copy_to_user(outputdata, data, outputdatalength))
-		return -EFAULT;
-	return 0;
-}
-
-static int convert_response(struct zcrypt_device *zdev,
-			    struct ap_message *reply,
-			    char __user *outputdata,
-			    unsigned int outputdatalength)
-{
-	/* Response type byte is the second byte in the response. */
-	switch (((unsigned char *) reply->message)[1]) {
-	case TYPE82_RSP_CODE:
-	case TYPE88_RSP_CODE:
-		return convert_error(zdev, reply);
-	case TYPE80_RSP_CODE:
-		return convert_type80(zdev, reply,
-				      outputdata, outputdatalength);
-	default: /* Unknown response type, this should NEVER EVER happen */
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-}
-
-/**
- * This function is called from the AP bus code after a crypto request
- * "msg" has finished with the reply message "reply".
- * It is called from tasklet context.
- * @ap_dev: pointer to the AP device
- * @msg: pointer to the AP message
- * @reply: pointer to the AP reply message
- */
-static void zcrypt_cex2a_receive(struct ap_device *ap_dev,
-				 struct ap_message *msg,
-				 struct ap_message *reply)
-{
-	static struct error_hdr error_reply = {
-		.type = TYPE82_RSP_CODE,
-		.reply_code = REP82_ERROR_MACHINE_FAILURE,
-	};
-	struct type80_hdr *t80h;
-	int length;
-
-	/* Copy the reply message to the request message buffer. */
-	if (IS_ERR(reply)) {
-		memcpy(msg->message, &error_reply, sizeof(error_reply));
-		goto out;
-	}
-	t80h = reply->message;
-	if (t80h->type == TYPE80_RSP_CODE) {
-		if (ap_dev->device_type == AP_DEVICE_TYPE_CEX2A)
-			length = min(CEX2A_MAX_RESPONSE_SIZE, (int) t80h->len);
-		else
-			length = min(CEX3A_MAX_RESPONSE_SIZE, (int) t80h->len);
-		memcpy(msg->message, reply->message, length);
-	} else
-		memcpy(msg->message, reply->message, sizeof error_reply);
-out:
-	complete((struct completion *) msg->private);
-}
-
-static atomic_t zcrypt_step = ATOMIC_INIT(0);
-
-/**
- * The request distributor calls this function if it picked the CEX2A
- * device to handle a modexpo request.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  CEX2A device to the request distributor
- * @mex: pointer to the modexpo request buffer
- */
-static long zcrypt_cex2a_modexpo(struct zcrypt_device *zdev,
-				 struct ica_rsa_modexpo *mex)
-{
-	struct ap_message ap_msg;
-	struct completion work;
-	int rc;
-
-	ap_init_message(&ap_msg);
-	if (zdev->user_space_type == ZCRYPT_CEX2A)
-		ap_msg.message = kmalloc(CEX2A_MAX_MESSAGE_SIZE, GFP_KERNEL);
-	else
-		ap_msg.message = kmalloc(CEX3A_MAX_MESSAGE_SIZE, GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_cex2a_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &work;
-	rc = ICAMEX_msg_to_type50MEX_msg(zdev, &ap_msg, mex);
-	if (rc)
-		goto out_free;
-	init_completion(&work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&work);
-	if (rc == 0)
-		rc = convert_response(zdev, &ap_msg, mex->outputdata,
-				      mex->outputdatalength);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-out_free:
-	kfree(ap_msg.message);
-	return rc;
-}
-
-/**
- * The request distributor calls this function if it picked the CEX2A
- * device to handle a modexpo_crt request.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  CEX2A device to the request distributor
- * @crt: pointer to the modexpoc_crt request buffer
- */
-static long zcrypt_cex2a_modexpo_crt(struct zcrypt_device *zdev,
-				     struct ica_rsa_modexpo_crt *crt)
-{
-	struct ap_message ap_msg;
-	struct completion work;
-	int rc;
-
-	ap_init_message(&ap_msg);
-	if (zdev->user_space_type == ZCRYPT_CEX2A)
-		ap_msg.message = kmalloc(CEX2A_MAX_MESSAGE_SIZE, GFP_KERNEL);
-	else
-		ap_msg.message = kmalloc(CEX3A_MAX_MESSAGE_SIZE, GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_cex2a_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &work;
-	rc = ICACRT_msg_to_type50CRT_msg(zdev, &ap_msg, crt);
-	if (rc)
-		goto out_free;
-	init_completion(&work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&work);
-	if (rc == 0)
-		rc = convert_response(zdev, &ap_msg, crt->outputdata,
-				      crt->outputdatalength);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-out_free:
-	kfree(ap_msg.message);
-	return rc;
-}
-
-/**
- * The crypto operations for a CEX2A card.
- */
-static struct zcrypt_ops zcrypt_cex2a_ops = {
-	.rsa_modexpo = zcrypt_cex2a_modexpo,
-	.rsa_modexpo_crt = zcrypt_cex2a_modexpo_crt,
-};
-
-/**
  * Probe function for CEX2A cards. It always accepts the AP device
  * since the bus_match already checked the hardware type.
  * @ap_dev: pointer to the AP device.
@@ -449,7 +111,8 @@ static int zcrypt_cex2a_probe(struct ap_device *ap_dev)
 		zdev->min_mod_size = CEX2A_MIN_MOD_SIZE;
 		zdev->max_mod_size = CEX2A_MAX_MOD_SIZE;
 		zdev->max_exp_bit_length = CEX2A_MAX_MOD_SIZE;
-		if (ap_4096_commands_available(ap_dev->qid)) {
+		if (ap_test_bit(&ap_dev->functions, AP_FUNC_MEX4K) &&
+		    ap_test_bit(&ap_dev->functions, AP_FUNC_CRT4K)) {
 			zdev->max_mod_size = CEX3A_MAX_MOD_SIZE;
 			zdev->max_exp_bit_length = CEX3A_MAX_MOD_SIZE;
 		}
@@ -457,16 +120,18 @@ static int zcrypt_cex2a_probe(struct ap_device *ap_dev)
 		zdev->speed_rating = CEX3A_SPEED_RATING;
 		break;
 	}
-	if (zdev != NULL) {
-		zdev->ap_dev = ap_dev;
-		zdev->ops = &zcrypt_cex2a_ops;
-		zdev->online = 1;
-		ap_dev->reply = &zdev->reply;
-		ap_dev->private = zdev;
-		rc = zcrypt_device_register(zdev);
-	}
+	if (!zdev)
+		return -ENODEV;
+	zdev->ops = zcrypt_msgtype_request(MSGTYPE50_NAME,
+					   MSGTYPE50_VARIANT_DEFAULT);
+	zdev->ap_dev = ap_dev;
+	zdev->online = 1;
+	ap_dev->reply = &zdev->reply;
+	ap_dev->private = zdev;
+	rc = zcrypt_device_register(zdev);
 	if (rc) {
 		ap_dev->private = NULL;
+		zcrypt_msgtype_release(zdev->ops);
 		zcrypt_device_free(zdev);
 	}
 	return rc;
@@ -479,8 +144,10 @@ static int zcrypt_cex2a_probe(struct ap_device *ap_dev)
 static void zcrypt_cex2a_remove(struct ap_device *ap_dev)
 {
 	struct zcrypt_device *zdev = ap_dev->private;
+	struct zcrypt_ops *zops = zdev->ops;
 
 	zcrypt_device_unregister(zdev);
+	zcrypt_msgtype_release(zops);
 }
 
 int __init zcrypt_cex2a_init(void)
diff --git a/drivers/s390/crypto/zcrypt_cex4.c b/drivers/s390/crypto/zcrypt_cex4.c
new file mode 100644
index 000000000000..ce1226398ac9
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_cex4.c
@@ -0,0 +1,149 @@
+/*
+ *  Copyright IBM Corp. 2012
+ *  Author(s): Holger Dengler <hd@linux.vnet.ibm.com>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/atomic.h>
+#include <linux/uaccess.h>
+
+#include "ap_bus.h"
+#include "zcrypt_api.h"
+#include "zcrypt_msgtype6.h"
+#include "zcrypt_msgtype50.h"
+#include "zcrypt_error.h"
+#include "zcrypt_cex4.h"
+
+#define CEX4A_MIN_MOD_SIZE	  1	/*    8 bits	*/
+#define CEX4A_MAX_MOD_SIZE_2K	256	/* 2048 bits	*/
+#define CEX4A_MAX_MOD_SIZE_4K	512	/* 4096 bits	*/
+
+#define CEX4C_MIN_MOD_SIZE	 16	/*  256 bits	*/
+#define CEX4C_MAX_MOD_SIZE	512	/* 4096 bits	*/
+
+#define CEX4A_SPEED_RATING	900	 /* TODO new card, new speed rating */
+#define CEX4C_SPEED_RATING	6500	 /* TODO new card, new speed rating */
+
+#define CEX4A_MAX_MESSAGE_SIZE	MSGTYPE50_CRB3_MAX_MSG_SIZE
+#define CEX4C_MAX_MESSAGE_SIZE	MSGTYPE06_MAX_MSG_SIZE
+
+#define CEX4_CLEANUP_TIME	(15*HZ)
+
+static struct ap_device_id zcrypt_cex4_ids[] = {
+	{ AP_DEVICE(AP_DEVICE_TYPE_CEX4)  },
+	{ /* end of list */ },
+};
+
+MODULE_DEVICE_TABLE(ap, zcrypt_cex4_ids);
+MODULE_AUTHOR("IBM Corporation");
+MODULE_DESCRIPTION("CEX4 Cryptographic Card device driver, " \
+		   "Copyright IBM Corp. 2012");
+MODULE_LICENSE("GPL");
+
+static int zcrypt_cex4_probe(struct ap_device *ap_dev);
+static void zcrypt_cex4_remove(struct ap_device *ap_dev);
+
+static struct ap_driver zcrypt_cex4_driver = {
+	.probe = zcrypt_cex4_probe,
+	.remove = zcrypt_cex4_remove,
+	.ids = zcrypt_cex4_ids,
+	.request_timeout = CEX4_CLEANUP_TIME,
+};
+
+/**
+ * Probe function for CEX4 cards. It always accepts the AP device
+ * since the bus_match already checked the hardware type.
+ * @ap_dev: pointer to the AP device.
+ */
+static int zcrypt_cex4_probe(struct ap_device *ap_dev)
+{
+	struct zcrypt_device *zdev = NULL;
+	int rc = 0;
+
+	switch (ap_dev->device_type) {
+	case AP_DEVICE_TYPE_CEX4:
+		if (ap_test_bit(&ap_dev->functions, AP_FUNC_ACCEL)) {
+			zdev = zcrypt_device_alloc(CEX4A_MAX_MESSAGE_SIZE);
+			if (!zdev)
+				return -ENOMEM;
+			zdev->type_string = "CEX4A";
+			zdev->user_space_type = ZCRYPT_CEX3A;
+			zdev->min_mod_size = CEX4A_MIN_MOD_SIZE;
+			if (ap_test_bit(&ap_dev->functions, AP_FUNC_MEX4K) &&
+			    ap_test_bit(&ap_dev->functions, AP_FUNC_CRT4K)) {
+				zdev->max_mod_size =
+					CEX4A_MAX_MOD_SIZE_4K;
+				zdev->max_exp_bit_length =
+					CEX4A_MAX_MOD_SIZE_4K;
+			} else {
+				zdev->max_mod_size =
+					CEX4A_MAX_MOD_SIZE_2K;
+				zdev->max_exp_bit_length =
+					CEX4A_MAX_MOD_SIZE_2K;
+			}
+			zdev->short_crt = 1;
+			zdev->speed_rating = CEX4A_SPEED_RATING;
+			zdev->ops = zcrypt_msgtype_request(MSGTYPE50_NAME,
+							   MSGTYPE50_VARIANT_DEFAULT);
+		} else if (ap_test_bit(&ap_dev->functions, AP_FUNC_COPRO)) {
+			zdev = zcrypt_device_alloc(CEX4C_MAX_MESSAGE_SIZE);
+			if (!zdev)
+				return -ENOMEM;
+			zdev->type_string = "CEX4C";
+			zdev->user_space_type = ZCRYPT_CEX3C;
+			zdev->min_mod_size = CEX4C_MIN_MOD_SIZE;
+			zdev->max_mod_size = CEX4C_MAX_MOD_SIZE;
+			zdev->max_exp_bit_length = CEX4C_MAX_MOD_SIZE;
+			zdev->short_crt = 0;
+			zdev->speed_rating = CEX4C_SPEED_RATING;
+			zdev->ops = zcrypt_msgtype_request(MSGTYPE06_NAME,
+							   MSGTYPE06_VARIANT_DEFAULT);
+		}
+		break;
+	}
+	if (!zdev)
+		return -ENODEV;
+	zdev->ap_dev = ap_dev;
+	zdev->online = 1;
+	ap_dev->reply = &zdev->reply;
+	ap_dev->private = zdev;
+	rc = zcrypt_device_register(zdev);
+	if (rc) {
+		zcrypt_msgtype_release(zdev->ops);
+		ap_dev->private = NULL;
+		zcrypt_device_free(zdev);
+	}
+	return rc;
+}
+
+/**
+ * This is called to remove the extended CEX4 driver information
+ * if an AP device is removed.
+ */
+static void zcrypt_cex4_remove(struct ap_device *ap_dev)
+{
+	struct zcrypt_device *zdev = ap_dev->private;
+	struct zcrypt_ops *zops;
+
+	if (zdev) {
+		zops = zdev->ops;
+		zcrypt_device_unregister(zdev);
+		zcrypt_msgtype_release(zops);
+	}
+}
+
+int __init zcrypt_cex4_init(void)
+{
+	return ap_driver_register(&zcrypt_cex4_driver, THIS_MODULE, "cex4");
+}
+
+void __exit zcrypt_cex4_exit(void)
+{
+	ap_driver_unregister(&zcrypt_cex4_driver);
+}
+
+module_init(zcrypt_cex4_init);
+module_exit(zcrypt_cex4_exit);
diff --git a/drivers/s390/crypto/zcrypt_cex4.h b/drivers/s390/crypto/zcrypt_cex4.h
new file mode 100644
index 000000000000..719571375ccc
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_cex4.h
@@ -0,0 +1,12 @@
+/*
+ *  Copyright IBM Corp. 2012
+ *  Author(s): Holger Dengler <hd@linux.vnet.ibm.com>
+ */
+
+#ifndef _ZCRYPT_CEX4_H_
+#define _ZCRYPT_CEX4_H_
+
+int zcrypt_cex4_init(void);
+void zcrypt_cex4_exit(void);
+
+#endif /* _ZCRYPT_CEX4_H_ */
diff --git a/drivers/s390/crypto/zcrypt_debug.h b/drivers/s390/crypto/zcrypt_debug.h
new file mode 100644
index 000000000000..841ea72e4a4e
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_debug.h
@@ -0,0 +1,59 @@
+/*
+ *  Copyright IBM Corp. 2012
+ *  Author(s): Holger Dengler (hd@linux.vnet.ibm.com)
+ */
+#ifndef ZCRYPT_DEBUG_H
+#define ZCRYPT_DEBUG_H
+
+#include <asm/debug.h>
+#include "zcrypt_api.h"
+
+/* that gives us 15 characters in the text event views */
+#define ZCRYPT_DBF_LEN	16
+
+/* sort out low debug levels early to avoid wasted sprints */
+static inline int zcrypt_dbf_passes(debug_info_t *dbf_grp, int level)
+{
+	return (level <= dbf_grp->level);
+}
+
+#define DBF_ERR		3	/* error conditions	*/
+#define DBF_WARN	4	/* warning conditions	*/
+#define DBF_INFO	6	/* informational	*/
+
+#define RC2WARN(rc) ((rc) ? DBF_WARN : DBF_INFO)
+
+#define ZCRYPT_DBF_COMMON(level, text...) \
+	do { \
+		if (zcrypt_dbf_passes(zcrypt_dbf_common, level)) { \
+			char debug_buffer[ZCRYPT_DBF_LEN]; \
+			snprintf(debug_buffer, ZCRYPT_DBF_LEN, text); \
+			debug_text_event(zcrypt_dbf_common, level, \
+					 debug_buffer); \
+		} \
+	} while (0)
+
+#define ZCRYPT_DBF_DEVICES(level, text...) \
+	do { \
+		if (zcrypt_dbf_passes(zcrypt_dbf_devices, level)) { \
+			char debug_buffer[ZCRYPT_DBF_LEN]; \
+			snprintf(debug_buffer, ZCRYPT_DBF_LEN, text); \
+			debug_text_event(zcrypt_dbf_devices, level, \
+					 debug_buffer); \
+		} \
+	} while (0)
+
+#define ZCRYPT_DBF_DEV(level, device, text...) \
+	do { \
+		if (zcrypt_dbf_passes(device->dbf_area, level)) { \
+			char debug_buffer[ZCRYPT_DBF_LEN]; \
+			snprintf(debug_buffer, ZCRYPT_DBF_LEN, text); \
+			debug_text_event(device->dbf_area, level, \
+					 debug_buffer); \
+		} \
+	} while (0)
+
+int zcrypt_debug_init(void);
+void zcrypt_debug_exit(void);
+
+#endif /* ZCRYPT_DEBUG_H */
diff --git a/drivers/s390/crypto/zcrypt_error.h b/drivers/s390/crypto/zcrypt_error.h
index 0965e2626d18..0079b6617211 100644
--- a/drivers/s390/crypto/zcrypt_error.h
+++ b/drivers/s390/crypto/zcrypt_error.h
@@ -26,6 +26,8 @@
 #ifndef _ZCRYPT_ERROR_H_
 #define _ZCRYPT_ERROR_H_
 
+#include <linux/atomic.h>
+#include "zcrypt_debug.h"
 #include "zcrypt_api.h"
 
 /**
@@ -108,16 +110,27 @@ static inline int convert_error(struct zcrypt_device *zdev,
 		 * and then repeat the request.
 		 */
 		WARN_ON(1);
+		atomic_set(&zcrypt_rescan_req, 1);
 		zdev->online = 0;
+		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
+			       zdev->ap_dev->qid,
+			       zdev->online, ehdr->reply_code);
 		return -EAGAIN;
 	case REP82_ERROR_TRANSPORT_FAIL:
 	case REP82_ERROR_MACHINE_FAILURE:
 	//   REP88_ERROR_MODULE_FAILURE		// '10' CEX2A
 		/* If a card fails disable it and repeat the request. */
+		atomic_set(&zcrypt_rescan_req, 1);
 		zdev->online = 0;
+		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
+			       zdev->ap_dev->qid,
+			       zdev->online, ehdr->reply_code);
 		return -EAGAIN;
 	default:
 		zdev->online = 0;
+		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
+			       zdev->ap_dev->qid,
+			       zdev->online, ehdr->reply_code);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
diff --git a/drivers/s390/crypto/zcrypt_msgtype50.c b/drivers/s390/crypto/zcrypt_msgtype50.c
new file mode 100644
index 000000000000..035b6dc31b71
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_msgtype50.c
@@ -0,0 +1,531 @@
+/*
+ *  zcrypt 2.1.0
+ *
+ *  Copyright IBM Corp. 2001, 2012
+ *  Author(s): Robert Burroughs
+ *	       Eric Rossman (edrossma@us.ibm.com)
+ *
+ *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
+ *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
+ *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/atomic.h>
+#include <linux/uaccess.h>
+
+#include "ap_bus.h"
+#include "zcrypt_api.h"
+#include "zcrypt_error.h"
+#include "zcrypt_msgtype50.h"
+
+#define CEX3A_MAX_MOD_SIZE	512	/* 4096 bits	*/
+
+#define CEX2A_MAX_RESPONSE_SIZE 0x110	/* max outputdatalength + type80_hdr */
+
+#define CEX3A_MAX_RESPONSE_SIZE	0x210	/* 512 bit modulus
+					 * (max outputdatalength) +
+					 * type80_hdr*/
+
+MODULE_AUTHOR("IBM Corporation");
+MODULE_DESCRIPTION("Cryptographic Accelerator (message type 50), " \
+		   "Copyright IBM Corp. 2001, 2012");
+MODULE_LICENSE("GPL");
+
+static void zcrypt_cex2a_receive(struct ap_device *, struct ap_message *,
+				 struct ap_message *);
+
+/**
+ * The type 50 message family is associated with a CEX2A card.
+ *
+ * The four members of the family are described below.
+ *
+ * Note that all unsigned char arrays are right-justified and left-padded
+ * with zeroes.
+ *
+ * Note that all reserved fields must be zeroes.
+ */
+struct type50_hdr {
+	unsigned char	reserved1;
+	unsigned char	msg_type_code;	/* 0x50 */
+	unsigned short	msg_len;
+	unsigned char	reserved2;
+	unsigned char	ignored;
+	unsigned short	reserved3;
+} __packed;
+
+#define TYPE50_TYPE_CODE	0x50
+
+#define TYPE50_MEB1_FMT		0x0001
+#define TYPE50_MEB2_FMT		0x0002
+#define TYPE50_MEB3_FMT		0x0003
+#define TYPE50_CRB1_FMT		0x0011
+#define TYPE50_CRB2_FMT		0x0012
+#define TYPE50_CRB3_FMT		0x0013
+
+/* Mod-Exp, with a small modulus */
+struct type50_meb1_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0001 */
+	unsigned char	reserved[6];
+	unsigned char	exponent[128];
+	unsigned char	modulus[128];
+	unsigned char	message[128];
+} __packed;
+
+/* Mod-Exp, with a large modulus */
+struct type50_meb2_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0002 */
+	unsigned char	reserved[6];
+	unsigned char	exponent[256];
+	unsigned char	modulus[256];
+	unsigned char	message[256];
+} __packed;
+
+/* Mod-Exp, with a larger modulus */
+struct type50_meb3_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0003 */
+	unsigned char	reserved[6];
+	unsigned char	exponent[512];
+	unsigned char	modulus[512];
+	unsigned char	message[512];
+} __packed;
+
+/* CRT, with a small modulus */
+struct type50_crb1_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0011 */
+	unsigned char	reserved[6];
+	unsigned char	p[64];
+	unsigned char	q[64];
+	unsigned char	dp[64];
+	unsigned char	dq[64];
+	unsigned char	u[64];
+	unsigned char	message[128];
+} __packed;
+
+/* CRT, with a large modulus */
+struct type50_crb2_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0012 */
+	unsigned char	reserved[6];
+	unsigned char	p[128];
+	unsigned char	q[128];
+	unsigned char	dp[128];
+	unsigned char	dq[128];
+	unsigned char	u[128];
+	unsigned char	message[256];
+} __packed;
+
+/* CRT, with a larger modulus */
+struct type50_crb3_msg {
+	struct type50_hdr header;
+	unsigned short	keyblock_type;	/* 0x0013 */
+	unsigned char	reserved[6];
+	unsigned char	p[256];
+	unsigned char	q[256];
+	unsigned char	dp[256];
+	unsigned char	dq[256];
+	unsigned char	u[256];
+	unsigned char	message[512];
+} __packed;
+
+/**
+ * The type 80 response family is associated with a CEX2A card.
+ *
+ * Note that all unsigned char arrays are right-justified and left-padded
+ * with zeroes.
+ *
+ * Note that all reserved fields must be zeroes.
+ */
+
+#define TYPE80_RSP_CODE 0x80
+
+struct type80_hdr {
+	unsigned char	reserved1;
+	unsigned char	type;		/* 0x80 */
+	unsigned short	len;
+	unsigned char	code;		/* 0x00 */
+	unsigned char	reserved2[3];
+	unsigned char	reserved3[8];
+} __packed;
+
+/**
+ * Convert a ICAMEX message to a type50 MEX message.
+ *
+ * @zdev: crypto device pointer
+ * @zreq: crypto request pointer
+ * @mex: pointer to user input data
+ *
+ * Returns 0 on success or -EFAULT.
+ */
+static int ICAMEX_msg_to_type50MEX_msg(struct zcrypt_device *zdev,
+				       struct ap_message *ap_msg,
+				       struct ica_rsa_modexpo *mex)
+{
+	unsigned char *mod, *exp, *inp;
+	int mod_len;
+
+	mod_len = mex->inputdatalength;
+
+	if (mod_len <= 128) {
+		struct type50_meb1_msg *meb1 = ap_msg->message;
+		memset(meb1, 0, sizeof(*meb1));
+		ap_msg->length = sizeof(*meb1);
+		meb1->header.msg_type_code = TYPE50_TYPE_CODE;
+		meb1->header.msg_len = sizeof(*meb1);
+		meb1->keyblock_type = TYPE50_MEB1_FMT;
+		mod = meb1->modulus + sizeof(meb1->modulus) - mod_len;
+		exp = meb1->exponent + sizeof(meb1->exponent) - mod_len;
+		inp = meb1->message + sizeof(meb1->message) - mod_len;
+	} else if (mod_len <= 256) {
+		struct type50_meb2_msg *meb2 = ap_msg->message;
+		memset(meb2, 0, sizeof(*meb2));
+		ap_msg->length = sizeof(*meb2);
+		meb2->header.msg_type_code = TYPE50_TYPE_CODE;
+		meb2->header.msg_len = sizeof(*meb2);
+		meb2->keyblock_type = TYPE50_MEB2_FMT;
+		mod = meb2->modulus + sizeof(meb2->modulus) - mod_len;
+		exp = meb2->exponent + sizeof(meb2->exponent) - mod_len;
+		inp = meb2->message + sizeof(meb2->message) - mod_len;
+	} else {
+		/* mod_len > 256 = 4096 bit RSA Key */
+		struct type50_meb3_msg *meb3 = ap_msg->message;
+		memset(meb3, 0, sizeof(*meb3));
+		ap_msg->length = sizeof(*meb3);
+		meb3->header.msg_type_code = TYPE50_TYPE_CODE;
+		meb3->header.msg_len = sizeof(*meb3);
+		meb3->keyblock_type = TYPE50_MEB3_FMT;
+		mod = meb3->modulus + sizeof(meb3->modulus) - mod_len;
+		exp = meb3->exponent + sizeof(meb3->exponent) - mod_len;
+		inp = meb3->message + sizeof(meb3->message) - mod_len;
+	}
+
+	if (copy_from_user(mod, mex->n_modulus, mod_len) ||
+	    copy_from_user(exp, mex->b_key, mod_len) ||
+	    copy_from_user(inp, mex->inputdata, mod_len))
+		return -EFAULT;
+	return 0;
+}
+
+/**
+ * Convert a ICACRT message to a type50 CRT message.
+ *
+ * @zdev: crypto device pointer
+ * @zreq: crypto request pointer
+ * @crt: pointer to user input data
+ *
+ * Returns 0 on success or -EFAULT.
+ */
+static int ICACRT_msg_to_type50CRT_msg(struct zcrypt_device *zdev,
+				       struct ap_message *ap_msg,
+				       struct ica_rsa_modexpo_crt *crt)
+{
+	int mod_len, short_len, long_len, long_offset, limit;
+	unsigned char *p, *q, *dp, *dq, *u, *inp;
+
+	mod_len = crt->inputdatalength;
+	short_len = mod_len / 2;
+	long_len = mod_len / 2 + 8;
+
+	/*
+	 * CEX2A cannot handle p, dp, or U > 128 bytes.
+	 * If we have one of these, we need to do extra checking.
+	 * For CEX3A the limit is 256 bytes.
+	 */
+	if (zdev->max_mod_size == CEX3A_MAX_MOD_SIZE)
+		limit = 256;
+	else
+		limit = 128;
+
+	if (long_len > limit) {
+		/*
+		 * zcrypt_rsa_crt already checked for the leading
+		 * zeroes of np_prime, bp_key and u_mult_inc.
+		 */
+		long_offset = long_len - limit;
+		long_len = limit;
+	} else
+		long_offset = 0;
+
+	/*
+	 * Instead of doing extra work for p, dp, U > 64 bytes, we'll just use
+	 * the larger message structure.
+	 */
+	if (long_len <= 64) {
+		struct type50_crb1_msg *crb1 = ap_msg->message;
+		memset(crb1, 0, sizeof(*crb1));
+		ap_msg->length = sizeof(*crb1);
+		crb1->header.msg_type_code = TYPE50_TYPE_CODE;
+		crb1->header.msg_len = sizeof(*crb1);
+		crb1->keyblock_type = TYPE50_CRB1_FMT;
+		p = crb1->p + sizeof(crb1->p) - long_len;
+		q = crb1->q + sizeof(crb1->q) - short_len;
+		dp = crb1->dp + sizeof(crb1->dp) - long_len;
+		dq = crb1->dq + sizeof(crb1->dq) - short_len;
+		u = crb1->u + sizeof(crb1->u) - long_len;
+		inp = crb1->message + sizeof(crb1->message) - mod_len;
+	} else if (long_len <= 128) {
+		struct type50_crb2_msg *crb2 = ap_msg->message;
+		memset(crb2, 0, sizeof(*crb2));
+		ap_msg->length = sizeof(*crb2);
+		crb2->header.msg_type_code = TYPE50_TYPE_CODE;
+		crb2->header.msg_len = sizeof(*crb2);
+		crb2->keyblock_type = TYPE50_CRB2_FMT;
+		p = crb2->p + sizeof(crb2->p) - long_len;
+		q = crb2->q + sizeof(crb2->q) - short_len;
+		dp = crb2->dp + sizeof(crb2->dp) - long_len;
+		dq = crb2->dq + sizeof(crb2->dq) - short_len;
+		u = crb2->u + sizeof(crb2->u) - long_len;
+		inp = crb2->message + sizeof(crb2->message) - mod_len;
+	} else {
+		/* long_len >= 256 */
+		struct type50_crb3_msg *crb3 = ap_msg->message;
+		memset(crb3, 0, sizeof(*crb3));
+		ap_msg->length = sizeof(*crb3);
+		crb3->header.msg_type_code = TYPE50_TYPE_CODE;
+		crb3->header.msg_len = sizeof(*crb3);
+		crb3->keyblock_type = TYPE50_CRB3_FMT;
+		p = crb3->p + sizeof(crb3->p) - long_len;
+		q = crb3->q + sizeof(crb3->q) - short_len;
+		dp = crb3->dp + sizeof(crb3->dp) - long_len;
+		dq = crb3->dq + sizeof(crb3->dq) - short_len;
+		u = crb3->u + sizeof(crb3->u) - long_len;
+		inp = crb3->message + sizeof(crb3->message) - mod_len;
+	}
+
+	if (copy_from_user(p, crt->np_prime + long_offset, long_len) ||
+	    copy_from_user(q, crt->nq_prime, short_len) ||
+	    copy_from_user(dp, crt->bp_key + long_offset, long_len) ||
+	    copy_from_user(dq, crt->bq_key, short_len) ||
+	    copy_from_user(u, crt->u_mult_inv + long_offset, long_len) ||
+	    copy_from_user(inp, crt->inputdata, mod_len))
+		return -EFAULT;
+
+	return 0;
+}
+
+/**
+ * Copy results from a type 80 reply message back to user space.
+ *
+ * @zdev: crypto device pointer
+ * @reply: reply AP message.
+ * @data: pointer to user output data
+ * @length: size of user output data
+ *
+ * Returns 0 on success or -EFAULT.
+ */
+static int convert_type80(struct zcrypt_device *zdev,
+			  struct ap_message *reply,
+			  char __user *outputdata,
+			  unsigned int outputdatalength)
+{
+	struct type80_hdr *t80h = reply->message;
+	unsigned char *data;
+
+	if (t80h->len < sizeof(*t80h) + outputdatalength) {
+		/* The result is too short, the CEX2A card may not do that.. */
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+	if (zdev->user_space_type == ZCRYPT_CEX2A)
+		BUG_ON(t80h->len > CEX2A_MAX_RESPONSE_SIZE);
+	else
+		BUG_ON(t80h->len > CEX3A_MAX_RESPONSE_SIZE);
+	data = reply->message + t80h->len - outputdatalength;
+	if (copy_to_user(outputdata, data, outputdatalength))
+		return -EFAULT;
+	return 0;
+}
+
+static int convert_response(struct zcrypt_device *zdev,
+			    struct ap_message *reply,
+			    char __user *outputdata,
+			    unsigned int outputdatalength)
+{
+	/* Response type byte is the second byte in the response. */
+	switch (((unsigned char *) reply->message)[1]) {
+	case TYPE82_RSP_CODE:
+	case TYPE88_RSP_CODE:
+		return convert_error(zdev, reply);
+	case TYPE80_RSP_CODE:
+		return convert_type80(zdev, reply,
+				      outputdata, outputdatalength);
+	default: /* Unknown response type, this should NEVER EVER happen */
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+}
+
+/**
+ * This function is called from the AP bus code after a crypto request
+ * "msg" has finished with the reply message "reply".
+ * It is called from tasklet context.
+ * @ap_dev: pointer to the AP device
+ * @msg: pointer to the AP message
+ * @reply: pointer to the AP reply message
+ */
+static void zcrypt_cex2a_receive(struct ap_device *ap_dev,
+				 struct ap_message *msg,
+				 struct ap_message *reply)
+{
+	static struct error_hdr error_reply = {
+		.type = TYPE82_RSP_CODE,
+		.reply_code = REP82_ERROR_MACHINE_FAILURE,
+	};
+	struct type80_hdr *t80h;
+	int length;
+
+	/* Copy the reply message to the request message buffer. */
+	if (IS_ERR(reply)) {
+		memcpy(msg->message, &error_reply, sizeof(error_reply));
+		goto out;
+	}
+	t80h = reply->message;
+	if (t80h->type == TYPE80_RSP_CODE) {
+		if (ap_dev->device_type == AP_DEVICE_TYPE_CEX2A)
+			length = min_t(int,
+				       CEX2A_MAX_RESPONSE_SIZE, t80h->len);
+		else
+			length = min_t(int,
+				       CEX3A_MAX_RESPONSE_SIZE, t80h->len);
+		memcpy(msg->message, reply->message, length);
+	} else
+		memcpy(msg->message, reply->message, sizeof(error_reply));
+out:
+	complete((struct completion *) msg->private);
+}
+
+static atomic_t zcrypt_step = ATOMIC_INIT(0);
+
+/**
+ * The request distributor calls this function if it picked the CEX2A
+ * device to handle a modexpo request.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  CEX2A device to the request distributor
+ * @mex: pointer to the modexpo request buffer
+ */
+static long zcrypt_cex2a_modexpo(struct zcrypt_device *zdev,
+				 struct ica_rsa_modexpo *mex)
+{
+	struct ap_message ap_msg;
+	struct completion work;
+	int rc;
+
+	ap_init_message(&ap_msg);
+	if (zdev->user_space_type == ZCRYPT_CEX2A)
+		ap_msg.message = kmalloc(MSGTYPE50_CRB2_MAX_MSG_SIZE,
+					 GFP_KERNEL);
+	else
+		ap_msg.message = kmalloc(MSGTYPE50_CRB3_MAX_MSG_SIZE,
+					 GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_cex2a_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &work;
+	rc = ICAMEX_msg_to_type50MEX_msg(zdev, &ap_msg, mex);
+	if (rc)
+		goto out_free;
+	init_completion(&work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&work);
+	if (rc == 0)
+		rc = convert_response(zdev, &ap_msg, mex->outputdata,
+				      mex->outputdatalength);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+out_free:
+	kfree(ap_msg.message);
+	return rc;
+}
+
+/**
+ * The request distributor calls this function if it picked the CEX2A
+ * device to handle a modexpo_crt request.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  CEX2A device to the request distributor
+ * @crt: pointer to the modexpoc_crt request buffer
+ */
+static long zcrypt_cex2a_modexpo_crt(struct zcrypt_device *zdev,
+				     struct ica_rsa_modexpo_crt *crt)
+{
+	struct ap_message ap_msg;
+	struct completion work;
+	int rc;
+
+	ap_init_message(&ap_msg);
+	if (zdev->user_space_type == ZCRYPT_CEX2A)
+		ap_msg.message = kmalloc(MSGTYPE50_CRB2_MAX_MSG_SIZE,
+					 GFP_KERNEL);
+	else
+		ap_msg.message = kmalloc(MSGTYPE50_CRB3_MAX_MSG_SIZE,
+					 GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_cex2a_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &work;
+	rc = ICACRT_msg_to_type50CRT_msg(zdev, &ap_msg, crt);
+	if (rc)
+		goto out_free;
+	init_completion(&work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&work);
+	if (rc == 0)
+		rc = convert_response(zdev, &ap_msg, crt->outputdata,
+				      crt->outputdatalength);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+out_free:
+	kfree(ap_msg.message);
+	return rc;
+}
+
+/**
+ * The crypto operations for message type 50.
+ */
+static struct zcrypt_ops zcrypt_msgtype50_ops = {
+	.rsa_modexpo = zcrypt_cex2a_modexpo,
+	.rsa_modexpo_crt = zcrypt_cex2a_modexpo_crt,
+	.owner = THIS_MODULE,
+	.variant = MSGTYPE50_VARIANT_DEFAULT,
+};
+
+int __init zcrypt_msgtype50_init(void)
+{
+	zcrypt_msgtype_register(&zcrypt_msgtype50_ops);
+	return 0;
+}
+
+void __exit zcrypt_msgtype50_exit(void)
+{
+	zcrypt_msgtype_unregister(&zcrypt_msgtype50_ops);
+}
+
+module_init(zcrypt_msgtype50_init);
+module_exit(zcrypt_msgtype50_exit);
diff --git a/drivers/s390/crypto/zcrypt_msgtype50.h b/drivers/s390/crypto/zcrypt_msgtype50.h
new file mode 100644
index 000000000000..e56dc72c7733
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_msgtype50.h
@@ -0,0 +1,39 @@
+/*
+ *  zcrypt 2.1.0
+ *
+ *  Copyright IBM Corp. 2001, 2012
+ *  Author(s): Robert Burroughs
+ *	       Eric Rossman (edrossma@us.ibm.com)
+ *
+ *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
+ *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ZCRYPT_MSGTYPE50_H_
+#define _ZCRYPT_MSGTYPE50_H_
+
+#define MSGTYPE50_NAME			"zcrypt_msgtype50"
+#define MSGTYPE50_VARIANT_DEFAULT	0
+
+#define MSGTYPE50_CRB2_MAX_MSG_SIZE	0x390 /*sizeof(struct type50_crb2_msg)*/
+#define MSGTYPE50_CRB3_MAX_MSG_SIZE	0x710 /*sizeof(struct type50_crb3_msg)*/
+
+int zcrypt_msgtype50_init(void);
+void zcrypt_msgtype50_exit(void);
+
+#endif /* _ZCRYPT_MSGTYPE50_H_ */
diff --git a/drivers/s390/crypto/zcrypt_msgtype6.c b/drivers/s390/crypto/zcrypt_msgtype6.c
new file mode 100644
index 000000000000..7d97fa5a26d0
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_msgtype6.c
@@ -0,0 +1,856 @@
+/*
+ *  zcrypt 2.1.0
+ *
+ *  Copyright IBM Corp. 2001, 2012
+ *  Author(s): Robert Burroughs
+ *	       Eric Rossman (edrossma@us.ibm.com)
+ *
+ *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
+ *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
+ *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/atomic.h>
+#include <linux/uaccess.h>
+
+#include "ap_bus.h"
+#include "zcrypt_api.h"
+#include "zcrypt_error.h"
+#include "zcrypt_msgtype6.h"
+#include "zcrypt_cca_key.h"
+
+#define PCIXCC_MIN_MOD_SIZE_OLD	 64	/*  512 bits	*/
+#define PCIXCC_MAX_ICA_RESPONSE_SIZE 0x77c /* max size type86 v2 reply	    */
+
+#define CEIL4(x) ((((x)+3)/4)*4)
+
+struct response_type {
+	struct completion work;
+	int type;
+};
+#define PCIXCC_RESPONSE_TYPE_ICA  0
+#define PCIXCC_RESPONSE_TYPE_XCRB 1
+
+MODULE_AUTHOR("IBM Corporation");
+MODULE_DESCRIPTION("Cryptographic Coprocessor (message type 6), " \
+		   "Copyright IBM Corp. 2001, 2012");
+MODULE_LICENSE("GPL");
+
+static void zcrypt_msgtype6_receive(struct ap_device *, struct ap_message *,
+				 struct ap_message *);
+
+/**
+ * CPRB
+ *	  Note that all shorts, ints and longs are little-endian.
+ *	  All pointer fields are 32-bits long, and mean nothing
+ *
+ *	  A request CPRB is followed by a request_parameter_block.
+ *
+ *	  The request (or reply) parameter block is organized thus:
+ *	    function code
+ *	    VUD block
+ *	    key block
+ */
+struct CPRB {
+	unsigned short cprb_len;	/* CPRB length			 */
+	unsigned char cprb_ver_id;	/* CPRB version id.		 */
+	unsigned char pad_000;		/* Alignment pad byte.		 */
+	unsigned char srpi_rtcode[4];	/* SRPI return code LELONG	 */
+	unsigned char srpi_verb;	/* SRPI verb type		 */
+	unsigned char flags;		/* flags			 */
+	unsigned char func_id[2];	/* function id			 */
+	unsigned char checkpoint_flag;	/*				 */
+	unsigned char resv2;		/* reserved			 */
+	unsigned short req_parml;	/* request parameter buffer	 */
+					/* length 16-bit little endian	 */
+	unsigned char req_parmp[4];	/* request parameter buffer	 *
+					 * pointer (means nothing: the	 *
+					 * parameter buffer follows	 *
+					 * the CPRB).			 */
+	unsigned char req_datal[4];	/* request data buffer		 */
+					/* length	  ULELONG	 */
+	unsigned char req_datap[4];	/* request data buffer		 */
+					/* pointer			 */
+	unsigned short rpl_parml;	/* reply  parameter buffer	 */
+					/* length 16-bit little endian	 */
+	unsigned char pad_001[2];	/* Alignment pad bytes. ULESHORT */
+	unsigned char rpl_parmp[4];	/* reply parameter buffer	 *
+					 * pointer (means nothing: the	 *
+					 * parameter buffer follows	 *
+					 * the CPRB).			 */
+	unsigned char rpl_datal[4];	/* reply data buffer len ULELONG */
+	unsigned char rpl_datap[4];	/* reply data buffer		 */
+					/* pointer			 */
+	unsigned short ccp_rscode;	/* server reason code	ULESHORT */
+	unsigned short ccp_rtcode;	/* server return code	ULESHORT */
+	unsigned char repd_parml[2];	/* replied parameter len ULESHORT*/
+	unsigned char mac_data_len[2];	/* Mac Data Length	ULESHORT */
+	unsigned char repd_datal[4];	/* replied data length	ULELONG	 */
+	unsigned char req_pc[2];	/* PC identifier		 */
+	unsigned char res_origin[8];	/* resource origin		 */
+	unsigned char mac_value[8];	/* Mac Value			 */
+	unsigned char logon_id[8];	/* Logon Identifier		 */
+	unsigned char usage_domain[2];	/* cdx				 */
+	unsigned char resv3[18];	/* reserved for requestor	 */
+	unsigned short svr_namel;	/* server name length  ULESHORT	 */
+	unsigned char svr_name[8];	/* server name			 */
+} __packed;
+
+struct function_and_rules_block {
+	unsigned char function_code[2];
+	unsigned short ulen;
+	unsigned char only_rule[8];
+} __packed;
+
+/**
+ * The following is used to initialize the CPRBX passed to the PCIXCC/CEX2C
+ * card in a type6 message. The 3 fields that must be filled in at execution
+ * time are  req_parml, rpl_parml and usage_domain.
+ * Everything about this interface is ascii/big-endian, since the
+ * device does *not* have 'Intel inside'.
+ *
+ * The CPRBX is followed immediately by the parm block.
+ * The parm block contains:
+ * - function code ('PD' 0x5044 or 'PK' 0x504B)
+ * - rule block (one of:)
+ *   + 0x000A 'PKCS-1.2' (MCL2 'PD')
+ *   + 0x000A 'ZERO-PAD' (MCL2 'PK')
+ *   + 0x000A 'ZERO-PAD' (MCL3 'PD' or CEX2C 'PD')
+ *   + 0x000A 'MRP     ' (MCL3 'PK' or CEX2C 'PK')
+ * - VUD block
+ */
+static struct CPRBX static_cprbx = {
+	.cprb_len	=  0x00DC,
+	.cprb_ver_id	=  0x02,
+	.func_id	= {0x54, 0x32},
+};
+
+/**
+ * Convert a ICAMEX message to a type6 MEX message.
+ *
+ * @zdev: crypto device pointer
+ * @ap_msg: pointer to AP message
+ * @mex: pointer to user input data
+ *
+ * Returns 0 on success or -EFAULT.
+ */
+static int ICAMEX_msg_to_type6MEX_msgX(struct zcrypt_device *zdev,
+				       struct ap_message *ap_msg,
+				       struct ica_rsa_modexpo *mex)
+{
+	static struct type6_hdr static_type6_hdrX = {
+		.type		=  0x06,
+		.offset1	=  0x00000058,
+		.agent_id	= {'C', 'A',},
+		.function_code	= {'P', 'K'},
+	};
+	static struct function_and_rules_block static_pke_fnr = {
+		.function_code	= {'P', 'K'},
+		.ulen		= 10,
+		.only_rule	= {'M', 'R', 'P', ' ', ' ', ' ', ' ', ' '}
+	};
+	static struct function_and_rules_block static_pke_fnr_MCL2 = {
+		.function_code	= {'P', 'K'},
+		.ulen		= 10,
+		.only_rule	= {'Z', 'E', 'R', 'O', '-', 'P', 'A', 'D'}
+	};
+	struct {
+		struct type6_hdr hdr;
+		struct CPRBX cprbx;
+		struct function_and_rules_block fr;
+		unsigned short length;
+		char text[0];
+	} __packed * msg = ap_msg->message;
+	int size;
+
+	/* VUD.ciphertext */
+	msg->length = mex->inputdatalength + 2;
+	if (copy_from_user(msg->text, mex->inputdata, mex->inputdatalength))
+		return -EFAULT;
+
+	/* Set up key which is located after the variable length text. */
+	size = zcrypt_type6_mex_key_en(mex, msg->text+mex->inputdatalength, 1);
+	if (size < 0)
+		return size;
+	size += sizeof(*msg) + mex->inputdatalength;
+
+	/* message header, cprbx and f&r */
+	msg->hdr = static_type6_hdrX;
+	msg->hdr.ToCardLen1 = size - sizeof(msg->hdr);
+	msg->hdr.FromCardLen1 = PCIXCC_MAX_ICA_RESPONSE_SIZE - sizeof(msg->hdr);
+
+	msg->cprbx = static_cprbx;
+	msg->cprbx.domain = AP_QID_QUEUE(zdev->ap_dev->qid);
+	msg->cprbx.rpl_msgbl = msg->hdr.FromCardLen1;
+
+	msg->fr = (zdev->user_space_type == ZCRYPT_PCIXCC_MCL2) ?
+		static_pke_fnr_MCL2 : static_pke_fnr;
+
+	msg->cprbx.req_parml = size - sizeof(msg->hdr) - sizeof(msg->cprbx);
+
+	ap_msg->length = size;
+	return 0;
+}
+
+/**
+ * Convert a ICACRT message to a type6 CRT message.
+ *
+ * @zdev: crypto device pointer
+ * @ap_msg: pointer to AP message
+ * @crt: pointer to user input data
+ *
+ * Returns 0 on success or -EFAULT.
+ */
+static int ICACRT_msg_to_type6CRT_msgX(struct zcrypt_device *zdev,
+				       struct ap_message *ap_msg,
+				       struct ica_rsa_modexpo_crt *crt)
+{
+	static struct type6_hdr static_type6_hdrX = {
+		.type		=  0x06,
+		.offset1	=  0x00000058,
+		.agent_id	= {'C', 'A',},
+		.function_code	= {'P', 'D'},
+	};
+	static struct function_and_rules_block static_pkd_fnr = {
+		.function_code	= {'P', 'D'},
+		.ulen		= 10,
+		.only_rule	= {'Z', 'E', 'R', 'O', '-', 'P', 'A', 'D'}
+	};
+
+	static struct function_and_rules_block static_pkd_fnr_MCL2 = {
+		.function_code	= {'P', 'D'},
+		.ulen		= 10,
+		.only_rule	= {'P', 'K', 'C', 'S', '-', '1', '.', '2'}
+	};
+	struct {
+		struct type6_hdr hdr;
+		struct CPRBX cprbx;
+		struct function_and_rules_block fr;
+		unsigned short length;
+		char text[0];
+	} __packed * msg = ap_msg->message;
+	int size;
+
+	/* VUD.ciphertext */
+	msg->length = crt->inputdatalength + 2;
+	if (copy_from_user(msg->text, crt->inputdata, crt->inputdatalength))
+		return -EFAULT;
+
+	/* Set up key which is located after the variable length text. */
+	size = zcrypt_type6_crt_key(crt, msg->text + crt->inputdatalength, 1);
+	if (size < 0)
+		return size;
+	size += sizeof(*msg) + crt->inputdatalength;	/* total size of msg */
+
+	/* message header, cprbx and f&r */
+	msg->hdr = static_type6_hdrX;
+	msg->hdr.ToCardLen1 = size -  sizeof(msg->hdr);
+	msg->hdr.FromCardLen1 = PCIXCC_MAX_ICA_RESPONSE_SIZE - sizeof(msg->hdr);
+
+	msg->cprbx = static_cprbx;
+	msg->cprbx.domain = AP_QID_QUEUE(zdev->ap_dev->qid);
+	msg->cprbx.req_parml = msg->cprbx.rpl_msgbl =
+		size - sizeof(msg->hdr) - sizeof(msg->cprbx);
+
+	msg->fr = (zdev->user_space_type == ZCRYPT_PCIXCC_MCL2) ?
+		static_pkd_fnr_MCL2 : static_pkd_fnr;
+
+	ap_msg->length = size;
+	return 0;
+}
+
+/**
+ * Convert a XCRB message to a type6 CPRB message.
+ *
+ * @zdev: crypto device pointer
+ * @ap_msg: pointer to AP message
+ * @xcRB: pointer to user input data
+ *
+ * Returns 0 on success or -EFAULT, -EINVAL.
+ */
+struct type86_fmt2_msg {
+	struct type86_hdr hdr;
+	struct type86_fmt2_ext fmt2;
+} __packed;
+
+static int XCRB_msg_to_type6CPRB_msgX(struct zcrypt_device *zdev,
+				       struct ap_message *ap_msg,
+				       struct ica_xcRB *xcRB)
+{
+	static struct type6_hdr static_type6_hdrX = {
+		.type		=  0x06,
+		.offset1	=  0x00000058,
+	};
+	struct {
+		struct type6_hdr hdr;
+		struct CPRBX cprbx;
+	} __packed * msg = ap_msg->message;
+
+	int rcblen = CEIL4(xcRB->request_control_blk_length);
+	int replylen;
+	char *req_data = ap_msg->message + sizeof(struct type6_hdr) + rcblen;
+	char *function_code;
+
+	/* length checks */
+	ap_msg->length = sizeof(struct type6_hdr) +
+		CEIL4(xcRB->request_control_blk_length) +
+		xcRB->request_data_length;
+	if (ap_msg->length > MSGTYPE06_MAX_MSG_SIZE)
+		return -EINVAL;
+	replylen = sizeof(struct type86_fmt2_msg) +
+		CEIL4(xcRB->reply_control_blk_length) +
+		xcRB->reply_data_length;
+	if (replylen > MSGTYPE06_MAX_MSG_SIZE)
+		return -EINVAL;
+
+	/* prepare type6 header */
+	msg->hdr = static_type6_hdrX;
+	memcpy(msg->hdr.agent_id , &(xcRB->agent_ID), sizeof(xcRB->agent_ID));
+	msg->hdr.ToCardLen1 = xcRB->request_control_blk_length;
+	if (xcRB->request_data_length) {
+		msg->hdr.offset2 = msg->hdr.offset1 + rcblen;
+		msg->hdr.ToCardLen2 = xcRB->request_data_length;
+	}
+	msg->hdr.FromCardLen1 = xcRB->reply_control_blk_length;
+	msg->hdr.FromCardLen2 = xcRB->reply_data_length;
+
+	/* prepare CPRB */
+	if (copy_from_user(&(msg->cprbx), xcRB->request_control_blk_addr,
+		    xcRB->request_control_blk_length))
+		return -EFAULT;
+	if (msg->cprbx.cprb_len + sizeof(msg->hdr.function_code) >
+	    xcRB->request_control_blk_length)
+		return -EINVAL;
+	function_code = ((unsigned char *)&msg->cprbx) + msg->cprbx.cprb_len;
+	memcpy(msg->hdr.function_code, function_code,
+	       sizeof(msg->hdr.function_code));
+
+	if (memcmp(function_code, "US", 2) == 0)
+		ap_msg->special = 1;
+	else
+		ap_msg->special = 0;
+
+	/* copy data block */
+	if (xcRB->request_data_length &&
+	    copy_from_user(req_data, xcRB->request_data_address,
+		xcRB->request_data_length))
+		return -EFAULT;
+	return 0;
+}
+
+/**
+ * Copy results from a type 86 ICA reply message back to user space.
+ *
+ * @zdev: crypto device pointer
+ * @reply: reply AP message.
+ * @data: pointer to user output data
+ * @length: size of user output data
+ *
+ * Returns 0 on success or -EINVAL, -EFAULT, -EAGAIN in case of an error.
+ */
+struct type86x_reply {
+	struct type86_hdr hdr;
+	struct type86_fmt2_ext fmt2;
+	struct CPRBX cprbx;
+	unsigned char pad[4];	/* 4 byte function code/rules block ? */
+	unsigned short length;
+	char text[0];
+} __packed;
+
+static int convert_type86_ica(struct zcrypt_device *zdev,
+			  struct ap_message *reply,
+			  char __user *outputdata,
+			  unsigned int outputdatalength)
+{
+	static unsigned char static_pad[] = {
+		0x00, 0x02,
+		0x1B, 0x7B, 0x5D, 0xB5, 0x75, 0x01, 0x3D, 0xFD,
+		0x8D, 0xD1, 0xC7, 0x03, 0x2D, 0x09, 0x23, 0x57,
+		0x89, 0x49, 0xB9, 0x3F, 0xBB, 0x99, 0x41, 0x5B,
+		0x75, 0x21, 0x7B, 0x9D, 0x3B, 0x6B, 0x51, 0x39,
+		0xBB, 0x0D, 0x35, 0xB9, 0x89, 0x0F, 0x93, 0xA5,
+		0x0B, 0x47, 0xF1, 0xD3, 0xBB, 0xCB, 0xF1, 0x9D,
+		0x23, 0x73, 0x71, 0xFF, 0xF3, 0xF5, 0x45, 0xFB,
+		0x61, 0x29, 0x23, 0xFD, 0xF1, 0x29, 0x3F, 0x7F,
+		0x17, 0xB7, 0x1B, 0xA9, 0x19, 0xBD, 0x57, 0xA9,
+		0xD7, 0x95, 0xA3, 0xCB, 0xED, 0x1D, 0xDB, 0x45,
+		0x7D, 0x11, 0xD1, 0x51, 0x1B, 0xED, 0x71, 0xE9,
+		0xB1, 0xD1, 0xAB, 0xAB, 0x21, 0x2B, 0x1B, 0x9F,
+		0x3B, 0x9F, 0xF7, 0xF7, 0xBD, 0x63, 0xEB, 0xAD,
+		0xDF, 0xB3, 0x6F, 0x5B, 0xDB, 0x8D, 0xA9, 0x5D,
+		0xE3, 0x7D, 0x77, 0x49, 0x47, 0xF5, 0xA7, 0xFD,
+		0xAB, 0x2F, 0x27, 0x35, 0x77, 0xD3, 0x49, 0xC9,
+		0x09, 0xEB, 0xB1, 0xF9, 0xBF, 0x4B, 0xCB, 0x2B,
+		0xEB, 0xEB, 0x05, 0xFF, 0x7D, 0xC7, 0x91, 0x8B,
+		0x09, 0x83, 0xB9, 0xB9, 0x69, 0x33, 0x39, 0x6B,
+		0x79, 0x75, 0x19, 0xBF, 0xBB, 0x07, 0x1D, 0xBD,
+		0x29, 0xBF, 0x39, 0x95, 0x93, 0x1D, 0x35, 0xC7,
+		0xC9, 0x4D, 0xE5, 0x97, 0x0B, 0x43, 0x9B, 0xF1,
+		0x16, 0x93, 0x03, 0x1F, 0xA5, 0xFB, 0xDB, 0xF3,
+		0x27, 0x4F, 0x27, 0x61, 0x05, 0x1F, 0xB9, 0x23,
+		0x2F, 0xC3, 0x81, 0xA9, 0x23, 0x71, 0x55, 0x55,
+		0xEB, 0xED, 0x41, 0xE5, 0xF3, 0x11, 0xF1, 0x43,
+		0x69, 0x03, 0xBD, 0x0B, 0x37, 0x0F, 0x51, 0x8F,
+		0x0B, 0xB5, 0x89, 0x5B, 0x67, 0xA9, 0xD9, 0x4F,
+		0x01, 0xF9, 0x21, 0x77, 0x37, 0x73, 0x79, 0xC5,
+		0x7F, 0x51, 0xC1, 0xCF, 0x97, 0xA1, 0x75, 0xAD,
+		0x35, 0x9D, 0xD3, 0xD3, 0xA7, 0x9D, 0x5D, 0x41,
+		0x6F, 0x65, 0x1B, 0xCF, 0xA9, 0x87, 0x91, 0x09
+	};
+	struct type86x_reply *msg = reply->message;
+	unsigned short service_rc, service_rs;
+	unsigned int reply_len, pad_len;
+	char *data;
+
+	service_rc = msg->cprbx.ccp_rtcode;
+	if (unlikely(service_rc != 0)) {
+		service_rs = msg->cprbx.ccp_rscode;
+		if (service_rc == 8 && service_rs == 66)
+			return -EINVAL;
+		if (service_rc == 8 && service_rs == 65)
+			return -EINVAL;
+		if (service_rc == 8 && service_rs == 770)
+			return -EINVAL;
+		if (service_rc == 8 && service_rs == 783) {
+			zdev->min_mod_size = PCIXCC_MIN_MOD_SIZE_OLD;
+			return -EAGAIN;
+		}
+		if (service_rc == 12 && service_rs == 769)
+			return -EINVAL;
+		if (service_rc == 8 && service_rs == 72)
+			return -EINVAL;
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+	data = msg->text;
+	reply_len = msg->length - 2;
+	if (reply_len > outputdatalength)
+		return -EINVAL;
+	/*
+	 * For all encipher requests, the length of the ciphertext (reply_len)
+	 * will always equal the modulus length. For MEX decipher requests
+	 * the output needs to get padded. Minimum pad size is 10.
+	 *
+	 * Currently, the cases where padding will be added is for:
+	 * - PCIXCC_MCL2 using a CRT form token (since PKD didn't support
+	 *   ZERO-PAD and CRT is only supported for PKD requests)
+	 * - PCICC, always
+	 */
+	pad_len = outputdatalength - reply_len;
+	if (pad_len > 0) {
+		if (pad_len < 10)
+			return -EINVAL;
+		/* 'restore' padding left in the PCICC/PCIXCC card. */
+		if (copy_to_user(outputdata, static_pad, pad_len - 1))
+			return -EFAULT;
+		if (put_user(0, outputdata + pad_len - 1))
+			return -EFAULT;
+	}
+	/* Copy the crypto response to user space. */
+	if (copy_to_user(outputdata + pad_len, data, reply_len))
+		return -EFAULT;
+	return 0;
+}
+
+/**
+ * Copy results from a type 86 XCRB reply message back to user space.
+ *
+ * @zdev: crypto device pointer
+ * @reply: reply AP message.
+ * @xcRB: pointer to XCRB
+ *
+ * Returns 0 on success or -EINVAL, -EFAULT, -EAGAIN in case of an error.
+ */
+static int convert_type86_xcrb(struct zcrypt_device *zdev,
+			       struct ap_message *reply,
+			       struct ica_xcRB *xcRB)
+{
+	struct type86_fmt2_msg *msg = reply->message;
+	char *data = reply->message;
+
+	/* Copy CPRB to user */
+	if (copy_to_user(xcRB->reply_control_blk_addr,
+		data + msg->fmt2.offset1, msg->fmt2.count1))
+		return -EFAULT;
+	xcRB->reply_control_blk_length = msg->fmt2.count1;
+
+	/* Copy data buffer to user */
+	if (msg->fmt2.count2)
+		if (copy_to_user(xcRB->reply_data_addr,
+			data + msg->fmt2.offset2, msg->fmt2.count2))
+			return -EFAULT;
+	xcRB->reply_data_length = msg->fmt2.count2;
+	return 0;
+}
+
+static int convert_type86_rng(struct zcrypt_device *zdev,
+			  struct ap_message *reply,
+			  char *buffer)
+{
+	struct {
+		struct type86_hdr hdr;
+		struct type86_fmt2_ext fmt2;
+		struct CPRBX cprbx;
+	} __packed * msg = reply->message;
+	char *data = reply->message;
+
+	if (msg->cprbx.ccp_rtcode != 0 || msg->cprbx.ccp_rscode != 0)
+		return -EINVAL;
+	memcpy(buffer, data + msg->fmt2.offset2, msg->fmt2.count2);
+	return msg->fmt2.count2;
+}
+
+static int convert_response_ica(struct zcrypt_device *zdev,
+			    struct ap_message *reply,
+			    char __user *outputdata,
+			    unsigned int outputdatalength)
+{
+	struct type86x_reply *msg = reply->message;
+
+	/* Response type byte is the second byte in the response. */
+	switch (((unsigned char *) reply->message)[1]) {
+	case TYPE82_RSP_CODE:
+	case TYPE88_RSP_CODE:
+		return convert_error(zdev, reply);
+	case TYPE86_RSP_CODE:
+		if (msg->cprbx.ccp_rtcode &&
+		   (msg->cprbx.ccp_rscode == 0x14f) &&
+		   (outputdatalength > 256)) {
+			if (zdev->max_exp_bit_length <= 17) {
+				zdev->max_exp_bit_length = 17;
+				return -EAGAIN;
+			} else
+				return -EINVAL;
+		}
+		if (msg->hdr.reply_code)
+			return convert_error(zdev, reply);
+		if (msg->cprbx.cprb_ver_id == 0x02)
+			return convert_type86_ica(zdev, reply,
+						  outputdata, outputdatalength);
+		/* Fall through, no break, incorrect cprb version is an unknown
+		 * response */
+	default: /* Unknown response type, this should NEVER EVER happen */
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+}
+
+static int convert_response_xcrb(struct zcrypt_device *zdev,
+			    struct ap_message *reply,
+			    struct ica_xcRB *xcRB)
+{
+	struct type86x_reply *msg = reply->message;
+
+	/* Response type byte is the second byte in the response. */
+	switch (((unsigned char *) reply->message)[1]) {
+	case TYPE82_RSP_CODE:
+	case TYPE88_RSP_CODE:
+		xcRB->status = 0x0008044DL; /* HDD_InvalidParm */
+		return convert_error(zdev, reply);
+	case TYPE86_RSP_CODE:
+		if (msg->hdr.reply_code) {
+			memcpy(&(xcRB->status), msg->fmt2.apfs, sizeof(u32));
+			return convert_error(zdev, reply);
+		}
+		if (msg->cprbx.cprb_ver_id == 0x02)
+			return convert_type86_xcrb(zdev, reply, xcRB);
+		/* Fall through, no break, incorrect cprb version is an unknown
+		 * response */
+	default: /* Unknown response type, this should NEVER EVER happen */
+		xcRB->status = 0x0008044DL; /* HDD_InvalidParm */
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+}
+
+static int convert_response_rng(struct zcrypt_device *zdev,
+				 struct ap_message *reply,
+				 char *data)
+{
+	struct type86x_reply *msg = reply->message;
+
+	switch (msg->hdr.type) {
+	case TYPE82_RSP_CODE:
+	case TYPE88_RSP_CODE:
+		return -EINVAL;
+	case TYPE86_RSP_CODE:
+		if (msg->hdr.reply_code)
+			return -EINVAL;
+		if (msg->cprbx.cprb_ver_id == 0x02)
+			return convert_type86_rng(zdev, reply, data);
+		/* Fall through, no break, incorrect cprb version is an unknown
+		 * response */
+	default: /* Unknown response type, this should NEVER EVER happen */
+		zdev->online = 0;
+		return -EAGAIN;	/* repeat the request on a different device. */
+	}
+}
+
+/**
+ * This function is called from the AP bus code after a crypto request
+ * "msg" has finished with the reply message "reply".
+ * It is called from tasklet context.
+ * @ap_dev: pointer to the AP device
+ * @msg: pointer to the AP message
+ * @reply: pointer to the AP reply message
+ */
+static void zcrypt_msgtype6_receive(struct ap_device *ap_dev,
+				  struct ap_message *msg,
+				  struct ap_message *reply)
+{
+	static struct error_hdr error_reply = {
+		.type = TYPE82_RSP_CODE,
+		.reply_code = REP82_ERROR_MACHINE_FAILURE,
+	};
+	struct response_type *resp_type =
+		(struct response_type *) msg->private;
+	struct type86x_reply *t86r;
+	int length;
+
+	/* Copy the reply message to the request message buffer. */
+	if (IS_ERR(reply)) {
+		memcpy(msg->message, &error_reply, sizeof(error_reply));
+		goto out;
+	}
+	t86r = reply->message;
+	if (t86r->hdr.type == TYPE86_RSP_CODE &&
+		 t86r->cprbx.cprb_ver_id == 0x02) {
+		switch (resp_type->type) {
+		case PCIXCC_RESPONSE_TYPE_ICA:
+			length = sizeof(struct type86x_reply)
+				+ t86r->length - 2;
+			length = min(PCIXCC_MAX_ICA_RESPONSE_SIZE, length);
+			memcpy(msg->message, reply->message, length);
+			break;
+		case PCIXCC_RESPONSE_TYPE_XCRB:
+			length = t86r->fmt2.offset2 + t86r->fmt2.count2;
+			length = min(MSGTYPE06_MAX_MSG_SIZE, length);
+			memcpy(msg->message, reply->message, length);
+			break;
+		default:
+			memcpy(msg->message, &error_reply,
+			       sizeof(error_reply));
+		}
+	} else
+		memcpy(msg->message, reply->message, sizeof(error_reply));
+out:
+	complete(&(resp_type->work));
+}
+
+static atomic_t zcrypt_step = ATOMIC_INIT(0);
+
+/**
+ * The request distributor calls this function if it picked the PCIXCC/CEX2C
+ * device to handle a modexpo request.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  PCIXCC/CEX2C device to the request distributor
+ * @mex: pointer to the modexpo request buffer
+ */
+static long zcrypt_msgtype6_modexpo(struct zcrypt_device *zdev,
+				  struct ica_rsa_modexpo *mex)
+{
+	struct ap_message ap_msg;
+	struct response_type resp_type = {
+		.type = PCIXCC_RESPONSE_TYPE_ICA,
+	};
+	int rc;
+
+	ap_init_message(&ap_msg);
+	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_msgtype6_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &resp_type;
+	rc = ICAMEX_msg_to_type6MEX_msgX(zdev, &ap_msg, mex);
+	if (rc)
+		goto out_free;
+	init_completion(&resp_type.work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&resp_type.work);
+	if (rc == 0)
+		rc = convert_response_ica(zdev, &ap_msg, mex->outputdata,
+					  mex->outputdatalength);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+out_free:
+	free_page((unsigned long) ap_msg.message);
+	return rc;
+}
+
+/**
+ * The request distributor calls this function if it picked the PCIXCC/CEX2C
+ * device to handle a modexpo_crt request.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  PCIXCC/CEX2C device to the request distributor
+ * @crt: pointer to the modexpoc_crt request buffer
+ */
+static long zcrypt_msgtype6_modexpo_crt(struct zcrypt_device *zdev,
+				      struct ica_rsa_modexpo_crt *crt)
+{
+	struct ap_message ap_msg;
+	struct response_type resp_type = {
+		.type = PCIXCC_RESPONSE_TYPE_ICA,
+	};
+	int rc;
+
+	ap_init_message(&ap_msg);
+	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_msgtype6_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &resp_type;
+	rc = ICACRT_msg_to_type6CRT_msgX(zdev, &ap_msg, crt);
+	if (rc)
+		goto out_free;
+	init_completion(&resp_type.work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&resp_type.work);
+	if (rc == 0)
+		rc = convert_response_ica(zdev, &ap_msg, crt->outputdata,
+					  crt->outputdatalength);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+out_free:
+	free_page((unsigned long) ap_msg.message);
+	return rc;
+}
+
+/**
+ * The request distributor calls this function if it picked the PCIXCC/CEX2C
+ * device to handle a send_cprb request.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  PCIXCC/CEX2C device to the request distributor
+ * @xcRB: pointer to the send_cprb request buffer
+ */
+static long zcrypt_msgtype6_send_cprb(struct zcrypt_device *zdev,
+				    struct ica_xcRB *xcRB)
+{
+	struct ap_message ap_msg;
+	struct response_type resp_type = {
+		.type = PCIXCC_RESPONSE_TYPE_XCRB,
+	};
+	int rc;
+
+	ap_init_message(&ap_msg);
+	ap_msg.message = kmalloc(MSGTYPE06_MAX_MSG_SIZE, GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_msgtype6_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &resp_type;
+	rc = XCRB_msg_to_type6CPRB_msgX(zdev, &ap_msg, xcRB);
+	if (rc)
+		goto out_free;
+	init_completion(&resp_type.work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&resp_type.work);
+	if (rc == 0)
+		rc = convert_response_xcrb(zdev, &ap_msg, xcRB);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+out_free:
+	kzfree(ap_msg.message);
+	return rc;
+}
+
+/**
+ * The request distributor calls this function if it picked the PCIXCC/CEX2C
+ * device to generate random data.
+ * @zdev: pointer to zcrypt_device structure that identifies the
+ *	  PCIXCC/CEX2C device to the request distributor
+ * @buffer: pointer to a memory page to return random data
+ */
+
+static long zcrypt_msgtype6_rng(struct zcrypt_device *zdev,
+				    char *buffer)
+{
+	struct ap_message ap_msg;
+	struct response_type resp_type = {
+		.type = PCIXCC_RESPONSE_TYPE_XCRB,
+	};
+	int rc;
+
+	ap_init_message(&ap_msg);
+	ap_msg.message = kmalloc(MSGTYPE06_MAX_MSG_SIZE, GFP_KERNEL);
+	if (!ap_msg.message)
+		return -ENOMEM;
+	ap_msg.receive = zcrypt_msgtype6_receive;
+	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
+				atomic_inc_return(&zcrypt_step);
+	ap_msg.private = &resp_type;
+	rng_type6CPRB_msgX(zdev->ap_dev, &ap_msg, ZCRYPT_RNG_BUFFER_SIZE);
+	init_completion(&resp_type.work);
+	ap_queue_message(zdev->ap_dev, &ap_msg);
+	rc = wait_for_completion_interruptible(&resp_type.work);
+	if (rc == 0)
+		rc = convert_response_rng(zdev, &ap_msg, buffer);
+	else
+		/* Signal pending. */
+		ap_cancel_message(zdev->ap_dev, &ap_msg);
+	kfree(ap_msg.message);
+	return rc;
+}
+
+/**
+ * The crypto operations for a PCIXCC/CEX2C card.
+ */
+static struct zcrypt_ops zcrypt_msgtype6_norng_ops = {
+	.owner = THIS_MODULE,
+	.variant = MSGTYPE06_VARIANT_NORNG,
+	.rsa_modexpo = zcrypt_msgtype6_modexpo,
+	.rsa_modexpo_crt = zcrypt_msgtype6_modexpo_crt,
+	.send_cprb = zcrypt_msgtype6_send_cprb,
+};
+
+static struct zcrypt_ops zcrypt_msgtype6_ops = {
+	.owner = THIS_MODULE,
+	.variant = MSGTYPE06_VARIANT_DEFAULT,
+	.rsa_modexpo = zcrypt_msgtype6_modexpo,
+	.rsa_modexpo_crt = zcrypt_msgtype6_modexpo_crt,
+	.send_cprb = zcrypt_msgtype6_send_cprb,
+	.rng = zcrypt_msgtype6_rng,
+};
+
+int __init zcrypt_msgtype6_init(void)
+{
+	zcrypt_msgtype_register(&zcrypt_msgtype6_norng_ops);
+	zcrypt_msgtype_register(&zcrypt_msgtype6_ops);
+	return 0;
+}
+
+void __exit zcrypt_msgtype6_exit(void)
+{
+	zcrypt_msgtype_unregister(&zcrypt_msgtype6_norng_ops);
+	zcrypt_msgtype_unregister(&zcrypt_msgtype6_ops);
+}
+
+module_init(zcrypt_msgtype6_init);
+module_exit(zcrypt_msgtype6_exit);
diff --git a/drivers/s390/crypto/zcrypt_msgtype6.h b/drivers/s390/crypto/zcrypt_msgtype6.h
new file mode 100644
index 000000000000..1e500d3c0735
--- /dev/null
+++ b/drivers/s390/crypto/zcrypt_msgtype6.h
@@ -0,0 +1,169 @@
+/*
+ *  zcrypt 2.1.0
+ *
+ *  Copyright IBM Corp. 2001, 2012
+ *  Author(s): Robert Burroughs
+ *	       Eric Rossman (edrossma@us.ibm.com)
+ *
+ *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
+ *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ZCRYPT_MSGTYPE6_H_
+#define _ZCRYPT_MSGTYPE6_H_
+
+#include <asm/zcrypt.h>
+
+#define MSGTYPE06_NAME			"zcrypt_msgtype6"
+#define MSGTYPE06_VARIANT_DEFAULT	0
+#define MSGTYPE06_VARIANT_NORNG		1
+
+#define MSGTYPE06_MAX_MSG_SIZE		(12*1024)
+
+/**
+ * The type 6 message family is associated with PCICC or PCIXCC cards.
+ *
+ * It contains a message header followed by a CPRB, both of which
+ * are described below.
+ *
+ * Note that all reserved fields must be zeroes.
+ */
+struct type6_hdr {
+	unsigned char reserved1;	/* 0x00				*/
+	unsigned char type;		/* 0x06				*/
+	unsigned char reserved2[2];	/* 0x0000			*/
+	unsigned char right[4];		/* 0x00000000			*/
+	unsigned char reserved3[2];	/* 0x0000			*/
+	unsigned char reserved4[2];	/* 0x0000			*/
+	unsigned char apfs[4];		/* 0x00000000			*/
+	unsigned int  offset1;		/* 0x00000058 (offset to CPRB)	*/
+	unsigned int  offset2;		/* 0x00000000			*/
+	unsigned int  offset3;		/* 0x00000000			*/
+	unsigned int  offset4;		/* 0x00000000			*/
+	unsigned char agent_id[16];	/* PCICC:			*/
+					/*    0x0100			*/
+					/*    0x4343412d4150504c202020	*/
+					/*    0x010101			*/
+					/* PCIXCC:			*/
+					/*    0x4341000000000000	*/
+					/*    0x0000000000000000	*/
+	unsigned char rqid[2];		/* rqid.  internal to 603	*/
+	unsigned char reserved5[2];	/* 0x0000			*/
+	unsigned char function_code[2];	/* for PKD, 0x5044 (ascii 'PD')	*/
+	unsigned char reserved6[2];	/* 0x0000			*/
+	unsigned int  ToCardLen1;	/* (request CPRB len + 3) & -4	*/
+	unsigned int  ToCardLen2;	/* db len 0x00000000 for PKD	*/
+	unsigned int  ToCardLen3;	/* 0x00000000			*/
+	unsigned int  ToCardLen4;	/* 0x00000000			*/
+	unsigned int  FromCardLen1;	/* response buffer length	*/
+	unsigned int  FromCardLen2;	/* db len 0x00000000 for PKD	*/
+	unsigned int  FromCardLen3;	/* 0x00000000			*/
+	unsigned int  FromCardLen4;	/* 0x00000000			*/
+} __packed;
+
+/**
+ * The type 86 message family is associated with PCICC and PCIXCC cards.
+ *
+ * It contains a message header followed by a CPRB.  The CPRB is
+ * the same as the request CPRB, which is described above.
+ *
+ * If format is 1, an error condition exists and no data beyond
+ * the 8-byte message header is of interest.
+ *
+ * The non-error message is shown below.
+ *
+ * Note that all reserved fields must be zeroes.
+ */
+struct type86_hdr {
+	unsigned char reserved1;	/* 0x00				*/
+	unsigned char type;		/* 0x86				*/
+	unsigned char format;		/* 0x01 (error) or 0x02 (ok)	*/
+	unsigned char reserved2;	/* 0x00				*/
+	unsigned char reply_code;	/* reply code (see above)	*/
+	unsigned char reserved3[3];	/* 0x000000			*/
+} __packed;
+
+#define TYPE86_RSP_CODE 0x86
+#define TYPE86_FMT2	0x02
+
+struct type86_fmt2_ext {
+	unsigned char	  reserved[4];	/* 0x00000000			*/
+	unsigned char	  apfs[4];	/* final status			*/
+	unsigned int	  count1;	/* length of CPRB + parameters	*/
+	unsigned int	  offset1;	/* offset to CPRB		*/
+	unsigned int	  count2;	/* 0x00000000			*/
+	unsigned int	  offset2;	/* db offset 0x00000000 for PKD	*/
+	unsigned int	  count3;	/* 0x00000000			*/
+	unsigned int	  offset3;	/* 0x00000000			*/
+	unsigned int	  count4;	/* 0x00000000			*/
+	unsigned int	  offset4;	/* 0x00000000			*/
+} __packed;
+
+/**
+ * Prepare a type6 CPRB message for random number generation
+ *
+ * @ap_dev: AP device pointer
+ * @ap_msg: pointer to AP message
+ */
+static inline void rng_type6CPRB_msgX(struct ap_device *ap_dev,
+			       struct ap_message *ap_msg,
+			       unsigned random_number_length)
+{
+	struct {
+		struct type6_hdr hdr;
+		struct CPRBX cprbx;
+		char function_code[2];
+		short int rule_length;
+		char rule[8];
+		short int verb_length;
+		short int key_length;
+	} __packed * msg = ap_msg->message;
+	static struct type6_hdr static_type6_hdrX = {
+		.type		= 0x06,
+		.offset1	= 0x00000058,
+		.agent_id	= {'C', 'A'},
+		.function_code	= {'R', 'L'},
+		.ToCardLen1	= sizeof(*msg) - sizeof(msg->hdr),
+		.FromCardLen1	= sizeof(*msg) - sizeof(msg->hdr),
+	};
+	static struct CPRBX local_cprbx = {
+		.cprb_len	= 0x00dc,
+		.cprb_ver_id	= 0x02,
+		.func_id	= {0x54, 0x32},
+		.req_parml	= sizeof(*msg) - sizeof(msg->hdr) -
+				  sizeof(msg->cprbx),
+		.rpl_msgbl	= sizeof(*msg) - sizeof(msg->hdr),
+	};
+
+	msg->hdr = static_type6_hdrX;
+	msg->hdr.FromCardLen2 = random_number_length,
+	msg->cprbx = local_cprbx;
+	msg->cprbx.rpl_datal = random_number_length,
+	msg->cprbx.domain = AP_QID_QUEUE(ap_dev->qid);
+	memcpy(msg->function_code, msg->hdr.function_code, 0x02);
+	msg->rule_length = 0x0a;
+	memcpy(msg->rule, "RANDOM  ", 8);
+	msg->verb_length = 0x02;
+	msg->key_length = 0x02;
+	ap_msg->length = sizeof(*msg);
+}
+
+int zcrypt_msgtype6_init(void);
+void zcrypt_msgtype6_exit(void);
+
+#endif /* _ZCRYPT_MSGTYPE6_H_ */
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index ccb4f8b60c75..c7275e303a0d 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -1,13 +1,14 @@
 /*
  *  zcrypt 2.1.0
  *
- *  Copyright IBM Corp. 2001, 2006
+ *  Copyright IBM Corp. 2001, 2012
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
  *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
  *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
  *				  Ralph Wuerthner <rwuerthn@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.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
@@ -35,9 +36,10 @@
 #include "ap_bus.h"
 #include "zcrypt_api.h"
 #include "zcrypt_error.h"
-#include "zcrypt_pcicc.h"
+#include "zcrypt_msgtype6.h"
 #include "zcrypt_pcixcc.h"
 #include "zcrypt_cca_key.h"
+#include "zcrypt_msgtype6.h"
 
 #define PCIXCC_MIN_MOD_SIZE	 16	/*  128 bits	*/
 #define PCIXCC_MIN_MOD_SIZE_OLD	 64	/*  512 bits	*/
@@ -75,14 +77,12 @@ static struct ap_device_id zcrypt_pcixcc_ids[] = {
 
 MODULE_DEVICE_TABLE(ap, zcrypt_pcixcc_ids);
 MODULE_AUTHOR("IBM Corporation");
-MODULE_DESCRIPTION("PCIXCC Cryptographic Coprocessor device driver, "
-		   "Copyright IBM Corp. 2001, 2006");
+MODULE_DESCRIPTION("PCIXCC Cryptographic Coprocessor device driver, " \
+		   "Copyright IBM Corp. 2001, 2012");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_pcixcc_probe(struct ap_device *ap_dev);
 static void zcrypt_pcixcc_remove(struct ap_device *ap_dev);
-static void zcrypt_pcixcc_receive(struct ap_device *, struct ap_message *,
-				 struct ap_message *);
 
 static struct ap_driver zcrypt_pcixcc_driver = {
 	.probe = zcrypt_pcixcc_probe,
@@ -92,766 +92,6 @@ static struct ap_driver zcrypt_pcixcc_driver = {
 };
 
 /**
- * The following is used to initialize the CPRBX passed to the PCIXCC/CEX2C
- * card in a type6 message. The 3 fields that must be filled in at execution
- * time are  req_parml, rpl_parml and usage_domain.
- * Everything about this interface is ascii/big-endian, since the
- * device does *not* have 'Intel inside'.
- *
- * The CPRBX is followed immediately by the parm block.
- * The parm block contains:
- * - function code ('PD' 0x5044 or 'PK' 0x504B)
- * - rule block (one of:)
- *   + 0x000A 'PKCS-1.2' (MCL2 'PD')
- *   + 0x000A 'ZERO-PAD' (MCL2 'PK')
- *   + 0x000A 'ZERO-PAD' (MCL3 'PD' or CEX2C 'PD')
- *   + 0x000A 'MRP     ' (MCL3 'PK' or CEX2C 'PK')
- * - VUD block
- */
-static struct CPRBX static_cprbx = {
-	.cprb_len	=  0x00DC,
-	.cprb_ver_id	=  0x02,
-	.func_id	= {0x54,0x32},
-};
-
-/**
- * Convert a ICAMEX message to a type6 MEX message.
- *
- * @zdev: crypto device pointer
- * @ap_msg: pointer to AP message
- * @mex: pointer to user input data
- *
- * Returns 0 on success or -EFAULT.
- */
-static int ICAMEX_msg_to_type6MEX_msgX(struct zcrypt_device *zdev,
-				       struct ap_message *ap_msg,
-				       struct ica_rsa_modexpo *mex)
-{
-	static struct type6_hdr static_type6_hdrX = {
-		.type		=  0x06,
-		.offset1	=  0x00000058,
-		.agent_id	= {'C','A',},
-		.function_code	= {'P','K'},
-	};
-	static struct function_and_rules_block static_pke_fnr = {
-		.function_code	= {'P','K'},
-		.ulen		= 10,
-		.only_rule	= {'M','R','P',' ',' ',' ',' ',' '}
-	};
-	static struct function_and_rules_block static_pke_fnr_MCL2 = {
-		.function_code	= {'P','K'},
-		.ulen		= 10,
-		.only_rule	= {'Z','E','R','O','-','P','A','D'}
-	};
-	struct {
-		struct type6_hdr hdr;
-		struct CPRBX cprbx;
-		struct function_and_rules_block fr;
-		unsigned short length;
-		char text[0];
-	} __attribute__((packed)) *msg = ap_msg->message;
-	int size;
-
-	/* VUD.ciphertext */
-	msg->length = mex->inputdatalength + 2;
-	if (copy_from_user(msg->text, mex->inputdata, mex->inputdatalength))
-		return -EFAULT;
-
-	/* Set up key which is located after the variable length text. */
-	size = zcrypt_type6_mex_key_en(mex, msg->text+mex->inputdatalength, 1);
-	if (size < 0)
-		return size;
-	size += sizeof(*msg) + mex->inputdatalength;
-
-	/* message header, cprbx and f&r */
-	msg->hdr = static_type6_hdrX;
-	msg->hdr.ToCardLen1 = size - sizeof(msg->hdr);
-	msg->hdr.FromCardLen1 = PCIXCC_MAX_ICA_RESPONSE_SIZE - sizeof(msg->hdr);
-
-	msg->cprbx = static_cprbx;
-	msg->cprbx.domain = AP_QID_QUEUE(zdev->ap_dev->qid);
-	msg->cprbx.rpl_msgbl = msg->hdr.FromCardLen1;
-
-	msg->fr = (zdev->user_space_type == ZCRYPT_PCIXCC_MCL2) ?
-		static_pke_fnr_MCL2 : static_pke_fnr;
-
-	msg->cprbx.req_parml = size - sizeof(msg->hdr) - sizeof(msg->cprbx);
-
-	ap_msg->length = size;
-	return 0;
-}
-
-/**
- * Convert a ICACRT message to a type6 CRT message.
- *
- * @zdev: crypto device pointer
- * @ap_msg: pointer to AP message
- * @crt: pointer to user input data
- *
- * Returns 0 on success or -EFAULT.
- */
-static int ICACRT_msg_to_type6CRT_msgX(struct zcrypt_device *zdev,
-				       struct ap_message *ap_msg,
-				       struct ica_rsa_modexpo_crt *crt)
-{
-	static struct type6_hdr static_type6_hdrX = {
-		.type		=  0x06,
-		.offset1	=  0x00000058,
-		.agent_id	= {'C','A',},
-		.function_code	= {'P','D'},
-	};
-	static struct function_and_rules_block static_pkd_fnr = {
-		.function_code	= {'P','D'},
-		.ulen		= 10,
-		.only_rule	= {'Z','E','R','O','-','P','A','D'}
-	};
-
-	static struct function_and_rules_block static_pkd_fnr_MCL2 = {
-		.function_code	= {'P','D'},
-		.ulen		= 10,
-		.only_rule	= {'P','K','C','S','-','1','.','2'}
-	};
-	struct {
-		struct type6_hdr hdr;
-		struct CPRBX cprbx;
-		struct function_and_rules_block fr;
-		unsigned short length;
-		char text[0];
-	} __attribute__((packed)) *msg = ap_msg->message;
-	int size;
-
-	/* VUD.ciphertext */
-	msg->length = crt->inputdatalength + 2;
-	if (copy_from_user(msg->text, crt->inputdata, crt->inputdatalength))
-		return -EFAULT;
-
-	/* Set up key which is located after the variable length text. */
-	size = zcrypt_type6_crt_key(crt, msg->text + crt->inputdatalength, 1);
-	if (size < 0)
-		return size;
-	size += sizeof(*msg) + crt->inputdatalength;	/* total size of msg */
-
-	/* message header, cprbx and f&r */
-	msg->hdr = static_type6_hdrX;
-	msg->hdr.ToCardLen1 = size -  sizeof(msg->hdr);
-	msg->hdr.FromCardLen1 = PCIXCC_MAX_ICA_RESPONSE_SIZE - sizeof(msg->hdr);
-
-	msg->cprbx = static_cprbx;
-	msg->cprbx.domain = AP_QID_QUEUE(zdev->ap_dev->qid);
-	msg->cprbx.req_parml = msg->cprbx.rpl_msgbl =
-		size - sizeof(msg->hdr) - sizeof(msg->cprbx);
-
-	msg->fr = (zdev->user_space_type == ZCRYPT_PCIXCC_MCL2) ?
-		static_pkd_fnr_MCL2 : static_pkd_fnr;
-
-	ap_msg->length = size;
-	return 0;
-}
-
-/**
- * Convert a XCRB message to a type6 CPRB message.
- *
- * @zdev: crypto device pointer
- * @ap_msg: pointer to AP message
- * @xcRB: pointer to user input data
- *
- * Returns 0 on success or -EFAULT, -EINVAL.
- */
-struct type86_fmt2_msg {
-	struct type86_hdr hdr;
-	struct type86_fmt2_ext fmt2;
-} __attribute__((packed));
-
-static int XCRB_msg_to_type6CPRB_msgX(struct zcrypt_device *zdev,
-				       struct ap_message *ap_msg,
-				       struct ica_xcRB *xcRB)
-{
-	static struct type6_hdr static_type6_hdrX = {
-		.type		=  0x06,
-		.offset1	=  0x00000058,
-	};
-	struct {
-		struct type6_hdr hdr;
-		struct CPRBX cprbx;
-	} __attribute__((packed)) *msg = ap_msg->message;
-
-	int rcblen = CEIL4(xcRB->request_control_blk_length);
-	int replylen;
-	char *req_data = ap_msg->message + sizeof(struct type6_hdr) + rcblen;
-	char *function_code;
-
-	/* length checks */
-	ap_msg->length = sizeof(struct type6_hdr) +
-		CEIL4(xcRB->request_control_blk_length) +
-		xcRB->request_data_length;
-	if (ap_msg->length > PCIXCC_MAX_XCRB_MESSAGE_SIZE)
-		return -EINVAL;
-	replylen = sizeof(struct type86_fmt2_msg) +
-		CEIL4(xcRB->reply_control_blk_length) +
-		xcRB->reply_data_length;
-	if (replylen > PCIXCC_MAX_XCRB_MESSAGE_SIZE)
-		return -EINVAL;
-
-	/* prepare type6 header */
-	msg->hdr = static_type6_hdrX;
-	memcpy(msg->hdr.agent_id , &(xcRB->agent_ID), sizeof(xcRB->agent_ID));
-	msg->hdr.ToCardLen1 = xcRB->request_control_blk_length;
-	if (xcRB->request_data_length) {
-		msg->hdr.offset2 = msg->hdr.offset1 + rcblen;
-		msg->hdr.ToCardLen2 = xcRB->request_data_length;
-	}
-	msg->hdr.FromCardLen1 = xcRB->reply_control_blk_length;
-	msg->hdr.FromCardLen2 = xcRB->reply_data_length;
-
-	/* prepare CPRB */
-	if (copy_from_user(&(msg->cprbx), xcRB->request_control_blk_addr,
-		    xcRB->request_control_blk_length))
-		return -EFAULT;
-	if (msg->cprbx.cprb_len + sizeof(msg->hdr.function_code) >
-	    xcRB->request_control_blk_length)
-		return -EINVAL;
-	function_code = ((unsigned char *)&msg->cprbx) + msg->cprbx.cprb_len;
-	memcpy(msg->hdr.function_code, function_code, sizeof(msg->hdr.function_code));
-
-	if (memcmp(function_code, "US", 2) == 0)
-		ap_msg->special = 1;
-	else
-		ap_msg->special = 0;
-
-	/* copy data block */
-	if (xcRB->request_data_length &&
-	    copy_from_user(req_data, xcRB->request_data_address,
-		xcRB->request_data_length))
-		return -EFAULT;
-	return 0;
-}
-
-/**
- * Prepare a type6 CPRB message for random number generation
- *
- * @ap_dev: AP device pointer
- * @ap_msg: pointer to AP message
- */
-static void rng_type6CPRB_msgX(struct ap_device *ap_dev,
-			       struct ap_message *ap_msg,
-			       unsigned random_number_length)
-{
-	struct {
-		struct type6_hdr hdr;
-		struct CPRBX cprbx;
-		char function_code[2];
-		short int rule_length;
-		char rule[8];
-		short int verb_length;
-		short int key_length;
-	} __attribute__((packed)) *msg = ap_msg->message;
-	static struct type6_hdr static_type6_hdrX = {
-		.type		= 0x06,
-		.offset1	= 0x00000058,
-		.agent_id	= {'C', 'A'},
-		.function_code	= {'R', 'L'},
-		.ToCardLen1	= sizeof *msg - sizeof(msg->hdr),
-		.FromCardLen1	= sizeof *msg - sizeof(msg->hdr),
-	};
-	static struct CPRBX local_cprbx = {
-		.cprb_len	= 0x00dc,
-		.cprb_ver_id	= 0x02,
-		.func_id	= {0x54, 0x32},
-		.req_parml	= sizeof *msg - sizeof(msg->hdr) -
-				  sizeof(msg->cprbx),
-		.rpl_msgbl	= sizeof *msg - sizeof(msg->hdr),
-	};
-
-	msg->hdr = static_type6_hdrX;
-	msg->hdr.FromCardLen2 = random_number_length,
-	msg->cprbx = local_cprbx;
-	msg->cprbx.rpl_datal = random_number_length,
-	msg->cprbx.domain = AP_QID_QUEUE(ap_dev->qid);
-	memcpy(msg->function_code, msg->hdr.function_code, 0x02);
-	msg->rule_length = 0x0a;
-	memcpy(msg->rule, "RANDOM  ", 8);
-	msg->verb_length = 0x02;
-	msg->key_length = 0x02;
-	ap_msg->length = sizeof *msg;
-}
-
-/**
- * Copy results from a type 86 ICA reply message back to user space.
- *
- * @zdev: crypto device pointer
- * @reply: reply AP message.
- * @data: pointer to user output data
- * @length: size of user output data
- *
- * Returns 0 on success or -EINVAL, -EFAULT, -EAGAIN in case of an error.
- */
-struct type86x_reply {
-	struct type86_hdr hdr;
-	struct type86_fmt2_ext fmt2;
-	struct CPRBX cprbx;
-	unsigned char pad[4];	/* 4 byte function code/rules block ? */
-	unsigned short length;
-	char text[0];
-} __attribute__((packed));
-
-static int convert_type86_ica(struct zcrypt_device *zdev,
-			  struct ap_message *reply,
-			  char __user *outputdata,
-			  unsigned int outputdatalength)
-{
-	static unsigned char static_pad[] = {
-		0x00,0x02,
-		0x1B,0x7B,0x5D,0xB5,0x75,0x01,0x3D,0xFD,
-		0x8D,0xD1,0xC7,0x03,0x2D,0x09,0x23,0x57,
-		0x89,0x49,0xB9,0x3F,0xBB,0x99,0x41,0x5B,
-		0x75,0x21,0x7B,0x9D,0x3B,0x6B,0x51,0x39,
-		0xBB,0x0D,0x35,0xB9,0x89,0x0F,0x93,0xA5,
-		0x0B,0x47,0xF1,0xD3,0xBB,0xCB,0xF1,0x9D,
-		0x23,0x73,0x71,0xFF,0xF3,0xF5,0x45,0xFB,
-		0x61,0x29,0x23,0xFD,0xF1,0x29,0x3F,0x7F,
-		0x17,0xB7,0x1B,0xA9,0x19,0xBD,0x57,0xA9,
-		0xD7,0x95,0xA3,0xCB,0xED,0x1D,0xDB,0x45,
-		0x7D,0x11,0xD1,0x51,0x1B,0xED,0x71,0xE9,
-		0xB1,0xD1,0xAB,0xAB,0x21,0x2B,0x1B,0x9F,
-		0x3B,0x9F,0xF7,0xF7,0xBD,0x63,0xEB,0xAD,
-		0xDF,0xB3,0x6F,0x5B,0xDB,0x8D,0xA9,0x5D,
-		0xE3,0x7D,0x77,0x49,0x47,0xF5,0xA7,0xFD,
-		0xAB,0x2F,0x27,0x35,0x77,0xD3,0x49,0xC9,
-		0x09,0xEB,0xB1,0xF9,0xBF,0x4B,0xCB,0x2B,
-		0xEB,0xEB,0x05,0xFF,0x7D,0xC7,0x91,0x8B,
-		0x09,0x83,0xB9,0xB9,0x69,0x33,0x39,0x6B,
-		0x79,0x75,0x19,0xBF,0xBB,0x07,0x1D,0xBD,
-		0x29,0xBF,0x39,0x95,0x93,0x1D,0x35,0xC7,
-		0xC9,0x4D,0xE5,0x97,0x0B,0x43,0x9B,0xF1,
-		0x16,0x93,0x03,0x1F,0xA5,0xFB,0xDB,0xF3,
-		0x27,0x4F,0x27,0x61,0x05,0x1F,0xB9,0x23,
-		0x2F,0xC3,0x81,0xA9,0x23,0x71,0x55,0x55,
-		0xEB,0xED,0x41,0xE5,0xF3,0x11,0xF1,0x43,
-		0x69,0x03,0xBD,0x0B,0x37,0x0F,0x51,0x8F,
-		0x0B,0xB5,0x89,0x5B,0x67,0xA9,0xD9,0x4F,
-		0x01,0xF9,0x21,0x77,0x37,0x73,0x79,0xC5,
-		0x7F,0x51,0xC1,0xCF,0x97,0xA1,0x75,0xAD,
-		0x35,0x9D,0xD3,0xD3,0xA7,0x9D,0x5D,0x41,
-		0x6F,0x65,0x1B,0xCF,0xA9,0x87,0x91,0x09
-	};
-	struct type86x_reply *msg = reply->message;
-	unsigned short service_rc, service_rs;
-	unsigned int reply_len, pad_len;
-	char *data;
-
-	service_rc = msg->cprbx.ccp_rtcode;
-	if (unlikely(service_rc != 0)) {
-		service_rs = msg->cprbx.ccp_rscode;
-		if (service_rc == 8 && service_rs == 66)
-			return -EINVAL;
-		if (service_rc == 8 && service_rs == 65)
-			return -EINVAL;
-		if (service_rc == 8 && service_rs == 770)
-			return -EINVAL;
-		if (service_rc == 8 && service_rs == 783) {
-			zdev->min_mod_size = PCIXCC_MIN_MOD_SIZE_OLD;
-			return -EAGAIN;
-		}
-		if (service_rc == 12 && service_rs == 769)
-			return -EINVAL;
-		if (service_rc == 8 && service_rs == 72)
-			return -EINVAL;
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-	data = msg->text;
-	reply_len = msg->length - 2;
-	if (reply_len > outputdatalength)
-		return -EINVAL;
-	/*
-	 * For all encipher requests, the length of the ciphertext (reply_len)
-	 * will always equal the modulus length. For MEX decipher requests
-	 * the output needs to get padded. Minimum pad size is 10.
-	 *
-	 * Currently, the cases where padding will be added is for:
-	 * - PCIXCC_MCL2 using a CRT form token (since PKD didn't support
-	 *   ZERO-PAD and CRT is only supported for PKD requests)
-	 * - PCICC, always
-	 */
-	pad_len = outputdatalength - reply_len;
-	if (pad_len > 0) {
-		if (pad_len < 10)
-			return -EINVAL;
-		/* 'restore' padding left in the PCICC/PCIXCC card. */
-		if (copy_to_user(outputdata, static_pad, pad_len - 1))
-			return -EFAULT;
-		if (put_user(0, outputdata + pad_len - 1))
-			return -EFAULT;
-	}
-	/* Copy the crypto response to user space. */
-	if (copy_to_user(outputdata + pad_len, data, reply_len))
-		return -EFAULT;
-	return 0;
-}
-
-/**
- * Copy results from a type 86 XCRB reply message back to user space.
- *
- * @zdev: crypto device pointer
- * @reply: reply AP message.
- * @xcRB: pointer to XCRB
- *
- * Returns 0 on success or -EINVAL, -EFAULT, -EAGAIN in case of an error.
- */
-static int convert_type86_xcrb(struct zcrypt_device *zdev,
-			       struct ap_message *reply,
-			       struct ica_xcRB *xcRB)
-{
-	struct type86_fmt2_msg *msg = reply->message;
-	char *data = reply->message;
-
-	/* Copy CPRB to user */
-	if (copy_to_user(xcRB->reply_control_blk_addr,
-		data + msg->fmt2.offset1, msg->fmt2.count1))
-		return -EFAULT;
-	xcRB->reply_control_blk_length = msg->fmt2.count1;
-
-	/* Copy data buffer to user */
-	if (msg->fmt2.count2)
-		if (copy_to_user(xcRB->reply_data_addr,
-			data + msg->fmt2.offset2, msg->fmt2.count2))
-			return -EFAULT;
-	xcRB->reply_data_length = msg->fmt2.count2;
-	return 0;
-}
-
-static int convert_type86_rng(struct zcrypt_device *zdev,
-			  struct ap_message *reply,
-			  char *buffer)
-{
-	struct {
-		struct type86_hdr hdr;
-		struct type86_fmt2_ext fmt2;
-		struct CPRBX cprbx;
-	} __attribute__((packed)) *msg = reply->message;
-	char *data = reply->message;
-
-	if (msg->cprbx.ccp_rtcode != 0 || msg->cprbx.ccp_rscode != 0)
-		return -EINVAL;
-	memcpy(buffer, data + msg->fmt2.offset2, msg->fmt2.count2);
-	return msg->fmt2.count2;
-}
-
-static int convert_response_ica(struct zcrypt_device *zdev,
-			    struct ap_message *reply,
-			    char __user *outputdata,
-			    unsigned int outputdatalength)
-{
-	struct type86x_reply *msg = reply->message;
-
-	/* Response type byte is the second byte in the response. */
-	switch (((unsigned char *) reply->message)[1]) {
-	case TYPE82_RSP_CODE:
-	case TYPE88_RSP_CODE:
-		return convert_error(zdev, reply);
-	case TYPE86_RSP_CODE:
-		if (msg->cprbx.ccp_rtcode &&
-		   (msg->cprbx.ccp_rscode == 0x14f) &&
-		   (outputdatalength > 256)) {
-			if (zdev->max_exp_bit_length <= 17) {
-				zdev->max_exp_bit_length = 17;
-				return -EAGAIN;
-			} else
-				return -EINVAL;
-		}
-		if (msg->hdr.reply_code)
-			return convert_error(zdev, reply);
-		if (msg->cprbx.cprb_ver_id == 0x02)
-			return convert_type86_ica(zdev, reply,
-						  outputdata, outputdatalength);
-		/* Fall through, no break, incorrect cprb version is an unknown
-		 * response */
-	default: /* Unknown response type, this should NEVER EVER happen */
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-}
-
-static int convert_response_xcrb(struct zcrypt_device *zdev,
-			    struct ap_message *reply,
-			    struct ica_xcRB *xcRB)
-{
-	struct type86x_reply *msg = reply->message;
-
-	/* Response type byte is the second byte in the response. */
-	switch (((unsigned char *) reply->message)[1]) {
-	case TYPE82_RSP_CODE:
-	case TYPE88_RSP_CODE:
-		xcRB->status = 0x0008044DL; /* HDD_InvalidParm */
-		return convert_error(zdev, reply);
-	case TYPE86_RSP_CODE:
-		if (msg->hdr.reply_code) {
-			memcpy(&(xcRB->status), msg->fmt2.apfs, sizeof(u32));
-			return convert_error(zdev, reply);
-		}
-		if (msg->cprbx.cprb_ver_id == 0x02)
-			return convert_type86_xcrb(zdev, reply, xcRB);
-		/* Fall through, no break, incorrect cprb version is an unknown
-		 * response */
-	default: /* Unknown response type, this should NEVER EVER happen */
-		xcRB->status = 0x0008044DL; /* HDD_InvalidParm */
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-}
-
-static int convert_response_rng(struct zcrypt_device *zdev,
-				 struct ap_message *reply,
-				 char *data)
-{
-	struct type86x_reply *msg = reply->message;
-
-	switch (msg->hdr.type) {
-	case TYPE82_RSP_CODE:
-	case TYPE88_RSP_CODE:
-		return -EINVAL;
-	case TYPE86_RSP_CODE:
-		if (msg->hdr.reply_code)
-			return -EINVAL;
-		if (msg->cprbx.cprb_ver_id == 0x02)
-			return convert_type86_rng(zdev, reply, data);
-		/* Fall through, no break, incorrect cprb version is an unknown
-		 * response */
-	default: /* Unknown response type, this should NEVER EVER happen */
-		zdev->online = 0;
-		return -EAGAIN;	/* repeat the request on a different device. */
-	}
-}
-
-/**
- * This function is called from the AP bus code after a crypto request
- * "msg" has finished with the reply message "reply".
- * It is called from tasklet context.
- * @ap_dev: pointer to the AP device
- * @msg: pointer to the AP message
- * @reply: pointer to the AP reply message
- */
-static void zcrypt_pcixcc_receive(struct ap_device *ap_dev,
-				  struct ap_message *msg,
-				  struct ap_message *reply)
-{
-	static struct error_hdr error_reply = {
-		.type = TYPE82_RSP_CODE,
-		.reply_code = REP82_ERROR_MACHINE_FAILURE,
-	};
-	struct response_type *resp_type =
-		(struct response_type *) msg->private;
-	struct type86x_reply *t86r;
-	int length;
-
-	/* Copy the reply message to the request message buffer. */
-	if (IS_ERR(reply)) {
-		memcpy(msg->message, &error_reply, sizeof(error_reply));
-		goto out;
-	}
-	t86r = reply->message;
-	if (t86r->hdr.type == TYPE86_RSP_CODE &&
-		 t86r->cprbx.cprb_ver_id == 0x02) {
-		switch (resp_type->type) {
-		case PCIXCC_RESPONSE_TYPE_ICA:
-			length = sizeof(struct type86x_reply)
-				+ t86r->length - 2;
-			length = min(PCIXCC_MAX_ICA_RESPONSE_SIZE, length);
-			memcpy(msg->message, reply->message, length);
-			break;
-		case PCIXCC_RESPONSE_TYPE_XCRB:
-			length = t86r->fmt2.offset2 + t86r->fmt2.count2;
-			length = min(PCIXCC_MAX_XCRB_MESSAGE_SIZE, length);
-			memcpy(msg->message, reply->message, length);
-			break;
-		default:
-			memcpy(msg->message, &error_reply, sizeof error_reply);
-		}
-	} else
-		memcpy(msg->message, reply->message, sizeof error_reply);
-out:
-	complete(&(resp_type->work));
-}
-
-static atomic_t zcrypt_step = ATOMIC_INIT(0);
-
-/**
- * The request distributor calls this function if it picked the PCIXCC/CEX2C
- * device to handle a modexpo request.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  PCIXCC/CEX2C device to the request distributor
- * @mex: pointer to the modexpo request buffer
- */
-static long zcrypt_pcixcc_modexpo(struct zcrypt_device *zdev,
-				  struct ica_rsa_modexpo *mex)
-{
-	struct ap_message ap_msg;
-	struct response_type resp_type = {
-		.type = PCIXCC_RESPONSE_TYPE_ICA,
-	};
-	int rc;
-
-	ap_init_message(&ap_msg);
-	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_pcixcc_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &resp_type;
-	rc = ICAMEX_msg_to_type6MEX_msgX(zdev, &ap_msg, mex);
-	if (rc)
-		goto out_free;
-	init_completion(&resp_type.work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&resp_type.work);
-	if (rc == 0)
-		rc = convert_response_ica(zdev, &ap_msg, mex->outputdata,
-					  mex->outputdatalength);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-out_free:
-	free_page((unsigned long) ap_msg.message);
-	return rc;
-}
-
-/**
- * The request distributor calls this function if it picked the PCIXCC/CEX2C
- * device to handle a modexpo_crt request.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  PCIXCC/CEX2C device to the request distributor
- * @crt: pointer to the modexpoc_crt request buffer
- */
-static long zcrypt_pcixcc_modexpo_crt(struct zcrypt_device *zdev,
-				      struct ica_rsa_modexpo_crt *crt)
-{
-	struct ap_message ap_msg;
-	struct response_type resp_type = {
-		.type = PCIXCC_RESPONSE_TYPE_ICA,
-	};
-	int rc;
-
-	ap_init_message(&ap_msg);
-	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_pcixcc_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &resp_type;
-	rc = ICACRT_msg_to_type6CRT_msgX(zdev, &ap_msg, crt);
-	if (rc)
-		goto out_free;
-	init_completion(&resp_type.work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&resp_type.work);
-	if (rc == 0)
-		rc = convert_response_ica(zdev, &ap_msg, crt->outputdata,
-					  crt->outputdatalength);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-out_free:
-	free_page((unsigned long) ap_msg.message);
-	return rc;
-}
-
-/**
- * The request distributor calls this function if it picked the PCIXCC/CEX2C
- * device to handle a send_cprb request.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  PCIXCC/CEX2C device to the request distributor
- * @xcRB: pointer to the send_cprb request buffer
- */
-static long zcrypt_pcixcc_send_cprb(struct zcrypt_device *zdev,
-				    struct ica_xcRB *xcRB)
-{
-	struct ap_message ap_msg;
-	struct response_type resp_type = {
-		.type = PCIXCC_RESPONSE_TYPE_XCRB,
-	};
-	int rc;
-
-	ap_init_message(&ap_msg);
-	ap_msg.message = kmalloc(PCIXCC_MAX_XCRB_MESSAGE_SIZE, GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_pcixcc_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &resp_type;
-	rc = XCRB_msg_to_type6CPRB_msgX(zdev, &ap_msg, xcRB);
-	if (rc)
-		goto out_free;
-	init_completion(&resp_type.work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&resp_type.work);
-	if (rc == 0)
-		rc = convert_response_xcrb(zdev, &ap_msg, xcRB);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-out_free:
-	kzfree(ap_msg.message);
-	return rc;
-}
-
-/**
- * The request distributor calls this function if it picked the PCIXCC/CEX2C
- * device to generate random data.
- * @zdev: pointer to zcrypt_device structure that identifies the
- *	  PCIXCC/CEX2C device to the request distributor
- * @buffer: pointer to a memory page to return random data
- */
-
-static long zcrypt_pcixcc_rng(struct zcrypt_device *zdev,
-				    char *buffer)
-{
-	struct ap_message ap_msg;
-	struct response_type resp_type = {
-		.type = PCIXCC_RESPONSE_TYPE_XCRB,
-	};
-	int rc;
-
-	ap_init_message(&ap_msg);
-	ap_msg.message = kmalloc(PCIXCC_MAX_XCRB_MESSAGE_SIZE, GFP_KERNEL);
-	if (!ap_msg.message)
-		return -ENOMEM;
-	ap_msg.receive = zcrypt_pcixcc_receive;
-	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
-				atomic_inc_return(&zcrypt_step);
-	ap_msg.private = &resp_type;
-	rng_type6CPRB_msgX(zdev->ap_dev, &ap_msg, ZCRYPT_RNG_BUFFER_SIZE);
-	init_completion(&resp_type.work);
-	ap_queue_message(zdev->ap_dev, &ap_msg);
-	rc = wait_for_completion_interruptible(&resp_type.work);
-	if (rc == 0)
-		rc = convert_response_rng(zdev, &ap_msg, buffer);
-	else
-		/* Signal pending. */
-		ap_cancel_message(zdev->ap_dev, &ap_msg);
-	kfree(ap_msg.message);
-	return rc;
-}
-
-/**
- * The crypto operations for a PCIXCC/CEX2C card.
- */
-static struct zcrypt_ops zcrypt_pcixcc_ops = {
-	.rsa_modexpo = zcrypt_pcixcc_modexpo,
-	.rsa_modexpo_crt = zcrypt_pcixcc_modexpo_crt,
-	.send_cprb = zcrypt_pcixcc_send_cprb,
-};
-
-static struct zcrypt_ops zcrypt_pcixcc_with_rng_ops = {
-	.rsa_modexpo = zcrypt_pcixcc_modexpo,
-	.rsa_modexpo_crt = zcrypt_pcixcc_modexpo_crt,
-	.send_cprb = zcrypt_pcixcc_send_cprb,
-	.rng = zcrypt_pcixcc_rng,
-};
-
-/**
  * Micro-code detection function. Its sends a message to a pcixcc card
  * to find out the microcode level.
  * @ap_dev: pointer to the AP device.
@@ -1083,9 +323,11 @@ static int zcrypt_pcixcc_probe(struct ap_device *ap_dev)
 		return rc;
 	}
 	if (rc)
-		zdev->ops = &zcrypt_pcixcc_with_rng_ops;
+		zdev->ops = zcrypt_msgtype_request(MSGTYPE06_NAME,
+						   MSGTYPE06_VARIANT_DEFAULT);
 	else
-		zdev->ops = &zcrypt_pcixcc_ops;
+		zdev->ops = zcrypt_msgtype_request(MSGTYPE06_NAME,
+						   MSGTYPE06_VARIANT_NORNG);
 	ap_dev->reply = &zdev->reply;
 	ap_dev->private = zdev;
 	rc = zcrypt_device_register(zdev);
@@ -1095,6 +337,7 @@ static int zcrypt_pcixcc_probe(struct ap_device *ap_dev)
 
  out_free:
 	ap_dev->private = NULL;
+	zcrypt_msgtype_release(zdev->ops);
 	zcrypt_device_free(zdev);
 	return rc;
 }
@@ -1106,8 +349,10 @@ static int zcrypt_pcixcc_probe(struct ap_device *ap_dev)
 static void zcrypt_pcixcc_remove(struct ap_device *ap_dev)
 {
 	struct zcrypt_device *zdev = ap_dev->private;
+	struct zcrypt_ops *zops = zdev->ops;
 
 	zcrypt_device_unregister(zdev);
+	zcrypt_msgtype_release(zops);
 }
 
 int __init zcrypt_pcixcc_init(void)
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.h b/drivers/s390/crypto/zcrypt_pcixcc.h
index c7cdf599e46b..eacafc8962f2 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.h
+++ b/drivers/s390/crypto/zcrypt_pcixcc.h
@@ -1,12 +1,13 @@
 /*
  *  zcrypt 2.1.0
  *
- *  Copyright IBM Corp. 2001, 2006
+ *  Copyright IBM Corp. 2001, 2012
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
  *  Hotplug & misc device support: Jochen Roehrig (roehrig@de.ibm.com)
  *  Major cleanup & driver split: Martin Schwidefsky <schwidefsky@de.ibm.com>
+ *  MSGTYPE restruct:		  Holger Dengler <hd@linux.vnet.ibm.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
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 7a8b09612c41..cf6da7fafe54 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -2993,7 +2993,7 @@ static void qeth_get_trap_id(struct qeth_card *card, struct qeth_trap_id *tid)
 	struct sysinfo_2_2_2 *info222 = (struct sysinfo_2_2_2 *)info;
 	struct sysinfo_3_2_2 *info322 = (struct sysinfo_3_2_2 *)info;
 	struct ccw_dev_id ccwid;
-	int level, rc;
+	int level;
 
 	tid->chpid = card->info.chpid;
 	ccw_device_get_id(CARD_RDEV(card), &ccwid);
@@ -3001,17 +3001,10 @@ static void qeth_get_trap_id(struct qeth_card *card, struct qeth_trap_id *tid)
 	tid->devno = ccwid.devno;
 	if (!info)
 		return;
-
-	rc = stsi(NULL, 0, 0, 0);
-	if (rc == -ENOSYS)
-		level = rc;
-	else
-		level = (((unsigned int) rc) >> 28);
-
-	if ((level >= 2) && (stsi(info222, 2, 2, 2) != -ENOSYS))
+	level = stsi(NULL, 0, 0, 0);
+	if ((level >= 2) && (stsi(info222, 2, 2, 2) == 0))
 		tid->lparnr = info222->lpar_number;
-
-	if ((level >= 3) && (stsi(info322, 3, 2, 2) != -ENOSYS)) {
+	if ((level >= 3) && (stsi(info322, 3, 2, 2) == 0)) {
 		EBCASC(info322->vm[0].name, sizeof(info322->vm[0].name));
 		memcpy(tid->vmname, info322->vm[0].name, sizeof(tid->vmname));
 	}
diff --git a/drivers/scsi/aic7xxx/aic79xx_core.c b/drivers/scsi/aic7xxx/aic79xx_core.c
index 25417d0e7acb..0bcacf71aef8 100644
--- a/drivers/scsi/aic7xxx/aic79xx_core.c
+++ b/drivers/scsi/aic7xxx/aic79xx_core.c
@@ -2888,7 +2888,7 @@ ahd_handle_lqiphase_error(struct ahd_softc *ahd, u_int lqistat1)
 		ahd_outb(ahd, CLRINT, CLRSCSIINT);
 		ahd_unpause(ahd);
 	} else {
-		printk("Reseting Channel for LQI Phase error\n");
+		printk("Resetting Channel for LQI Phase error\n");
 		ahd_dump_card_state(ahd);
 		ahd_reset_channel(ahd, 'A', /*Initiate Reset*/TRUE);
 	}
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 8cdb79c2fcdf..21ad2902e5ce 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -5587,7 +5587,7 @@ static bfa_status_t bfa_dconf_flash_write(struct bfa_dconf_mod_s *dconf);
 static void bfa_dconf_init_cb(void *arg, bfa_status_t status);
 
 /*
- * Begining state of dconf module. Waiting for an event to start.
+ * Beginning state of dconf module. Waiting for an event to start.
  */
 static void
 bfa_dconf_sm_uninit(struct bfa_dconf_mod_s *dconf, enum bfa_dconf_event event)
diff --git a/drivers/scsi/bfa/bfa_ioc.h b/drivers/scsi/bfa/bfa_ioc.h
index 1a99d4b5b50f..7b916e04ca56 100644
--- a/drivers/scsi/bfa/bfa_ioc.h
+++ b/drivers/scsi/bfa/bfa_ioc.h
@@ -530,7 +530,7 @@ struct bfa_diag_results_fwping {
 
 struct bfa_diag_qtest_result_s {
 	u32	status;
-	u16	count;	/* sucessful queue test count */
+	u16	count;	/* successful queue test count */
 	u8	queue;
 	u8	rsvd;	/* 64-bit align */
 };
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index ae1cb7639d99..e0558656c646 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -908,7 +908,7 @@ static void bnx2fc_indicate_netevent(void *context, unsigned long event,
 		return;
 
 	default:
-		printk(KERN_ERR PFX "Unkonwn netevent %ld", event);
+		printk(KERN_ERR PFX "Unknown netevent %ld", event);
 		return;
 	}
 
@@ -1738,7 +1738,7 @@ static int bnx2fc_ulp_get_stats(void *handle)
 /**
  * bnx2fc_ulp_start - cnic callback to initialize & start adapter instance
  *
- * @handle:	transport handle pointing to adapter struture
+ * @handle:	transport handle pointing to adapter structure
  *
  * This function maps adapter structure to pcidev structure and initiates
  *	firmware handshake to enable/initialize on-chip FCoE components.
diff --git a/drivers/scsi/bnx2i/bnx2i_hwi.c b/drivers/scsi/bnx2i/bnx2i_hwi.c
index 33d6630529de..91eec60252ee 100644
--- a/drivers/scsi/bnx2i/bnx2i_hwi.c
+++ b/drivers/scsi/bnx2i/bnx2i_hwi.c
@@ -1264,6 +1264,9 @@ int bnx2i_send_fw_iscsi_init_msg(struct bnx2i_hba *hba)
 	int rc = 0;
 	u64 mask64;
 
+	memset(&iscsi_init, 0x00, sizeof(struct iscsi_kwqe_init1));
+	memset(&iscsi_init2, 0x00, sizeof(struct iscsi_kwqe_init2));
+
 	bnx2i_adjust_qp_size(hba);
 
 	iscsi_init.flags =
diff --git a/drivers/scsi/gdth.h b/drivers/scsi/gdth.h
index d3e4d7c6f577..fbf6f0f4b0dd 100644
--- a/drivers/scsi/gdth.h
+++ b/drivers/scsi/gdth.h
@@ -49,15 +49,6 @@
 /* GDT_ISA */
 #define GDT2_ID         0x0120941c              /* GDT2000/2020 */
 
-/* vendor ID, device IDs (PCI) */
-/* these defines should already exist in <linux/pci.h> */
-#ifndef PCI_VENDOR_ID_VORTEX
-#define PCI_VENDOR_ID_VORTEX            0x1119  /* PCI controller vendor ID */
-#endif
-#ifndef PCI_VENDOR_ID_INTEL
-#define PCI_VENDOR_ID_INTEL             0x8086  
-#endif
-
 #ifndef PCI_DEVICE_ID_VORTEX_GDT60x0
 /* GDT_PCI */
 #define PCI_DEVICE_ID_VORTEX_GDT60x0    0       /* GDT6000/6020/6050 */
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 796482badf13..2b4261cb7742 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -1315,8 +1315,9 @@ static void complete_scsi_command(struct CommandList *cp)
 	}
 		break;
 	case CMD_PROTOCOL_ERR:
+		cmd->result = DID_ERROR << 16;
 		dev_warn(&h->pdev->dev, "cp %p has "
-			"protocol error \n", cp);
+			"protocol error\n", cp);
 		break;
 	case CMD_HARDWARE_ERR:
 		cmd->result = DID_ERROR << 16;
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 467dc38246f9..0a2c5a8ebb82 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -192,7 +192,7 @@ static const struct ipr_chip_t ipr_chip[] = {
 	{ PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_IBM_CROCODILE, IPR_USE_MSI, IPR_SIS64, IPR_MMIO, &ipr_chip_cfg[2] }
 };
 
-static int ipr_max_bus_speeds [] = {
+static int ipr_max_bus_speeds[] = {
 	IPR_80MBs_SCSI_RATE, IPR_U160_SCSI_RATE, IPR_U320_SCSI_RATE
 };
 
@@ -562,7 +562,7 @@ static void ipr_trc_hook(struct ipr_cmnd *ipr_cmd,
 	trace_entry->u.add_data = add_data;
 }
 #else
-#define ipr_trc_hook(ipr_cmd, type, add_data) do { } while(0)
+#define ipr_trc_hook(ipr_cmd, type, add_data) do { } while (0)
 #endif
 
 /**
@@ -1002,7 +1002,7 @@ static void ipr_send_hcam(struct ipr_ioa_cfg *ioa_cfg, u8 type,
  **/
 static void ipr_update_ata_class(struct ipr_resource_entry *res, unsigned int proto)
 {
-	switch(proto) {
+	switch (proto) {
 	case IPR_PROTO_SATA:
 	case IPR_PROTO_SAS_STP:
 		res->ata_class = ATA_DEV_ATA;
@@ -3043,7 +3043,7 @@ static void ipr_get_ioa_dump(struct ipr_ioa_cfg *ioa_cfg, struct ipr_dump *dump)
 }
 
 #else
-#define ipr_get_ioa_dump(ioa_cfg, dump) do { } while(0)
+#define ipr_get_ioa_dump(ioa_cfg, dump) do { } while (0)
 #endif
 
 /**
@@ -3055,7 +3055,7 @@ static void ipr_get_ioa_dump(struct ipr_ioa_cfg *ioa_cfg, struct ipr_dump *dump)
  **/
 static void ipr_release_dump(struct kref *kref)
 {
-	struct ipr_dump *dump = container_of(kref,struct ipr_dump,kref);
+	struct ipr_dump *dump = container_of(kref, struct ipr_dump, kref);
 	struct ipr_ioa_cfg *ioa_cfg = dump->ioa_cfg;
 	unsigned long lock_flags = 0;
 	int i;
@@ -3142,7 +3142,7 @@ restart:
 				break;
 			}
 		}
-	} while(did_work);
+	} while (did_work);
 
 	list_for_each_entry(res, &ioa_cfg->used_res_q, queue) {
 		if (res->add_to_ml) {
@@ -3268,7 +3268,7 @@ static ssize_t ipr_show_log_level(struct device *dev,
  * 	number of bytes printed to buffer
  **/
 static ssize_t ipr_store_log_level(struct device *dev,
-			           struct device_attribute *attr,
+				   struct device_attribute *attr,
 				   const char *buf, size_t count)
 {
 	struct Scsi_Host *shost = class_to_shost(dev);
@@ -3315,7 +3315,7 @@ static ssize_t ipr_store_diagnostics(struct device *dev,
 		return -EACCES;
 
 	spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
@@ -3682,7 +3682,7 @@ static int ipr_update_ioa_ucode(struct ipr_ioa_cfg *ioa_cfg,
 	unsigned long lock_flags;
 
 	spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
@@ -3746,7 +3746,7 @@ static ssize_t ipr_store_update_fw(struct device *dev,
 	len = snprintf(fname, 99, "%s", buf);
 	fname[len-1] = '\0';
 
-	if(request_firmware(&fw_entry, fname, &ioa_cfg->pdev->dev)) {
+	if (request_firmware(&fw_entry, fname, &ioa_cfg->pdev->dev)) {
 		dev_err(&ioa_cfg->pdev->dev, "Firmware file %s not found\n", fname);
 		return -EIO;
 	}
@@ -4612,7 +4612,7 @@ static int ipr_slave_alloc(struct scsi_device *sdev)
  * Return value:
  * 	SUCCESS / FAILED
  **/
-static int __ipr_eh_host_reset(struct scsi_cmnd * scsi_cmd)
+static int __ipr_eh_host_reset(struct scsi_cmnd *scsi_cmd)
 {
 	struct ipr_ioa_cfg *ioa_cfg;
 	int rc;
@@ -4634,7 +4634,7 @@ static int __ipr_eh_host_reset(struct scsi_cmnd * scsi_cmd)
 	return rc;
 }
 
-static int ipr_eh_host_reset(struct scsi_cmnd * cmd)
+static int ipr_eh_host_reset(struct scsi_cmnd *cmd)
 {
 	int rc;
 
@@ -4701,7 +4701,7 @@ static int ipr_device_reset(struct ipr_ioa_cfg *ioa_cfg,
 	}
 
 	LEAVE;
-	return (IPR_IOASC_SENSE_KEY(ioasc) ? -EIO : 0);
+	return IPR_IOASC_SENSE_KEY(ioasc) ? -EIO : 0;
 }
 
 /**
@@ -4725,7 +4725,7 @@ static int ipr_sata_reset(struct ata_link *link, unsigned int *classes,
 
 	ENTER;
 	spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
@@ -4753,7 +4753,7 @@ static int ipr_sata_reset(struct ata_link *link, unsigned int *classes,
  * Return value:
  *	SUCCESS / FAILED
  **/
-static int __ipr_eh_dev_reset(struct scsi_cmnd * scsi_cmd)
+static int __ipr_eh_dev_reset(struct scsi_cmnd *scsi_cmd)
 {
 	struct ipr_cmnd *ipr_cmd;
 	struct ipr_ioa_cfg *ioa_cfg;
@@ -4811,10 +4811,10 @@ static int __ipr_eh_dev_reset(struct scsi_cmnd * scsi_cmd)
 	res->resetting_device = 0;
 
 	LEAVE;
-	return (rc ? FAILED : SUCCESS);
+	return rc ? FAILED : SUCCESS;
 }
 
-static int ipr_eh_dev_reset(struct scsi_cmnd * cmd)
+static int ipr_eh_dev_reset(struct scsi_cmnd *cmd)
 {
 	int rc;
 
@@ -4910,7 +4910,7 @@ static void ipr_abort_timeout(struct ipr_cmnd *ipr_cmd)
  * Return value:
  *	SUCCESS / FAILED
  **/
-static int ipr_cancel_op(struct scsi_cmnd * scsi_cmd)
+static int ipr_cancel_op(struct scsi_cmnd *scsi_cmd)
 {
 	struct ipr_cmnd *ipr_cmd;
 	struct ipr_ioa_cfg *ioa_cfg;
@@ -4979,7 +4979,7 @@ static int ipr_cancel_op(struct scsi_cmnd * scsi_cmd)
 		res->needs_sync_complete = 1;
 
 	LEAVE;
-	return (IPR_IOASC_SENSE_KEY(ioasc) ? FAILED : SUCCESS);
+	return IPR_IOASC_SENSE_KEY(ioasc) ? FAILED : SUCCESS;
 }
 
 /**
@@ -4989,7 +4989,7 @@ static int ipr_cancel_op(struct scsi_cmnd * scsi_cmd)
  * Return value:
  * 	SUCCESS / FAILED
  **/
-static int ipr_eh_abort(struct scsi_cmnd * scsi_cmd)
+static int ipr_eh_abort(struct scsi_cmnd *scsi_cmd)
 {
 	unsigned long flags;
 	int rc;
@@ -5907,7 +5907,7 @@ static int ipr_ioctl(struct scsi_device *sdev, int cmd, void __user *arg)
  * Return value:
  * 	pointer to buffer with description string
  **/
-static const char * ipr_ioa_info(struct Scsi_Host *host)
+static const char *ipr_ioa_info(struct Scsi_Host *host)
 {
 	static char buffer[512];
 	struct ipr_ioa_cfg *ioa_cfg;
@@ -5965,7 +5965,7 @@ static void ipr_ata_phy_reset(struct ata_port *ap)
 
 	ENTER;
 	spin_lock_irqsave(ioa_cfg->host->host_lock, flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, flags);
@@ -6005,7 +6005,7 @@ static void ipr_ata_post_internal(struct ata_queued_cmd *qc)
 	unsigned long flags;
 
 	spin_lock_irqsave(ioa_cfg->host->host_lock, flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, flags);
@@ -6330,7 +6330,7 @@ static int ipr_invalid_adapter(struct ipr_ioa_cfg *ioa_cfg)
 	int i;
 
 	if ((ioa_cfg->type == 0x5702) && (ioa_cfg->pdev->revision < 4)) {
-		for (i = 0; i < ARRAY_SIZE(ipr_blocked_processors); i++){
+		for (i = 0; i < ARRAY_SIZE(ipr_blocked_processors); i++) {
 			if (__is_processor(ipr_blocked_processors[i]))
 				return 1;
 		}
@@ -6608,7 +6608,7 @@ static void ipr_scsi_bus_speed_limit(struct ipr_ioa_cfg *ioa_cfg)
  * 	none
  **/
 static void ipr_modify_ioafp_mode_page_28(struct ipr_ioa_cfg *ioa_cfg,
-					  	struct ipr_mode_pages *mode_pages)
+					  struct ipr_mode_pages *mode_pages)
 {
 	int i, entry_length;
 	struct ipr_dev_bus_entry *bus;
@@ -8022,7 +8022,7 @@ static void ipr_reset_ioa_job(struct ipr_cmnd *ipr_cmd)
 		ipr_reinit_ipr_cmnd(ipr_cmd);
 		ipr_cmd->job_step_failed = ipr_reset_cmd_failed;
 		rc = ipr_cmd->job_step(ipr_cmd);
-	} while(rc == IPR_RC_JOB_CONTINUE);
+	} while (rc == IPR_RC_JOB_CONTINUE);
 }
 
 /**
@@ -8283,7 +8283,7 @@ static void ipr_free_cmd_blks(struct ipr_ioa_cfg *ioa_cfg)
 	}
 
 	if (ioa_cfg->ipr_cmd_pool)
-		pci_pool_destroy (ioa_cfg->ipr_cmd_pool);
+		pci_pool_destroy(ioa_cfg->ipr_cmd_pool);
 
 	kfree(ioa_cfg->ipr_cmnd_list);
 	kfree(ioa_cfg->ipr_cmnd_list_dma);
@@ -8363,8 +8363,8 @@ static int __devinit ipr_alloc_cmd_blks(struct ipr_ioa_cfg *ioa_cfg)
 	dma_addr_t dma_addr;
 	int i;
 
-	ioa_cfg->ipr_cmd_pool = pci_pool_create (IPR_NAME, ioa_cfg->pdev,
-						 sizeof(struct ipr_cmnd), 512, 0);
+	ioa_cfg->ipr_cmd_pool = pci_pool_create(IPR_NAME, ioa_cfg->pdev,
+						sizeof(struct ipr_cmnd), 512, 0);
 
 	if (!ioa_cfg->ipr_cmd_pool)
 		return -ENOMEM;
@@ -8378,7 +8378,7 @@ static int __devinit ipr_alloc_cmd_blks(struct ipr_ioa_cfg *ioa_cfg)
 	}
 
 	for (i = 0; i < IPR_NUM_CMD_BLKS; i++) {
-		ipr_cmd = pci_pool_alloc (ioa_cfg->ipr_cmd_pool, GFP_KERNEL, &dma_addr);
+		ipr_cmd = pci_pool_alloc(ioa_cfg->ipr_cmd_pool, GFP_KERNEL, &dma_addr);
 
 		if (!ipr_cmd) {
 			ipr_free_cmd_blks(ioa_cfg);
@@ -8964,7 +8964,7 @@ static void ipr_scan_vsets(struct ipr_ioa_cfg *ioa_cfg)
 	int target, lun;
 
 	for (target = 0; target < IPR_MAX_NUM_TARGETS_PER_BUS; target++)
-		for (lun = 0; lun < IPR_MAX_NUM_VSET_LUNS_PER_TARGET; lun++ )
+		for (lun = 0; lun < IPR_MAX_NUM_VSET_LUNS_PER_TARGET; lun++)
 			scsi_add_device(ioa_cfg->host, IPR_VSET_BUS, target, lun);
 }
 
@@ -9010,7 +9010,7 @@ static void __ipr_remove(struct pci_dev *pdev)
 	ENTER;
 
 	spin_lock_irqsave(ioa_cfg->host->host_lock, host_lock_flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, host_lock_flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, host_lock_flags);
@@ -9139,7 +9139,7 @@ static void ipr_shutdown(struct pci_dev *pdev)
 	unsigned long lock_flags = 0;
 
 	spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
-	while(ioa_cfg->in_reset_reload) {
+	while (ioa_cfg->in_reset_reload) {
 		spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 		wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
 		spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
@@ -9228,7 +9228,7 @@ static struct pci_device_id ipr_pci_table[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(pci, ipr_pci_table);
 
-static struct pci_error_handlers ipr_err_handler = {
+static const struct pci_error_handlers ipr_err_handler = {
 	.error_detected = ipr_pci_error_detected,
 	.slot_reset = ipr_pci_slot_reset,
 };
diff --git a/drivers/scsi/isci/host.c b/drivers/scsi/isci/host.c
index 45385f531649..b334fdc1726a 100644
--- a/drivers/scsi/isci/host.c
+++ b/drivers/scsi/isci/host.c
@@ -492,7 +492,7 @@ static void sci_controller_process_completions(struct isci_host *ihost)
 	u32 event_cycle;
 
 	dev_dbg(&ihost->pdev->dev,
-		"%s: completion queue begining get:0x%08x\n",
+		"%s: completion queue beginning get:0x%08x\n",
 		__func__,
 		ihost->completion_queue_get);
 
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 92c1d86d1fc6..9be45a2b2232 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -222,7 +222,7 @@ static struct sas_domain_function_template isci_transport_ops  = {
  * @isci_host: This parameter specifies the lldd specific wrapper for the
  *    libsas sas_ha struct.
  *
- * This method returns an error code indicating sucess or failure. The user
+ * This method returns an error code indicating success or failure. The user
  * should check for possible memory allocation error return otherwise, a zero
  * indicates success.
  */
diff --git a/drivers/scsi/isci/port.c b/drivers/scsi/isci/port.c
index 2fb85bf75449..13098b09a824 100644
--- a/drivers/scsi/isci/port.c
+++ b/drivers/scsi/isci/port.c
@@ -212,7 +212,7 @@ static void isci_port_link_up(struct isci_host *isci_host,
 		memcpy(iphy->sas_phy.attached_sas_addr,
 		       iphy->frame_rcvd.iaf.sas_addr, SAS_ADDR_SIZE);
 	} else {
-		dev_err(&isci_host->pdev->dev, "%s: unkown target\n", __func__);
+		dev_err(&isci_host->pdev->dev, "%s: unknown target\n", __func__);
 		success = false;
 	}
 
diff --git a/drivers/scsi/isci/request.c b/drivers/scsi/isci/request.c
index 7a0431c73493..c1bafc3f3fb1 100644
--- a/drivers/scsi/isci/request.c
+++ b/drivers/scsi/isci/request.c
@@ -2240,7 +2240,7 @@ static enum sci_status atapi_data_tc_completion_handler(struct isci_request *ire
 			status = ireq->sci_status;
 			sci_change_state(&idev->sm, SCI_STP_DEV_ATAPI_ERROR);
 		} else {
-			/* If receiving any non-sucess TC status, no UF
+			/* If receiving any non-success TC status, no UF
 			 * received yet, then an UF for the status fis
 			 * is coming after (XXX: suspect this is
 			 * actually a protocol error or a bug like the
diff --git a/drivers/scsi/isci/task.c b/drivers/scsi/isci/task.c
index 6bc74eb012c9..b6f19a1db780 100644
--- a/drivers/scsi/isci/task.c
+++ b/drivers/scsi/isci/task.c
@@ -532,7 +532,7 @@ int isci_task_abort_task(struct sas_task *task)
 		/* The request has already completed and there
 		* is nothing to do here other than to set the task
 		* done bit, and indicate that the task abort function
-		* was sucessful.
+		* was successful.
 		*/
 		spin_lock_irqsave(&task->task_state_lock, flags);
 		task->task_state_flags |= SAS_TASK_STATE_DONE;
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 45c15208be9f..628a703abddb 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -6607,7 +6607,7 @@ out_error:
  * we just use some constant number as place holder.
  *
  * Return codes
- *      0 - sucessful
+ *      0 - successful
  *      -ENOMEM - No availble memory
  *      -EIO - The mailbox failed to complete successfully.
  **/
@@ -10425,7 +10425,7 @@ static struct pci_device_id lpfc_id_table[] = {
 
 MODULE_DEVICE_TABLE(pci, lpfc_id_table);
 
-static struct pci_error_handlers lpfc_err_handler = {
+static const struct pci_error_handlers lpfc_err_handler = {
 	.error_detected = lpfc_io_error_detected,
 	.slot_reset = lpfc_io_slot_reset,
 	.resume = lpfc_io_resume,
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 9cbd20b1328b..0e7e144507b2 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -4739,7 +4739,7 @@ lpfc_sli4_read_rev(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
  * is attached to.
  *
  * Return codes
- *      0 - sucessful
+ *      0 - successful
  *      otherwise - failed to retrieve physical port name
  **/
 static int
@@ -15209,7 +15209,7 @@ lpfc_check_next_fcf_pri_level(struct lpfc_hba *phba)
 	/*
 	 * if next_fcf_pri was not set above and the list is not empty then
 	 * we have failed flogis on all of them. So reset flogi failed
-	 * and start at the begining.
+	 * and start at the beginning.
 	 */
 	if (!next_fcf_pri && !list_empty(&phba->fcf.fcf_pri_list)) {
 		list_for_each_entry(fcf_pri, &phba->fcf.fcf_pri_list, list) {
diff --git a/drivers/scsi/megaraid.c b/drivers/scsi/megaraid.c
index 97825f116954..76ad72d32c3f 100644
--- a/drivers/scsi/megaraid.c
+++ b/drivers/scsi/megaraid.c
@@ -305,12 +305,11 @@ mega_query_adapter(adapter_t *adapter)
 
 	adapter->host->sg_tablesize = adapter->sglen;
 
-
 	/* use HP firmware and bios version encoding
 	   Note: fw_version[0|1] and bios_version[0|1] were originally shifted
 	   right 8 bits making them zero. This 0 value was hardcoded to fix
 	   sparse warnings. */
-	if (adapter->product_info.subsysvid == HP_SUBSYS_VID) {
+	if (adapter->product_info.subsysvid == PCI_VENDOR_ID_HP) {
 		sprintf (adapter->fw_version, "%c%d%d.%d%d",
 			 adapter->product_info.fw_version[2],
 			 0,
@@ -4716,7 +4715,7 @@ megaraid_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	 * support, since this firmware cannot handle 64 bit
 	 * addressing
 	 */
-	if ((subsysvid == HP_SUBSYS_VID) &&
+	if ((subsysvid == PCI_VENDOR_ID_HP) &&
 	    ((subsysid == 0x60E7) || (subsysid == 0x60E8))) {
 		/*
 		 * which firmware
diff --git a/drivers/scsi/megaraid.h b/drivers/scsi/megaraid.h
index 9a7897f8ca43..4fb2adf6b80d 100644
--- a/drivers/scsi/megaraid.h
+++ b/drivers/scsi/megaraid.h
@@ -45,45 +45,10 @@
 
 #define MAX_DEV_TYPE	32
 
-#ifndef PCI_VENDOR_ID_LSI_LOGIC
-#define PCI_VENDOR_ID_LSI_LOGIC		0x1000
-#endif
-
-#ifndef PCI_VENDOR_ID_AMI
-#define PCI_VENDOR_ID_AMI		0x101E
-#endif
-
-#ifndef PCI_VENDOR_ID_DELL
-#define PCI_VENDOR_ID_DELL		0x1028
-#endif
-
-#ifndef PCI_VENDOR_ID_INTEL
-#define PCI_VENDOR_ID_INTEL		0x8086
-#endif
-
-#ifndef PCI_DEVICE_ID_AMI_MEGARAID
-#define PCI_DEVICE_ID_AMI_MEGARAID	0x9010
-#endif
-
-#ifndef PCI_DEVICE_ID_AMI_MEGARAID2
-#define PCI_DEVICE_ID_AMI_MEGARAID2	0x9060
-#endif
-
-#ifndef PCI_DEVICE_ID_AMI_MEGARAID3
-#define PCI_DEVICE_ID_AMI_MEGARAID3	0x1960
-#endif
-
 #define PCI_DEVICE_ID_DISCOVERY		0x000E
 #define PCI_DEVICE_ID_PERC4_DI		0x000F
 #define PCI_DEVICE_ID_PERC4_QC_VERDE	0x0407
 
-/* Sub-System Vendor IDs */
-#define	AMI_SUBSYS_VID			0x101E
-#define DELL_SUBSYS_VID			0x1028
-#define	HP_SUBSYS_VID			0x103C
-#define LSI_SUBSYS_VID			0x1000
-#define INTEL_SUBSYS_VID		0x8086
-
 #define HBA_SIGNATURE	      		0x3344
 #define HBA_SIGNATURE_471	  	0xCCCC
 #define HBA_SIGNATURE_64BIT		0x0299
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index b25757d1e91b..9d5a56c4b332 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -1209,6 +1209,13 @@ _base_check_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	u16 message_control;
 
 
+	/* Check whether controller SAS2008 B0 controller,
+	   if it is SAS2008 B0 controller use IO-APIC instead of MSIX */
+	if (ioc->pdev->device == MPI2_MFGPAGE_DEVID_SAS2008 &&
+	    ioc->pdev->revision == 0x01) {
+		return -EINVAL;
+	}
+
 	base = pci_find_capability(ioc->pdev, PCI_CAP_ID_MSIX);
 	if (!base) {
 		dfailprintk(ioc, printk(MPT2SAS_INFO_FMT "msix not "
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index b1ebd6f8dab3..1ccae45c5270 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -8306,7 +8306,7 @@ _scsih_pci_mmio_enabled(struct pci_dev *pdev)
 	return PCI_ERS_RESULT_NEED_RESET;
 }
 
-static struct pci_error_handlers _scsih_err_handler = {
+static const struct pci_error_handlers _scsih_err_handler = {
 	.error_detected = _scsih_pci_error_detected,
 	.mmio_enabled = _scsih_pci_mmio_enabled,
 	.slot_reset =	_scsih_pci_slot_reset,
diff --git a/drivers/scsi/mvumi.c b/drivers/scsi/mvumi.c
index 88cf1db21a79..783edc7c6b98 100644
--- a/drivers/scsi/mvumi.c
+++ b/drivers/scsi/mvumi.c
@@ -122,7 +122,7 @@ static struct mvumi_res *mvumi_alloc_mem_resource(struct mvumi_hba *mhba,
 
 	if (!res) {
 		dev_err(&mhba->pdev->dev,
-			"Failed to allocate memory for resouce manager.\n");
+			"Failed to allocate memory for resource manager.\n");
 		return NULL;
 	}
 
@@ -1007,13 +1007,13 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
 		tmp |= INT_MAP_COMAOUT | INT_MAP_COMAERR;
 		iowrite32(tmp, regs + CPU_ENPOINTA_MASK_REG);
 		iowrite32(mhba->list_num_io, mhba->ib_shadow);
-		/* Set InBound List Avaliable count shadow */
+		/* Set InBound List Available count shadow */
 		iowrite32(lower_32_bits(mhba->ib_shadow_phys),
 					regs + CLA_INB_AVAL_COUNT_BASEL);
 		iowrite32(upper_32_bits(mhba->ib_shadow_phys),
 					regs + CLA_INB_AVAL_COUNT_BASEH);
 
-		/* Set OutBound List Avaliable count shadow */
+		/* Set OutBound List Available count shadow */
 		iowrite32((mhba->list_num_io-1) | CL_POINTER_TOGGLE,
 						mhba->ob_shadow);
 		iowrite32(lower_32_bits(mhba->ob_shadow_phys), regs + 0x5B0);
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 9ce3a8f8754f..7cfdf2bd8edb 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -1615,13 +1615,11 @@ qla82xx_get_fw_offs(struct qla_hw_data *ha)
 char *
 qla82xx_pci_info_str(struct scsi_qla_host *vha, char *str)
 {
-	int pcie_reg;
 	struct qla_hw_data *ha = vha->hw;
 	char lwstr[6];
 	uint16_t lnk;
 
-	pcie_reg = pci_pcie_cap(ha->pdev);
-	pci_read_config_word(ha->pdev, pcie_reg + PCI_EXP_LNKSTA, &lnk);
+	pcie_capability_read_word(ha->pdev, PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
 	strcpy(str, "PCIe (");
@@ -2497,7 +2495,6 @@ fw_load_failed:
 int
 qla82xx_start_firmware(scsi_qla_host_t *vha)
 {
-	int           pcie_cap;
 	uint16_t      lnk;
 	struct qla_hw_data *ha = vha->hw;
 
@@ -2528,8 +2525,7 @@ qla82xx_start_firmware(scsi_qla_host_t *vha)
 	}
 
 	/* Negotiated Link width */
-	pcie_cap = pci_pcie_cap(ha->pdev);
-	pci_read_config_word(ha->pdev, pcie_cap + PCI_EXP_LNKSTA, &lnk);
+	pcie_capability_read_word(ha->pdev, PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
 	/* Synchronize with Receive peg */
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index fb8cd3847d4b..d3052622e77a 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -4471,7 +4471,7 @@ qla2xxx_pci_resume(struct pci_dev *pdev)
 	ha->flags.eeh_busy = 0;
 }
 
-static struct pci_error_handlers qla2xxx_err_handler = {
+static const struct pci_error_handlers qla2xxx_err_handler = {
 	.error_detected = qla2xxx_pci_error_detected,
 	.mmio_enabled = qla2xxx_pci_mmio_enabled,
 	.slot_reset = qla2xxx_pci_slot_reset,
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index 939d7261c37a..807bf76f1b6a 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -1566,7 +1566,6 @@ qla4_8xxx_set_qsnt_ready(struct scsi_qla_host *ha)
 static int
 qla4_8xxx_start_firmware(struct scsi_qla_host *ha, uint32_t image_start)
 {
-	int pcie_cap;
 	uint16_t lnk;
 
 	/* scrub dma mask expansion register */
@@ -1590,8 +1589,7 @@ qla4_8xxx_start_firmware(struct scsi_qla_host *ha, uint32_t image_start)
 	}
 
 	/* Negotiated Link width */
-	pcie_cap = pci_pcie_cap(ha->pdev);
-	pci_read_config_word(ha->pdev, pcie_cap + PCI_EXP_LNKSTA, &lnk);
+	pcie_capability_read_word(ha->pdev, PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
 	/* Synchronize with Receive peg */
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 9da426628b97..79243b76d17e 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -803,7 +803,7 @@ static void qla4xxx_conn_get_stats(struct iscsi_cls_conn *cls_conn,
 				     iscsi_stats_dma);
 	if (ret != QLA_SUCCESS) {
 		ql4_printk(KERN_ERR, ha,
-			   "Unable to retreive iscsi stats\n");
+			   "Unable to retrieve iscsi stats\n");
 		goto free_stats;
 	}
 
@@ -4338,7 +4338,7 @@ static int qla4xxx_compare_tuple_ddb(struct scsi_qla_host *ha,
 		return QLA_ERROR;
 
 	/* For multi sessions, driver generates the ISID, so do not compare
-	 * ISID in reset path since it would be a comparision between the
+	 * ISID in reset path since it would be a comparison between the
 	 * driver generated ISID and firmware generated ISID. This could
 	 * lead to adding duplicated DDBs in the list as driver generated
 	 * ISID would not match firmware generated ISID.
@@ -5326,7 +5326,7 @@ static void qla4xxx_destroy_fw_ddb_session(struct scsi_qla_host *ha)
 	}
 }
 /**
- * qla4xxx_remove_adapter - calback function to remove adapter.
+ * qla4xxx_remove_adapter - callback function to remove adapter.
  * @pci_dev: PCI device pointer
  **/
 static void __devexit qla4xxx_remove_adapter(struct pci_dev *pdev)
@@ -6148,7 +6148,7 @@ qla4xxx_pci_resume(struct pci_dev *pdev)
 	clear_bit(AF_EEH_BUSY, &ha->flags);
 }
 
-static struct pci_error_handlers qla4xxx_err_handler = {
+static const struct pci_error_handlers qla4xxx_err_handler = {
 	.error_detected = qla4xxx_pci_error_detected,
 	.mmio_enabled = qla4xxx_pci_mmio_enabled,
 	.slot_reset = qla4xxx_pci_slot_reset,
diff --git a/drivers/scsi/sym53c8xx_2/sym_glue.c b/drivers/scsi/sym53c8xx_2/sym_glue.c
index 36d1ed7817eb..e2b8e68b57e7 100644
--- a/drivers/scsi/sym53c8xx_2/sym_glue.c
+++ b/drivers/scsi/sym53c8xx_2/sym_glue.c
@@ -2117,7 +2117,7 @@ static struct pci_device_id sym2_id_table[] __devinitdata = {
 
 MODULE_DEVICE_TABLE(pci, sym2_id_table);
 
-static struct pci_error_handlers sym2_err_handler = {
+static const struct pci_error_handlers sym2_err_handler = {
 	.error_detected	= sym2_io_error_detected,
 	.mmio_enabled	= sym2_io_slot_dump,
 	.slot_reset	= sym2_io_slot_reset,
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index c7030fbee79c..3e79a2f00042 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -331,7 +331,7 @@ static void virtscsi_map_sgl(struct scatterlist *sg, unsigned int *p_idx,
 	int i;
 
 	for_each_sg(table->sgl, sg_elem, table->nents, i)
-		sg_set_buf(&sg[idx++], sg_virt(sg_elem), sg_elem->length);
+		sg[idx++] = *sg_elem;
 
 	*p_idx = idx;
 }
diff --git a/drivers/scsi/vmw_pvscsi.c b/drivers/scsi/vmw_pvscsi.c
index 4411d4224401..20b3a483c2cc 100644
--- a/drivers/scsi/vmw_pvscsi.c
+++ b/drivers/scsi/vmw_pvscsi.c
@@ -295,7 +295,7 @@ static void ll_adapter_reset(const struct pvscsi_adapter *adapter)
 
 static void ll_bus_reset(const struct pvscsi_adapter *adapter)
 {
-	dev_dbg(pvscsi_dev(adapter), "Reseting bus on %p\n", adapter);
+	dev_dbg(pvscsi_dev(adapter), "Resetting bus on %p\n", adapter);
 
 	pvscsi_write_cmd_desc(adapter, PVSCSI_CMD_RESET_BUS, NULL, 0);
 }
@@ -304,7 +304,7 @@ static void ll_device_reset(const struct pvscsi_adapter *adapter, u32 target)
 {
 	struct PVSCSICmdDescResetDevice cmd = { 0 };
 
-	dev_dbg(pvscsi_dev(adapter), "Reseting device: target=%u\n", target);
+	dev_dbg(pvscsi_dev(adapter), "Resetting device: target=%u\n", target);
 
 	cmd.target = target;
 
diff --git a/drivers/sh/intc/core.c b/drivers/sh/intc/core.c
index 32c26d795ed0..8f32a1323a79 100644
--- a/drivers/sh/intc/core.c
+++ b/drivers/sh/intc/core.c
@@ -355,7 +355,7 @@ int __init register_intc_controller(struct intc_desc *desc)
 			if (unlikely(res)) {
 				if (res == -EEXIST) {
 					res = irq_domain_associate(d->domain,
-								   irq, irq);
+								   irq2, irq2);
 					if (unlikely(res)) {
 						pr_err("domain association "
 						       "failure\n");
diff --git a/drivers/sh/pfc/pinctrl.c b/drivers/sh/pfc/pinctrl.c
index 2804eaae804e..0646bf6e7889 100644
--- a/drivers/sh/pfc/pinctrl.c
+++ b/drivers/sh/pfc/pinctrl.c
@@ -208,10 +208,13 @@ static int sh_pfc_gpio_request_enable(struct pinctrl_dev *pctldev,
 
 		break;
 	case PINMUX_TYPE_GPIO:
+	case PINMUX_TYPE_INPUT:
+	case PINMUX_TYPE_OUTPUT:
 		break;
 	default:
 		pr_err("Unsupported mux type (%d), bailing...\n", pinmux_type);
-		return -ENOTSUPP;
+		ret = -ENOTSUPP;
+		goto err;
 	}
 
 	ret = 0;
diff --git a/drivers/spi/spi-au1550.c b/drivers/spi/spi-au1550.c
index 5784c8799616..4de66d1cfe51 100644
--- a/drivers/spi/spi-au1550.c
+++ b/drivers/spi/spi-au1550.c
@@ -475,7 +475,7 @@ static irqreturn_t au1550_spi_dma_irq_callback(struct au1550_spi *hw)
 		/*
 		 * due to an spi error we consider transfer as done,
 		 * so mask all events until before next transfer start
-		 * and stop the possibly running dma immediatelly
+		 * and stop the possibly running dma immediately
 		 */
 		au1550_spi_mask_ack_all(hw);
 		au1xxx_dbdma_stop(hw->dma_rx_ch);
diff --git a/drivers/spi/spi-bfin-sport.c b/drivers/spi/spi-bfin-sport.c
index 1fe51198a622..6555ecd07302 100644
--- a/drivers/spi/spi-bfin-sport.c
+++ b/drivers/spi/spi-bfin-sport.c
@@ -467,7 +467,7 @@ bfin_sport_spi_pump_transfers(unsigned long data)
 		dev_dbg(drv_data->dev, "IO write error!\n");
 		drv_data->state = ERROR_STATE;
 	} else {
-		/* Update total byte transfered */
+		/* Update total byte transferred */
 		message->actual_length += transfer->len;
 		/* Move to next transfer of this msg */
 		drv_data->state = bfin_sport_spi_next_transfer(drv_data);
diff --git a/drivers/spi/spi-oc-tiny.c b/drivers/spi/spi-oc-tiny.c
index 698018fd992b..9d9071b730be 100644
--- a/drivers/spi/spi-oc-tiny.c
+++ b/drivers/spi/spi-oc-tiny.c
@@ -129,7 +129,7 @@ static int tiny_spi_txrx_bufs(struct spi_device *spi, struct spi_transfer *t)
 	unsigned int i;
 
 	if (hw->irq >= 0) {
-		/* use intrrupt driven data transfer */
+		/* use interrupt driven data transfer */
 		hw->len = t->len;
 		hw->txp = t->tx_buf;
 		hw->rxp = t->rx_buf;
diff --git a/drivers/spi/spi-ppc4xx.c b/drivers/spi/spi-ppc4xx.c
index 75ac9d48ef46..7a85f22b6474 100644
--- a/drivers/spi/spi-ppc4xx.c
+++ b/drivers/spi/spi-ppc4xx.c
@@ -101,7 +101,7 @@ struct spi_ppc4xx_regs {
 	u8 dummy;
 	/*
 	 * Clock divisor modulus register
-	 * This uses the follwing formula:
+	 * This uses the following formula:
 	 *    SCPClkOut = OPBCLK/(4(CDM + 1))
 	 * or
 	 *    CDM = (OPBCLK/4*SCPClkOut) - 1
@@ -201,7 +201,7 @@ static int spi_ppc4xx_setupxfer(struct spi_device *spi, struct spi_transfer *t)
 		return -EINVAL;
 	}
 
-	/* Write new configration */
+	/* Write new configuration */
 	out_8(&hw->regs->mode, cs->mode);
 
 	/* Set the clock */
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index cd56dcf46320..1284c9b74653 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -505,7 +505,7 @@ static int pch_spi_transfer(struct spi_device *pspi, struct spi_message *pmsg)
 	}
 
 	if (unlikely(pspi->max_speed_hz == 0)) {
-		dev_err(&pspi->dev, "%s pch_spi_tranfer maxspeed=%d\n",
+		dev_err(&pspi->dev, "%s pch_spi_transfer maxspeed=%d\n",
 			__func__, pspi->max_speed_hz);
 		retval = -EINVAL;
 		goto err_out;
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index e3402d5644dd..d805eef11915 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -122,8 +122,6 @@ source "drivers/staging/android/Kconfig"
 
 source "drivers/staging/telephony/Kconfig"
 
-source "drivers/staging/ramster/Kconfig"
-
 source "drivers/staging/ozwpan/Kconfig"
 
 source "drivers/staging/ccg/Kconfig"
@@ -136,4 +134,14 @@ source "drivers/staging/csr/Kconfig"
 
 source "drivers/staging/omap-thermal/Kconfig"
 
+source "drivers/staging/ramster/Kconfig"
+
+source "drivers/staging/silicom/Kconfig"
+
+source "drivers/staging/ced1401/Kconfig"
+
+source "drivers/staging/imx-drm/Kconfig"
+
+source "drivers/staging/dgrp/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 3be59d02cae4..76e2ebd596ff 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -54,9 +54,13 @@ obj-$(CONFIG_MFD_NVEC)		+= nvec/
 obj-$(CONFIG_DRM_OMAP)		+= omapdrm/
 obj-$(CONFIG_ANDROID)		+= android/
 obj-$(CONFIG_PHONE)		+= telephony/
-obj-$(CONFIG_RAMSTER)		+= ramster/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
 obj-$(CONFIG_USB_G_CCG)		+= ccg/
 obj-$(CONFIG_WIMAX_GDM72XX)	+= gdm72xx/
 obj-$(CONFIG_CSR_WIFI)		+= csr/
 obj-$(CONFIG_OMAP_BANDGAP)	+= omap-thermal/
+obj-$(CONFIG_ZCACHE2)		+= ramster/
+obj-$(CONFIG_NET_VENDOR_SILICOM)	+= silicom/
+obj-$(CONFIG_CED1401)		+= ced1401/
+obj-$(CONFIG_DRM_IMX)		+= imx-drm/
+obj-$(CONFIG_DGRP)		+= dgrp/
diff --git a/drivers/staging/android/alarm-dev.c b/drivers/staging/android/alarm-dev.c
index 5b7064005188..a9b293ff3cc8 100644
--- a/drivers/staging/android/alarm-dev.c
+++ b/drivers/staging/android/alarm-dev.c
@@ -67,10 +67,8 @@ static struct devalarm alarms[ANDROID_ALARM_TYPE_COUNT];
 
 static int is_wakeup(enum android_alarm_type type)
 {
-	if (type == ANDROID_ALARM_RTC_WAKEUP ||
-			type == ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP)
-		return 1;
-	return 0;
+	return (type == ANDROID_ALARM_RTC_WAKEUP ||
+		type == ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP);
 }
 
 
@@ -85,12 +83,9 @@ static void devalarm_start(struct devalarm *alrm, ktime_t exp)
 
 static int devalarm_try_to_cancel(struct devalarm *alrm)
 {
-	int ret;
 	if (is_wakeup(alrm->type))
-		ret = alarm_try_to_cancel(&alrm->u.alrm);
-	else
-		ret = hrtimer_try_to_cancel(&alrm->u.hrt);
-	return ret;
+		return alarm_try_to_cancel(&alrm->u.alrm);
+	return hrtimer_try_to_cancel(&alrm->u.hrt);
 }
 
 static void devalarm_cancel(struct devalarm *alrm)
@@ -223,10 +218,12 @@ from_old_alarm_set:
 		case ANDROID_ALARM_ELAPSED_REALTIME:
 			get_monotonic_boottime(&tmp_time);
 			break;
-		case ANDROID_ALARM_TYPE_COUNT:
 		case ANDROID_ALARM_SYSTEMTIME:
 			ktime_get_ts(&tmp_time);
 			break;
+		default:
+			rv = -EINVAL;
+			goto err1;
 		}
 		if (copy_to_user((void __user *)arg, &tmp_time,
 		    sizeof(tmp_time))) {
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 69cf2db1d69c..94a740d2883d 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -1,20 +1,20 @@
 /* mm/ashmem.c
-**
-** Anonymous Shared Memory Subsystem, ashmem
-**
-** Copyright (C) 2008 Google, Inc.
-**
-** Robert Love <rlove@google.com>
-**
-** 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.
-*/
+ *
+ * Anonymous Shared Memory Subsystem, ashmem
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * Robert Love <rlove@google.com>
+ *
+ * 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.
+ */
 
 #define pr_fmt(fmt) "ashmem: " fmt
 
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index 574e99210c36..a807129c7b5a 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -365,7 +365,7 @@ binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer);
 /*
  * copied from get_unused_fd_flags
  */
-int task_get_unused_fd_flags(struct binder_proc *proc, int flags)
+static int task_get_unused_fd_flags(struct binder_proc *proc, int flags)
 {
 	struct files_struct *files = proc->files;
 	int fd, error;
@@ -415,13 +415,13 @@ repeat:
 	else
 		__clear_close_on_exec(fd, fdt);
 	files->next_fd = fd + 1;
-#if 1
+
 	/* Sanity check */
 	if (fdt->fd[fd] != NULL) {
 		pr_warn("get_unused_fd: slot %d not NULL!\n", fd);
 		fdt->fd[fd] = NULL;
 	}
-#endif
+
 	error = fd;
 
 out:
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index f7b8237d5be7..1d5ed475364b 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -32,38 +32,50 @@
 
 #include <asm/ioctls.h>
 
-/*
+/**
  * struct logger_log - represents a specific log, such as 'main' or 'radio'
+ * @buffer:	The actual ring buffer
+ * @misc:	The "misc" device representing the log
+ * @wq:		The wait queue for @readers
+ * @readers:	This log's readers
+ * @mutex:	The mutex that protects the @buffer
+ * @w_off:	The current write head offset
+ * @head:	The head, or location that readers start reading at.
+ * @size:	The size of the log
+ * @logs:	The list of log channels
  *
  * This structure lives from module insertion until module removal, so it does
  * not need additional reference counting. The structure is protected by the
  * mutex 'mutex'.
  */
 struct logger_log {
-	unsigned char		*buffer;/* the ring buffer itself */
-	struct miscdevice	misc;	/* misc device representing the log */
-	wait_queue_head_t	wq;	/* wait queue for readers */
-	struct list_head	readers; /* this log's readers */
-	struct mutex		mutex;	/* mutex protecting buffer */
-	size_t			w_off;	/* current write head offset */
-	size_t			head;	/* new readers start here */
-	size_t			size;	/* size of the log */
-	struct list_head	logs;	/* list of log channels (myself)*/
+	unsigned char		*buffer;
+	struct miscdevice	misc;
+	wait_queue_head_t	wq;
+	struct list_head	readers;
+	struct mutex		mutex;
+	size_t			w_off;
+	size_t			head;
+	size_t			size;
+	struct list_head	logs;
 };
 
 static LIST_HEAD(log_list);
 
 
-/*
+/**
  * struct logger_reader - a logging device open for reading
+ * @log:	The associated log
+ * @list:	The associated entry in @logger_log's list
+ * @r_off:	The current read head offset.
  *
  * This object lives from open to release, so we don't need additional
  * reference counting. The structure is protected by log->mutex.
  */
 struct logger_reader {
-	struct logger_log	*log;	/* associated log */
-	struct list_head	list;	/* entry in logger_log's list */
-	size_t			r_off;	/* current read head offset */
+	struct logger_log	*log;
+	struct list_head	list;
+	size_t			r_off;
 };
 
 /* logger_offset - returns index 'n' into the log via (optimized) modulus */
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
index 2cb06e9d8f98..9b929a8c7468 100644
--- a/drivers/staging/android/logger.h
+++ b/drivers/staging/android/logger.h
@@ -20,14 +20,24 @@
 #include <linux/types.h>
 #include <linux/ioctl.h>
 
+/**
+ * struct logger_entry - defines a single entry that is given to a logger
+ * @len:	The length of the payload
+ * @__pad:	Two bytes of padding that appear to be required
+ * @pid:	The generating process' process ID
+ * @tid:	The generating process' thread ID
+ * @sec:	The number of seconds that have elapsed since the Epoch
+ * @nsec:	The number of nanoseconds that have elapsed since @sec
+ * @msg:	The message that is to be logged
+ */
 struct logger_entry {
-	__u16		len;	/* length of the payload */
-	__u16		__pad;	/* no matter what, we get 2 bytes of padding */
-	__s32		pid;	/* generating process's pid */
-	__s32		tid;	/* generating process's tid */
-	__s32		sec;	/* seconds since Epoch */
-	__s32		nsec;	/* nanoseconds */
-	char		msg[0];	/* the entry's payload */
+	__u16		len;
+	__u16		__pad;
+	__s32		pid;
+	__s32		tid;
+	__s32		sec;
+	__s32		nsec;
+	char		msg[0];
 };
 
 #define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
index 45c522cbe784..e81451425c01 100644
--- a/drivers/staging/android/timed_gpio.c
+++ b/drivers/staging/android/timed_gpio.c
@@ -161,18 +161,7 @@ static struct platform_driver timed_gpio_driver = {
 	},
 };
 
-static int __init timed_gpio_init(void)
-{
-	return platform_driver_register(&timed_gpio_driver);
-}
-
-static void __exit timed_gpio_exit(void)
-{
-	platform_driver_unregister(&timed_gpio_driver);
-}
-
-module_init(timed_gpio_init);
-module_exit(timed_gpio_exit);
+module_platform_driver(timed_gpio_driver);
 
 MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
 MODULE_DESCRIPTION("timed gpio driver");
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
index f63c1d3aeb64..00185478647a 100644
--- a/drivers/staging/asus_oled/asus_oled.c
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -42,8 +42,6 @@
 #define ASUS_OLED_NAME			"asus-oled"
 #define ASUS_OLED_UNDERSCORE_NAME	"asus_oled"
 
-#define ASUS_OLED_ERROR			"Asus OLED Display Error: "
-
 #define ASUS_OLED_STATIC		's'
 #define ASUS_OLED_ROLL			'r'
 #define ASUS_OLED_FLASH			'f'
@@ -57,8 +55,9 @@
 #define USB_DEVICE_ID_ASUS_LCM2     0x175b
 
 MODULE_AUTHOR("Jakub Schmidtke, sjakub@gmail.com");
-MODULE_DESCRIPTION("Asus OLED Driver v" ASUS_OLED_VERSION);
+MODULE_DESCRIPTION("Asus OLED Driver");
 MODULE_LICENSE("GPL");
+MODULE_VERSION(ASUS_OLED_VERSION);
 
 static struct class *oled_class;
 static int oled_num;
@@ -138,6 +137,7 @@ struct asus_oled_dev {
 	size_t			buf_size;
 	char			*buf;
 	uint8_t			enabled;
+	uint8_t			enabled_post_resume;
 	struct device		*dev;
 };
 
@@ -383,13 +383,13 @@ static int append_values(struct asus_oled_dev *odev, uint8_t val, size_t count)
 
 		default:
 			i = 0;
-			printk(ASUS_OLED_ERROR "Unknown OLED Pack Mode: %d!\n",
+			dev_err(odev->dev, "Unknown OLED Pack Mode: %d!\n",
 			       odev->pack_mode);
 			break;
 		}
 
 		if (i >= odev->buf_size) {
-			printk(ASUS_OLED_ERROR "Buffer overflow! Report a bug:"
+			dev_err(odev->dev, "Buffer overflow! Report a bug:"
 			       "offs: %d >= %d i: %d (x: %d y: %d)\n",
 			       (int) odev->buf_offs, (int) odev->buf_size,
 			       (int) i, (int) x, (int) y);
@@ -435,7 +435,7 @@ static ssize_t odev_set_picture(struct asus_oled_dev *odev,
 		odev->buf = kmalloc(odev->buf_size, GFP_KERNEL);
 		if (odev->buf == NULL) {
 			odev->buf_size = 0;
-			printk(ASUS_OLED_ERROR "Out of memory!\n");
+			dev_err(odev->dev, "Out of memory!\n");
 			return -ENOMEM;
 		}
 
@@ -473,7 +473,7 @@ static ssize_t odev_set_picture(struct asus_oled_dev *odev,
 			odev->pic_mode = buf[1];
 			break;
 		default:
-			printk(ASUS_OLED_ERROR "Wrong picture mode: '%c'.\n",
+			dev_err(odev->dev, "Wrong picture mode: '%c'.\n",
 			       buf[1]);
 			return -EIO;
 			break;
@@ -533,7 +533,7 @@ static ssize_t odev_set_picture(struct asus_oled_dev *odev,
 
 		if (odev->buf == NULL) {
 			odev->buf_size = 0;
-			printk(ASUS_OLED_ERROR "Out of memory!\n");
+			dev_err(odev->dev, "Out of memory!\n");
 			return -ENOMEM;
 		}
 
@@ -593,15 +593,15 @@ static ssize_t odev_set_picture(struct asus_oled_dev *odev,
 	return count;
 
 error_width:
-	printk(ASUS_OLED_ERROR "Wrong picture width specified.\n");
+	dev_err(odev->dev, "Wrong picture width specified.\n");
 	return -EIO;
 
 error_height:
-	printk(ASUS_OLED_ERROR "Wrong picture height specified.\n");
+	dev_err(odev->dev, "Wrong picture height specified.\n");
 	return -EIO;
 
 error_header:
-	printk(ASUS_OLED_ERROR "Wrong picture header.\n");
+	dev_err(odev->dev, "Wrong picture header.\n");
 	return -EIO;
 }
 
@@ -766,11 +766,45 @@ static void asus_oled_disconnect(struct usb_interface *interface)
 	dev_info(&interface->dev, "Disconnected Asus OLED device\n");
 }
 
+#ifdef CONFIG_PM
+static int asus_oled_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct asus_oled_dev *odev;
+
+	odev = usb_get_intfdata(intf);
+	if (!odev)
+		return -ENODEV;
+
+	odev->enabled_post_resume = odev->enabled;
+	enable_oled(odev, 0);
+
+	return 0;
+}
+
+static int asus_oled_resume(struct usb_interface *intf)
+{
+	struct asus_oled_dev *odev;
+
+	odev = usb_get_intfdata(intf);
+	if (!odev)
+		return -ENODEV;
+
+	enable_oled(odev, odev->enabled_post_resume);
+
+	return 0;
+}
+#else
+#define asus_oled_suspend NULL
+#define asus_oled_resume NULL
+#endif
+
 static struct usb_driver oled_driver = {
 	.name =		ASUS_OLED_NAME,
 	.probe =	asus_oled_probe,
 	.disconnect =	asus_oled_disconnect,
 	.id_table =	id_table,
+	.suspend =	asus_oled_suspend,
+	.resume =	asus_oled_resume,
 };
 
 static CLASS_ATTR_STRING(version, S_IRUGO,
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index cf411d1706b1..3d02c2ebfb8d 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -820,6 +820,7 @@ cntrlEnd:
 
 		if (copy_from_user(psFwInfo, IoBuffer.InputBuffer, IoBuffer.InputLength)) {
 			up(&Adapter->fw_download_sema);
+			kfree(psFwInfo);
 			return -EFAULT;
 		}
 
@@ -829,6 +830,7 @@ cntrlEnd:
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Something else is wrong %lu\n",
 					psFwInfo->u32FirmwareLength);
 			up(&Adapter->fw_download_sema);
+			kfree(psFwInfo);
 			Status = -EINVAL;
 			return Status;
 		}
diff --git a/drivers/staging/bcm/CmHost.c b/drivers/staging/bcm/CmHost.c
index b54ec974477f..325b592fd41f 100644
--- a/drivers/staging/bcm/CmHost.c
+++ b/drivers/staging/bcm/CmHost.c
@@ -235,7 +235,7 @@ void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, BOOLEA
  * @ingroup ctrl_pkt_functions
  * copy classifier rule into the specified SF index
  */
-static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stConvergenceSLTypes  *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, struct bcm_convergence_types *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	/* VOID *pvPhsContext = NULL; */
@@ -428,7 +428,7 @@ VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRu
  * @ingroup ctrl_pkt_functions
  */
 static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer to the Adapter structure */
-			register pstServiceFlowParamSI psfLocalSet, /* <Pointer to the ServiceFlowParamSI structure */
+			register struct bcm_connect_mgr_params *psfLocalSet, /* Pointer to the connection manager parameters structure */
 			register UINT uiSearchRuleIndex, /* <Index of Queue, to which this data belongs */
 			register UCHAR ucDsxType,
 			stLocalSFAddIndicationAlt *pstAddIndication) {
@@ -439,7 +439,7 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	enum E_CLASSIFIER_ACTION eClassifierAction = eInvalidClassifierAction;
 	B_UINT16 u16PacketClassificationRuleIndex = 0;
 	int i;
-	stConvergenceSLTypes *psfCSType = NULL;
+	struct bcm_convergence_types *psfCSType = NULL;
 	S_PHS_RULE sPhsRule;
 	USHORT uVCID = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
 	UINT UGIValue = 0;
@@ -915,7 +915,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 	if (!pstAddIndication->sfAuthorizedSet.bValid)
 		pstAddIndication->sfAuthorizedSet.bValid = 1;
 	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++) {
-		stConvergenceSLTypes *psfCSType = NULL;
+		struct bcm_convergence_types *psfCSType = NULL;
 		psfCSType =  &pstAddIndication->sfAuthorizedSet.cConvergenceSLTypes[nIndex];
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "psfCSType = %p", psfCSType);
@@ -999,13 +999,10 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 #ifdef VERSION_D5
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLableLength: 0x%X ",
 				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x %02X %02X %02X %02X %02X %02X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[0],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[1],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[2],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[3],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[4],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x%*ph ",
+				6, psfCSType->cCPacketClassificationRule.
+					      u8IPv6FlowLable);
 #endif
 	}
 
@@ -1015,13 +1012,9 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", pstAddIndication->sfAdmittedSet.u16CID);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassNameLength: 0x%X",
 			pstAddIndication->sfAdmittedSet.u8ServiceClassNameLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassName: 0x %02X %02X %02X %02X %02X %02X",
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[0],
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[1],
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[2],
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[3],
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[4],
-			pstAddIndication->sfAdmittedSet.u8ServiceClassName[5]);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
+			"u8ServiceClassName: 0x%*ph",
+			6, pstAddIndication->sfAdmittedSet.u8ServiceClassName);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8MBSService: 0x%02X", pstAddIndication->sfAdmittedSet.u8MBSService);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8QosParamSet: 0x%02X", pstAddIndication->sfAdmittedSet.u8QosParamSet);
@@ -1066,7 +1059,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 		nCurClassifierCnt = MAX_CLASSIFIERS_IN_SF;
 
 	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++) {
-		stConvergenceSLTypes *psfCSType = NULL;
+		struct bcm_convergence_types *psfCSType = NULL;
 
 		psfCSType =  &pstAddIndication->sfAdmittedSet.cConvergenceSLTypes[nIndex];
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
@@ -1074,10 +1067,10 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 				psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPTypeOfServiceLength: 0x%02X",
 				psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPTypeOfService[3]: 0x%02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8IPTypeOfService[3]: 0x%*ph",
+				3, psfCSType->cCPacketClassificationRule.
+					      u8IPTypeOfService);
 		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Protocol: 0x%02X ", psfCSType->cCPacketClassificationRule.u8Protocol);
 
@@ -1098,20 +1091,20 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolSourcePortRangeLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolSourcePortRange[4]: 0x %02X %02X %02X %02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8ProtocolSourcePortRange[4]: "
+				"0x%*ph ", 4, psfCSType->
+						cCPacketClassificationRule.
+						u8ProtocolSourcePortRange);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolDestPortRangeLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolDestPortRange[4]: 0x %02X %02X %02X %02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8ProtocolDestPortRange[4]: "
+				"0x%*ph ", 4, psfCSType->
+						cCPacketClassificationRule.
+						u8ProtocolDestPortRange);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddressLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
@@ -1130,10 +1123,10 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 						u8EthernetSourceMACAddress);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthertypeLength: 0x%02X ", psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Ethertype[3]: 0x%02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8Ethertype[0],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[1],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8Ethertype[3]: 0x%*ph",
+				3, psfCSType->cCPacketClassificationRule.
+					      u8Ethertype);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16UserPriority: 0x%X ", psfCSType->cCPacketClassificationRule.u16UserPriority);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16VLANID: 0x%X ", psfCSType->cCPacketClassificationRule.u16VLANID);
@@ -1147,13 +1140,10 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 #ifdef VERSION_D5
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLableLength: 0x%X ",
 				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x %02X %02X %02X %02X %02X %02X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[0],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[1],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[2],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[3],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[4],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x%*ph ",
+				6, psfCSType->cCPacketClassificationRule.
+					      u8IPv6FlowLable);
 #endif
 	}
 
@@ -1162,13 +1152,9 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32SFID: 0x%X", pstAddIndication->sfActiveSet.u32SFID);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", pstAddIndication->sfActiveSet.u16CID);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassNameLength: 0x%X", pstAddIndication->sfActiveSet.u8ServiceClassNameLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassName: 0x %02X %02X %02X %02X %02X %02X",
-			pstAddIndication->sfActiveSet.u8ServiceClassName[0],
-			pstAddIndication->sfActiveSet.u8ServiceClassName[1],
-			pstAddIndication->sfActiveSet.u8ServiceClassName[2],
-			pstAddIndication->sfActiveSet.u8ServiceClassName[3],
-			pstAddIndication->sfActiveSet.u8ServiceClassName[4],
-			pstAddIndication->sfActiveSet.u8ServiceClassName[5]);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
+			"u8ServiceClassName: 0x%*ph",
+			6, pstAddIndication->sfActiveSet.u8ServiceClassName);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8MBSService: 0x%02X", pstAddIndication->sfActiveSet.u8MBSService);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8QosParamSet: 0x%02X", pstAddIndication->sfActiveSet.u8QosParamSet);
@@ -1212,7 +1198,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 		nCurClassifierCnt = MAX_CLASSIFIERS_IN_SF;
 
 	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++)	{
-		stConvergenceSLTypes *psfCSType = NULL;
+		struct bcm_convergence_types *psfCSType = NULL;
 
 		psfCSType =  &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
@@ -1314,7 +1300,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 
 static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter, ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
 {
-	UINT  nBytesToRead = sizeof(stServiceFlowParamSI);
+	UINT  nBytesToRead = sizeof(struct bcm_connect_mgr_params);
 
 	if (ulAddrSFParamSet == 0 || NULL == pucDestBuffer) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Got Param address as 0!!");
@@ -1331,7 +1317,7 @@ static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter, ULONG ulAdd
 
 static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer, ULONG ulAddrSFParamSet)
 {
-	UINT nBytesToWrite = sizeof(stServiceFlowParamSI);
+	UINT nBytesToWrite = sizeof(struct bcm_connect_mgr_params);
 	int ret = 0;
 
 	if (ulAddrSFParamSet == 0 || NULL == pucSrcBuffer)
@@ -1348,8 +1334,8 @@ static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
 ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer, UINT *puBufferLength)
 {
 	stLocalSFAddIndicationAlt *pstAddIndicationAlt = NULL;
-	stLocalSFAddIndication *pstAddIndication = NULL;
-	stLocalSFDeleteRequest *pstDeletionRequest;
+	struct bcm_add_indication *pstAddIndication = NULL;
+	struct bcm_del_request *pstDeletionRequest;
 	UINT uiSearchRuleIndex;
 	ULONG ulSFID;
 
@@ -1360,7 +1346,7 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	 * we can stop the further classifying the pkt for this SF.
 	 */
 	if (pstAddIndicationAlt->u8Type == DSD_REQ) {
-		pstDeletionRequest = (stLocalSFDeleteRequest *)pvBuffer;
+		pstDeletionRequest = (struct bcm_del_request *)pvBuffer;
 
 		ulSFID = ntohl(pstDeletionRequest->u32SFID);
 		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
@@ -1379,12 +1365,12 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 	/* For DSA_REQ, only up to "psfAuthorizedSet" parameter should be accessed by driver! */
 
-	pstAddIndication = kmalloc(sizeof(*pstAddIndication), GFP_KERNEL);
+	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication), GFP_KERNEL);
 	if (pstAddIndication == NULL)
 		return 0;
 
 	/* AUTHORIZED SET */
-	pstAddIndication->psfAuthorizedSet = (stServiceFlowParamSI *)
+	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)
 			GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAuthorizedSet) {
 		kfree(pstAddIndication);
@@ -1398,10 +1384,10 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 
 	/* this can't possibly be right */
-	pstAddIndication->psfAuthorizedSet = (stServiceFlowParamSI *)ntohl((ULONG)pstAddIndication->psfAuthorizedSet);
+	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAuthorizedSet);
 
 	if (pstAddIndicationAlt->u8Type == DSA_REQ) {
-		stLocalSFAddRequest AddRequest;
+		struct bcm_add_request AddRequest;
 
 		AddRequest.u8Type = pstAddIndicationAlt->u8Type;
 		AddRequest.eConnectionDir = pstAddIndicationAlt->u8Direction;
@@ -1409,8 +1395,8 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 		AddRequest.u16CID = pstAddIndicationAlt->u16CID;
 		AddRequest.u16VCID = pstAddIndicationAlt->u16VCID;
 		AddRequest.psfParameterSet = pstAddIndication->psfAuthorizedSet;
-		(*puBufferLength) = sizeof(stLocalSFAddRequest);
-		memcpy(pvBuffer, &AddRequest, sizeof(stLocalSFAddRequest));
+		(*puBufferLength) = sizeof(struct bcm_add_request);
+		memcpy(pvBuffer, &AddRequest, sizeof(struct bcm_add_request));
 		kfree(pstAddIndication);
 		return 1;
 	}
@@ -1426,7 +1412,7 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	pstAddIndication->u8CC = pstAddIndicationAlt->u8CC;
 
 	/* ADMITTED SET */
-	pstAddIndication->psfAdmittedSet = (stServiceFlowParamSI *)
+	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)
 		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAdmittedSet) {
 		kfree(pstAddIndication);
@@ -1437,10 +1423,10 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 		return 0;
 	}
 
-	pstAddIndication->psfAdmittedSet = (stServiceFlowParamSI *)ntohl((ULONG)pstAddIndication->psfAdmittedSet);
+	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAdmittedSet);
 
 	/* ACTIVE SET */
-	pstAddIndication->psfActiveSet = (stServiceFlowParamSI *)
+	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)
 		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfActiveSet) {
 		kfree(pstAddIndication);
@@ -1451,10 +1437,10 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 		return 0;
 	}
 
-	pstAddIndication->psfActiveSet = (stServiceFlowParamSI *)ntohl((ULONG)pstAddIndication->psfActiveSet);
+	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfActiveSet);
 
-	(*puBufferLength) = sizeof(stLocalSFAddIndication);
-	*(stLocalSFAddIndication *)pvBuffer = *pstAddIndication;
+	(*puBufferLength) = sizeof(struct bcm_add_indication);
+	*(struct bcm_add_indication *)pvBuffer = *pstAddIndication;
 	kfree(pstAddIndication);
 	return 1;
 }
@@ -1463,10 +1449,10 @@ static inline stLocalSFAddIndicationAlt
 *RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter, register PVOID pvBuffer)
 {
 	ULONG ulStatus = 0;
-	stLocalSFAddIndication *pstAddIndication = NULL;
+	struct bcm_add_indication *pstAddIndication = NULL;
 	stLocalSFAddIndicationAlt *pstAddIndicationDest = NULL;
 
-	pstAddIndication = (stLocalSFAddIndication *)(pvBuffer);
+	pstAddIndication = (struct bcm_add_indication *)(pvBuffer);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "=====>");
 	if ((pstAddIndication->u8Type == DSD_REQ) ||
 		(pstAddIndication->u8Type == DSD_RSP) ||
@@ -1553,7 +1539,7 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 	if (Adapter->astTargetDsxBuffer[0].ulTargetDsxBuffer)
 		return 1;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Size of Each DSX Buffer(Also size of ServiceFlowParamSI): %zx ", sizeof(stServiceFlowParamSI));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Size of Each DSX Buffer(Also size of connection manager parameters): %zx ", sizeof(struct bcm_connect_mgr_params));
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Reading DSX buffer From Target location %x ", DSX_MESSAGE_EXCHANGE_BUFFER);
 
 	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER, (PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
@@ -1564,7 +1550,7 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Base Address Of DSX  Target Buffer : 0x%lx", ulTargetDsxBuffersBase);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
-	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE / sizeof(stServiceFlowParamSI);
+	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE / sizeof(struct bcm_connect_mgr_params);
 
 	Adapter->ulTotalTargetBuffersAvailable =
 		ulCntTargetBuffers > MAX_TARGET_DSX_BUFFERS ?
@@ -1576,7 +1562,7 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 		Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer = ulTargetDsxBuffersBase;
 		Adapter->astTargetDsxBuffer[i].valid = 1;
 		Adapter->astTargetDsxBuffer[i].tid = 0;
-		ulTargetDsxBuffersBase += sizeof(stServiceFlowParamSI);
+		ulTargetDsxBuffersBase += sizeof(struct bcm_connect_mgr_params);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "  Target DSX Buffer %lx setup at 0x%lx",
 				i, Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer);
 	}
@@ -1647,7 +1633,7 @@ int FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
 BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer to the Adapter structure */
 				PVOID pvBuffer /* Starting Address of the Buffer, that contains the AddIndication Data */)
 {
-	stServiceFlowParamSI *psfLocalSet = NULL;
+	struct bcm_connect_mgr_params *psfLocalSet = NULL;
 	stLocalSFAddIndicationAlt *pstAddIndication = NULL;
 	stLocalSFChangeIndicationAlt *pstChangeIndication = NULL;
 	struct bcm_leader *pLeader = NULL;
@@ -1658,7 +1644,7 @@ BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer
 	 */
 	pstAddIndication = RestoreCmControlResponseMessage(Adapter, pvBuffer);
 	if (pstAddIndication == NULL) {
-		ClearTargetDSXBuffer(Adapter, ((stLocalSFAddIndication *)pvBuffer)->u16TID, FALSE);
+		ClearTargetDSXBuffer(Adapter, ((struct bcm_add_indication *)pvBuffer)->u16TID, FALSE);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Error in restoring Service Flow param structure from DSx message");
 		return FALSE;
 	}
@@ -1870,10 +1856,10 @@ BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer
 		UINT uiSearchRuleIndex;
 		ULONG ulSFID;
 
-		pLeader->PLength = sizeof(stLocalSFDeleteIndication);
-		*((stLocalSFDeleteIndication *)&(Adapter->caDsxReqResp[LEADER_SIZE])) = *((stLocalSFDeleteIndication *)pstAddIndication);
+		pLeader->PLength = sizeof(struct bcm_del_indication);
+		*((struct bcm_del_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE])) = *((struct bcm_del_indication *)pstAddIndication);
 
-		ulSFID = ntohl(((stLocalSFDeleteIndication *)pstAddIndication)->u32SFID);
+		ulSFID = ntohl(((struct bcm_del_indication *)pstAddIndication)->u32SFID);
 		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "DSD - Removing connection %x", uiSearchRuleIndex);
 
@@ -1884,7 +1870,7 @@ BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer
 		}
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSD RESPONSE TO MAC");
-		((stLocalSFDeleteIndication *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSD_RSP;
+		((struct bcm_del_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSD_RSP;
 		CopyBufferToControlPacket(Adapter, (PVOID)Adapter->caDsxReqResp);
 	}
 	case DSD_RSP:
@@ -1927,7 +1913,7 @@ int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId
 VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer)
 {
 	B_UINT32 u32NumofSFsinMsg = ntohl(*(puiBuffer + 1));
-	stIM_SFHostNotify *pHostInfo = NULL;
+	struct bcm_stim_sfhostnotify *pHostInfo = NULL;
 	UINT uiSearchRuleIndex = 0;
 	ULONG ulSFID = 0;
 
@@ -1936,7 +1922,7 @@ VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer
 
 	while (u32NumofSFsinMsg != 0 && u32NumofSFsinMsg < NO_OF_QUEUES) {
 		u32NumofSFsinMsg--;
-		pHostInfo = (stIM_SFHostNotify *)puiBuffer;
+		pHostInfo = (struct bcm_stim_sfhostnotify *)puiBuffer;
 		puiBuffer = (PUINT)(pHostInfo + 1);
 
 		ulSFID = ntohl(pHostInfo->SFID);
diff --git a/drivers/staging/bcm/CmHost.h b/drivers/staging/bcm/CmHost.h
index 4cc6f93f2321..1c5a07c7bbe2 100644
--- a/drivers/staging/bcm/CmHost.h
+++ b/drivers/staging/bcm/CmHost.h
@@ -35,8 +35,7 @@ typedef struct stLocalSFAddRequestAlt{
     B_UINT16                        u16VCID;
 
 
-	/// \brief structure ParameterSet
-    stServiceFlowParamSI              sfParameterSet;
+	struct bcm_connect_mgr_params sfParameterSet;
 
     //USE_MEMORY_MANAGER();
 }stLocalSFAddRequestAlt;
@@ -50,12 +49,9 @@ typedef struct stLocalSFAddIndicationAlt{
     B_UINT16                        u16CID;
     /// \brief 16bitVCID
     B_UINT16                        u16VCID;
-	/// \brief structure AuthorizedSet
-    stServiceFlowParamSI              sfAuthorizedSet;
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              sfAdmittedSet;
-	/// \brief structure ActiveSet
-    stServiceFlowParamSI              sfActiveSet;
+	struct bcm_connect_mgr_params sfAuthorizedSet;
+	struct bcm_connect_mgr_params sfAdmittedSet;
+	struct bcm_connect_mgr_params sfActiveSet;
 
 	B_UINT8 						u8CC;	/**<  Confirmation Code*/
 	B_UINT8 						u8Padd; 	/**<  8-bit Padding */
@@ -72,12 +68,9 @@ typedef struct stLocalSFAddConfirmationAlt{
     B_UINT16                        u16CID;
     /// \brief 16bitVCID
     B_UINT16                        u16VCID;
-    /// \brief structure AuthorizedSet
-    stServiceFlowParamSI              sfAuthorizedSet;
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              sfAdmittedSet;
-    /// \brief structure ActiveSet
-    stServiceFlowParamSI              sfActiveSet;
+	struct bcm_connect_mgr_params sfAuthorizedSet;
+	struct bcm_connect_mgr_params sfAdmittedSet;
+	struct bcm_connect_mgr_params sfActiveSet;
 }stLocalSFAddConfirmationAlt;
 
 
@@ -91,16 +84,13 @@ typedef struct stLocalSFChangeRequestAlt{
     /// \brief 16bitVCID
     B_UINT16                        u16VCID;
 	/*
-	//Pointer location at which following Service Flow param Structure can be read
-	//from the target. We get only the address location and we need to read out the
-	//entire SF param structure at the given location on target
+	//Pointer location at which following connection manager param Structure can be read
+	//from the target. We only get the address location and we need to read out the
+	//entire connection manager param structure at the given location on target
 	*/
-    /// \brief structure AuthorizedSet
-    stServiceFlowParamSI              sfAuthorizedSet;
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              sfAdmittedSet;
-    /// \brief structure ParameterSet
-    stServiceFlowParamSI              sfActiveSet;
+	struct bcm_connect_mgr_params sfAuthorizedSet;
+	struct bcm_connect_mgr_params sfAdmittedSet;
+	struct bcm_connect_mgr_params sfActiveSet;
 
 	B_UINT8 						u8CC;	/**<  Confirmation Code*/
 	B_UINT8 						u8Padd; 	/**<  8-bit Padding */
@@ -117,12 +107,9 @@ typedef struct stLocalSFChangeConfirmationAlt{
     B_UINT16                        u16CID;
     /// \brief 16bitVCID
     B_UINT16                        u16VCID;
-    /// \brief structure AuthorizedSet
-    stServiceFlowParamSI              sfAuthorizedSet;
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              sfAdmittedSet;
-    /// \brief structure ActiveSet
-    stServiceFlowParamSI              sfActiveSet;
+	struct bcm_connect_mgr_params sfAuthorizedSet;
+	struct bcm_connect_mgr_params sfAdmittedSet;
+	struct bcm_connect_mgr_params sfActiveSet;
 
 }stLocalSFChangeConfirmationAlt;
 
@@ -135,12 +122,9 @@ typedef struct stLocalSFChangeIndicationAlt{
     B_UINT16                        u16CID;
     /// \brief 16bitVCID
     B_UINT16                        u16VCID;
-    /// \brief structure AuthorizedSet
-    stServiceFlowParamSI              sfAuthorizedSet;
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              sfAdmittedSet;
-    /// \brief structure ActiveSet
-    stServiceFlowParamSI              sfActiveSet;
+	struct bcm_connect_mgr_params sfAuthorizedSet;
+	struct bcm_connect_mgr_params sfAdmittedSet;
+	struct bcm_connect_mgr_params sfActiveSet;
 
 	B_UINT8 						u8CC;	/**<  Confirmation Code*/
 	B_UINT8 						u8Padd; 	/**<  8-bit Padding */
diff --git a/drivers/staging/bcm/InterfaceInit.c b/drivers/staging/bcm/InterfaceInit.c
index 8f85de6a57ba..b05f5f73548c 100644
--- a/drivers/staging/bcm/InterfaceInit.c
+++ b/drivers/staging/bcm/InterfaceInit.c
@@ -8,6 +8,7 @@ static struct usb_device_id InterfaceUsbtable[] = {
 	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_226) },
 	{ USB_DEVICE(BCM_USB_VENDOR_ID_FOXCONN, BCM_USB_PRODUCT_ID_1901) },
 	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_ZTE_TU25) },
+	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_ZTE_226) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, InterfaceUsbtable);
@@ -669,16 +670,24 @@ struct class *bcm_class;
 
 static __init int bcm_init(void)
 {
-	printk(KERN_INFO "%s: %s, %s\n", DRV_NAME, DRV_DESCRIPTION, DRV_VERSION);
-	printk(KERN_INFO "%s\n", DRV_COPYRIGHT);
+	int retval;
+
+	pr_info("%s: %s, %s\n", DRV_NAME, DRV_DESCRIPTION, DRV_VERSION);
+	pr_info("%s\n", DRV_COPYRIGHT);
 
 	bcm_class = class_create(THIS_MODULE, DRV_NAME);
 	if (IS_ERR(bcm_class)) {
-		printk(KERN_ERR DRV_NAME ": could not create class\n");
+		pr_err(DRV_NAME ": could not create class\n");
 		return PTR_ERR(bcm_class);
 	}
 
-	return usb_register(&usbbcm_driver);
+	retval = usb_register(&usbbcm_driver);
+	if (retval < 0) {
+		pr_err(DRV_NAME ": could not register usb driver\n");
+		class_destroy(bcm_class);
+		return retval;
+	}
+	return 0;
 }
 
 static __exit void bcm_exit(void)
diff --git a/drivers/staging/bcm/InterfaceInit.h b/drivers/staging/bcm/InterfaceInit.h
index 058315a64c05..866924e35f9c 100644
--- a/drivers/staging/bcm/InterfaceInit.h
+++ b/drivers/staging/bcm/InterfaceInit.h
@@ -1,27 +1,26 @@
 #ifndef _INTERFACE_INIT_H
 #define _INTERFACE_INIT_H
 
-#define BCM_USB_VENDOR_ID_T3 	0x198f
-#define BCM_USB_VENDOR_ID_FOXCONN       0x0489
-#define BCM_USB_VENDOR_ID_ZTE   0x19d2
+#define BCM_USB_VENDOR_ID_T3	0x198f
+#define BCM_USB_VENDOR_ID_FOXCONN	0x0489
+#define BCM_USB_VENDOR_ID_ZTE	0x19d2
 
-#define BCM_USB_PRODUCT_ID_T3 	0x0300
-#define BCM_USB_PRODUCT_ID_T3B 	0x0210
-#define BCM_USB_PRODUCT_ID_T3L 	0x0220
-#define BCM_USB_PRODUCT_ID_SM250 	0xbccd
-#define BCM_USB_PRODUCT_ID_SYM  0x15E
-#define BCM_USB_PRODUCT_ID_1901 0xe017
-#define BCM_USB_PRODUCT_ID_226  0x0132
-#define BCM_USB_PRODUCT_ID_ZTE_TU25 0x0007
+#define BCM_USB_PRODUCT_ID_T3	0x0300
+#define BCM_USB_PRODUCT_ID_T3B	0x0210
+#define BCM_USB_PRODUCT_ID_T3L	0x0220
+#define BCM_USB_PRODUCT_ID_SM250	0xbccd
+#define BCM_USB_PRODUCT_ID_SYM	0x15E
+#define BCM_USB_PRODUCT_ID_1901	0xe017
+#define BCM_USB_PRODUCT_ID_226	0x0132 /* not sure if this is valid */
+#define BCM_USB_PRODUCT_ID_ZTE_226 0x172
+#define BCM_USB_PRODUCT_ID_ZTE_TU25	0x0007
 
-#define BCM_USB_MINOR_BASE 		192
+#define BCM_USB_MINOR_BASE	192
 
+int InterfaceInitialize(void);
 
-INT InterfaceInitialize(void);
+int InterfaceExit(void);
 
-INT InterfaceExit(void);
-
-INT usbbcm_worker_thread(PS_INTERFACE_ADAPTER psIntfAdapter);
+int usbbcm_worker_thread(PS_INTERFACE_ADAPTER psIntfAdapter);
 
 #endif
-
diff --git a/drivers/staging/bcm/Kconfig b/drivers/staging/bcm/Kconfig
index 96adb1026c4f..83c9752504d4 100644
--- a/drivers/staging/bcm/Kconfig
+++ b/drivers/staging/bcm/Kconfig
@@ -1,6 +1,6 @@
 config BCM_WIMAX
        tristate "Beceem BCS200/BCS220-3 and BCSM250 wimax support"
-       depends on USB && NET && EXPERIMENTAL
+       depends on USB && NET
        default N
        help
          This is an experimental driver for the Beceem WIMAX chipset used
diff --git a/drivers/staging/bcm/Misc.c b/drivers/staging/bcm/Misc.c
index f545716c666d..f13a9582a82f 100644
--- a/drivers/staging/bcm/Misc.c
+++ b/drivers/staging/bcm/Misc.c
@@ -752,7 +752,10 @@ VOID DumpPackInfo(struct bcm_mini_adapter *Adapter)
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "AuthzSet: %x\n", Adapter->PackInfo[uiLoopIndex].bAuthorizedSet);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ClassifyPrority: %x\n", Adapter->PackInfo[uiLoopIndex].bClassifierPriority);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiMaxLatency: %x\n", Adapter->PackInfo[uiLoopIndex].uiMaxLatency);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ServiceClassName: %x %x %x %x\n", Adapter->PackInfo[uiLoopIndex].ucServiceClassName[0], Adapter->PackInfo[uiLoopIndex].ucServiceClassName[1], Adapter->PackInfo[uiLoopIndex].ucServiceClassName[2], Adapter->PackInfo[uiLoopIndex].ucServiceClassName[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO,
+				DBG_LVL_ALL, "ServiceClassName: %*ph\n",
+				4, Adapter->PackInfo[uiLoopIndex].
+					    ucServiceClassName);
 /* BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "bHeaderSuppressionEnabled :%X\n", Adapter->PackInfo[uiLoopIndex].bHeaderSuppressionEnabled);
  * BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiTotalTxBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiTotalTxBytes);
  * BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiTotalRxBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiTotalRxBytes);
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
index 479574234e4c..6dc0bbcfeab0 100644
--- a/drivers/staging/bcm/PHSModule.c
+++ b/drivers/staging/bcm/PHSModule.c
@@ -66,7 +66,7 @@ Input parameters:		IN struct bcm_mini_adapter *Adapter         - Miniport Adapte
 						BOOLEAN bHeaderSuppressionEnabled - indicates if header suprression is enabled for SF.
 
 Return:					STATUS_SUCCESS - If the send was successful.
-						Other          - If an error occured.
+						Other  - If an error occurred.
 */
 
 int PHSTransmit(struct bcm_mini_adapter *Adapter,
@@ -346,7 +346,7 @@ int phs_init(PPHS_DEVICE_EXTENSION pPhsdeviceExtension, struct bcm_mini_adapter
 
 
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\n phs_init Successfull");
+	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "\n phs_init Successful");
 	return STATUS_SUCCESS;
 }
 
diff --git a/drivers/staging/bcm/Prototypes.h b/drivers/staging/bcm/Prototypes.h
index 3c8cc5ba2e2e..3ec8f800a5b0 100644
--- a/drivers/staging/bcm/Prototypes.h
+++ b/drivers/staging/bcm/Prototypes.h
@@ -95,7 +95,7 @@ void beceem_parse_target_struct(struct bcm_mini_adapter *Adapter);
 int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo);
 
 void CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
-		CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex);
+		struct bcm_connect_mgr_params *psfLocalSet, UINT uiSearchRuleIndex);
 
 VOID ResetCounters(struct bcm_mini_adapter *Adapter);
 
diff --git a/drivers/staging/bcm/Transmit.c b/drivers/staging/bcm/Transmit.c
index 5e603ce76fea..27e8c890777b 100644
--- a/drivers/staging/bcm/Transmit.c
+++ b/drivers/staging/bcm/Transmit.c
@@ -1,162 +1,149 @@
 /**
-@file Transmit.c
-@defgroup tx_functions Transmission
-@section Queueing
-@dot
-digraph transmit1 {
-node[shape=box]
-edge[weight=5;color=red]
-
-bcm_transmit->GetPacketQueueIndex[label="IP Packet"]
-GetPacketQueueIndex->IpVersion4[label="IPV4"]
-GetPacketQueueIndex->IpVersion6[label="IPV6"]
-}
-
-@enddot
-
-@section De-Queueing
-@dot
-digraph transmit2 {
-node[shape=box]
-edge[weight=5;color=red]
-interrupt_service_thread->transmit_packets
-tx_pkt_hdler->transmit_packets
-transmit_packets->CheckAndSendPacketFromIndex
-transmit_packets->UpdateTokenCount
-CheckAndSendPacketFromIndex->PruneQueue
-CheckAndSendPacketFromIndex->IsPacketAllowedForFlow
-CheckAndSendPacketFromIndex->SendControlPacket[label="control pkt"]
-SendControlPacket->bcm_cmd53
-CheckAndSendPacketFromIndex->SendPacketFromQueue[label="data pkt"]
-SendPacketFromQueue->SetupNextSend->bcm_cmd53
-}
-@enddot
-*/
+ * @file Transmit.c
+ * @defgroup tx_functions Transmission
+ * @section Queueing
+ * @dot
+ * digraph transmit1 {
+ * node[shape=box]
+ * edge[weight=5;color=red]
+ *
+ * bcm_transmit->GetPacketQueueIndex[label="IP Packet"]
+ * GetPacketQueueIndex->IpVersion4[label="IPV4"]
+ * GetPacketQueueIndex->IpVersion6[label="IPV6"]
+ * }
+ *
+ * @enddot
+ *
+ * @section De-Queueing
+ * @dot
+ * digraph transmit2 {
+ * node[shape=box]
+ * edge[weight=5;color=red]
+ * interrupt_service_thread->transmit_packets
+ * tx_pkt_hdler->transmit_packets
+ * transmit_packets->CheckAndSendPacketFromIndex
+ * transmit_packets->UpdateTokenCount
+ * CheckAndSendPacketFromIndex->PruneQueue
+ * CheckAndSendPacketFromIndex->IsPacketAllowedForFlow
+ * CheckAndSendPacketFromIndex->SendControlPacket[label="control pkt"]
+ * SendControlPacket->bcm_cmd53
+ * CheckAndSendPacketFromIndex->SendPacketFromQueue[label="data pkt"]
+ * SendPacketFromQueue->SetupNextSend->bcm_cmd53
+ * }
+ * @enddot
+ */
 
 #include "headers.h"
 
-
 /**
-@ingroup ctrl_pkt_functions
-This function dispatches control packet to the h/w interface
-@return zero(success) or -ve value(failure)
-*/
-INT SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
+ * @ingroup ctrl_pkt_functions
+ * This function dispatches control packet to the h/w interface
+ * @return zero(success) or -ve value(failure)
+ */
+int SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
 {
 	struct bcm_leader *PLeader = (struct bcm_leader *)pControlPacket;
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
-	if(!pControlPacket || !Adapter)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Got NULL Control Packet or Adapter");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
+	if (!pControlPacket || !Adapter) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Got NULL Control Packet or Adapter");
+		return STATUS_FAILURE;
+	}
+	if ((atomic_read(&Adapter->CurrNumFreeTxDesc) <
+			((PLeader->PLength-1)/MAX_DEVICE_DESC_SIZE)+1))	{
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
 		return STATUS_FAILURE;
 	}
-	if((atomic_read( &Adapter->CurrNumFreeTxDesc ) <
-		((PLeader->PLength-1)/MAX_DEVICE_DESC_SIZE)+1))
-    {
-    	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
-        return STATUS_FAILURE;
-    }
 
 	/* Update the netdevice statistics */
 	/* Dump Packet  */
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader Status: %x", PLeader->Status);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader VCID: %x",PLeader->Vcid);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader Length: %x",PLeader->PLength);
-	if(Adapter->device_removed)
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader Status: %x", PLeader->Status);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader VCID: %x", PLeader->Vcid);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Leader Length: %x", PLeader->PLength);
+	if (Adapter->device_removed)
 		return 0;
 
 	if (netif_msg_pktdata(Adapter))
 		print_hex_dump(KERN_DEBUG, PFX "tx control: ", DUMP_PREFIX_NONE,
-			       16, 1, pControlPacket, PLeader->PLength + LEADER_SIZE, 0);
+			16, 1, pControlPacket, PLeader->PLength + LEADER_SIZE, 0);
 
 	Adapter->interface_transmit(Adapter->pvInterfaceAdapter,
-					pControlPacket, (PLeader->PLength + LEADER_SIZE));
+				pControlPacket, (PLeader->PLength + LEADER_SIZE));
 
 	atomic_dec(&Adapter->CurrNumFreeTxDesc);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "<=========");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "<=========");
 	return STATUS_SUCCESS;
 }
 
 /**
-@ingroup tx_functions
-This function despatches the IP packets with the given vcid
-to the target via the host h/w interface.
-@return  zero(success) or -ve value(failure)
-*/
-INT SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USHORT Vcid)
+ * @ingroup tx_functions
+ * This function despatches the IP packets with the given vcid
+ * to the target via the host h/w interface.
+ * @return  zero(success) or -ve value(failure)
+ */
+int SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USHORT Vcid)
 {
-	int		status=0;
-	BOOLEAN bHeaderSupressionEnabled = FALSE;
-	B_UINT16            uiClassifierRuleID;
+	int	status = 0;
+	BOOLEAN	bHeaderSupressionEnabled = FALSE;
+	B_UINT16 uiClassifierRuleID;
 	u16	QueueIndex = skb_get_queue_mapping(Packet);
-	struct bcm_leader Leader={0};
+	struct bcm_leader Leader = {0};
 
-	if(Packet->len > MAX_DEVICE_DESC_SIZE)
-	{
+	if (Packet->len > MAX_DEVICE_DESC_SIZE) {
 		status = STATUS_FAILURE;
 		goto errExit;
 	}
 
 	/* Get the Classifier Rule ID */
-	uiClassifierRuleID = *((UINT32*) (Packet->cb)+SKB_CB_CLASSIFICATION_OFFSET);
+	uiClassifierRuleID = *((UINT32 *) (Packet->cb) + SKB_CB_CLASSIFICATION_OFFSET);
 
 	bHeaderSupressionEnabled = Adapter->PackInfo[QueueIndex].bHeaderSuppressionEnabled
 		& Adapter->bPHSEnabled;
 
-	if(Adapter->device_removed)
-		{
+	if (Adapter->device_removed) {
 		status = STATUS_FAILURE;
 		goto errExit;
-		}
+	}
 
 	status = PHSTransmit(Adapter, &Packet, Vcid, uiClassifierRuleID, bHeaderSupressionEnabled,
-							(UINT *)&Packet->len, Adapter->PackInfo[QueueIndex].bEthCSSupport);
+			(UINT *)&Packet->len, Adapter->PackInfo[QueueIndex].bEthCSSupport);
 
-	if(status != STATUS_SUCCESS)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL, "PHS Transmit failed..\n");
+	if (status != STATUS_SUCCESS) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL, "PHS Transmit failed..\n");
 		goto errExit;
 	}
 
-	Leader.Vcid	= Vcid;
+	Leader.Vcid = Vcid;
 
-	if(TCP_ACK == *((UINT32*) (Packet->cb) + SKB_CB_TCPACK_OFFSET ))
+	if (TCP_ACK == *((UINT32 *) (Packet->cb) + SKB_CB_TCPACK_OFFSET))
 		Leader.Status = LEADER_STATUS_TCP_ACK;
 	else
 		Leader.Status = LEADER_STATUS;
 
-	if(Adapter->PackInfo[QueueIndex].bEthCSSupport)
-	{
+	if (Adapter->PackInfo[QueueIndex].bEthCSSupport) {
 		Leader.PLength = Packet->len;
-		if(skb_headroom(Packet) < LEADER_SIZE)
-        {
-			if((status = skb_cow(Packet,LEADER_SIZE)))
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,"bcm_transmit : Failed To Increase headRoom\n");
+		if (skb_headroom(Packet) < LEADER_SIZE) {
+			status = skb_cow(Packet, LEADER_SIZE);
+			if (status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL, "bcm_transmit : Failed To Increase headRoom\n");
 				goto errExit;
 			}
 		}
 		skb_push(Packet, LEADER_SIZE);
 		memcpy(Packet->data, &Leader, LEADER_SIZE);
-	}
-	else
-	{
+	} else {
 		Leader.PLength = Packet->len - ETH_HLEN;
 		memcpy((struct bcm_leader *)skb_pull(Packet, (ETH_HLEN - LEADER_SIZE)), &Leader, LEADER_SIZE);
 	}
 
 	status = Adapter->interface_transmit(Adapter->pvInterfaceAdapter,
-			Packet->data, (Leader.PLength + LEADER_SIZE));
-	if(status)
-	{
+					Packet->data, (Leader.PLength + LEADER_SIZE));
+	if (status) {
 		++Adapter->dev->stats.tx_errors;
 		if (netif_msg_tx_err(Adapter))
 			pr_info(PFX "%s: transmit error %d\n", Adapter->dev->name,
 				status);
-	}
-	else
-	{
+	} else {
 		struct net_device_stats *netstats = &Adapter->dev->stats;
 		Adapter->PackInfo[QueueIndex].uiTotalTxBytes += Leader.PLength;
 
@@ -175,7 +162,6 @@ INT SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USH
 	atomic_dec(&Adapter->CurrNumFreeTxDesc);
 
 errExit:
-
 	dev_kfree_skb(Packet);
 	return status;
 }
@@ -188,73 +174,65 @@ static int tx_pending(struct bcm_mini_adapter *Adapter)
 }
 
 /**
-@ingroup tx_functions
-Transmit thread
-*/
-int tx_pkt_handler(struct bcm_mini_adapter *Adapter  /**< pointer to adapter object*/
-				)
+ * @ingroup tx_functions
+ * Transmit thread
+ */
+int tx_pkt_handler(struct bcm_mini_adapter *Adapter /**< pointer to adapter object*/)
 {
 	int status = 0;
 
-	while(! kthread_should_stop()) {
+	while (!kthread_should_stop()) {
 		/* FIXME - the timeout looks like workaround for racey usage of TxPktAvail */
-		if(Adapter->LinkUpStatus)
+		if (Adapter->LinkUpStatus)
 			wait_event_timeout(Adapter->tx_packet_wait_queue,
-					   tx_pending(Adapter), msecs_to_jiffies(10));
+					tx_pending(Adapter), msecs_to_jiffies(10));
 		else
 			wait_event_interruptible(Adapter->tx_packet_wait_queue,
-						 tx_pending(Adapter));
+						tx_pending(Adapter));
 
 		if (Adapter->device_removed)
 			break;
 
-		if(Adapter->downloadDDR == 1)
-		{
-			Adapter->downloadDDR +=1;
+		if (Adapter->downloadDDR == 1) {
+			Adapter->downloadDDR += 1;
 			status = download_ddr_settings(Adapter);
-			if(status)
+			if (status)
 				pr_err(PFX "DDR DOWNLOAD FAILED! %d\n", status);
 			continue;
 		}
 
-		//Check end point for halt/stall.
-		if(Adapter->bEndPointHalted == TRUE)
-		{
+		/* Check end point for halt/stall. */
+		if (Adapter->bEndPointHalted == TRUE) {
 			Bcm_clear_halt_of_endpoints(Adapter);
 			Adapter->bEndPointHalted = FALSE;
 			StartInterruptUrb((PS_INTERFACE_ADAPTER)(Adapter->pvInterfaceAdapter));
 		}
 
-		if(Adapter->LinkUpStatus && !Adapter->IdleMode)
-		{
-			if(atomic_read(&Adapter->TotalPacketCount))
-			{
+		if (Adapter->LinkUpStatus && !Adapter->IdleMode) {
+			if (atomic_read(&Adapter->TotalPacketCount))
 				update_per_sf_desc_cnts(Adapter);
-			}
 		}
 
-		if( atomic_read(&Adapter->CurrNumFreeTxDesc) &&
+		if (atomic_read(&Adapter->CurrNumFreeTxDesc) &&
 			Adapter->LinkStatus == SYNC_UP_REQUEST &&
-			!Adapter->bSyncUpRequestSent)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Calling LinkMessage");
+			!Adapter->bSyncUpRequestSent) {
+
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Calling LinkMessage");
 			LinkMessage(Adapter);
 		}
 
-		if((Adapter->IdleMode || Adapter->bShutStatus) && atomic_read(&Adapter->TotalPacketCount))
-		{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Device in Low Power mode...waking up");
-    			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
-				Adapter->bWakeUpDevice = TRUE;
-				wake_up(&Adapter->process_rx_cntrlpkt);
+		if ((Adapter->IdleMode || Adapter->bShutStatus) && atomic_read(&Adapter->TotalPacketCount)) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Device in Low Power mode...waking up");
+			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
+			Adapter->bWakeUpDevice = TRUE;
+			wake_up(&Adapter->process_rx_cntrlpkt);
 		}
 
 		transmit_packets(Adapter);
-
 		atomic_set(&Adapter->TxPktAvail, 0);
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Exiting the tx thread..\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Exiting the tx thread..\n");
 	Adapter->transmit_packet_thread = NULL;
 	return 0;
 }
diff --git a/drivers/staging/bcm/cntrl_SignalingInterface.h b/drivers/staging/bcm/cntrl_SignalingInterface.h
index 7619e4b819bd..990e809e9680 100644
--- a/drivers/staging/bcm/cntrl_SignalingInterface.h
+++ b/drivers/staging/bcm/cntrl_SignalingInterface.h
@@ -1,423 +1,311 @@
 #ifndef CNTRL_SIGNALING_INTERFACE_
 #define CNTRL_SIGNALING_INTERFACE_
 
-
-
-
-#define DSA_REQ 11
-#define DSA_RSP 12
-#define DSA_ACK 13
-#define DSC_REQ 14
-#define DSC_RSP 15
-#define DSC_ACK 16
-#define DSD_REQ 17
-#define DSD_RSP 18
-#define DSD_ACK 19
-#define MAX_CLASSIFIERS_IN_SF  4
-
-
-#define MAX_STRING_LEN 20
-#define MAX_PHS_LENGTHS 255
-#define VENDOR_PHS_PARAM_LENGTH 10
-#define MAX_NUM_ACTIVE_BS 10
-#define AUTH_TOKEN_LENGTH	10
-#define NUM_HARQ_CHANNELS	16	//Changed from 10 to 16 to accommodate all HARQ channels
-#define VENDOR_CLASSIFIER_PARAM_LENGTH 1 //Changed the size to 1 byte since we dnt use it
-#define  VENDOR_SPECIF_QOS_PARAM 1
-#define VENDOR_PHS_PARAM_LENGTH	10
-#define MBS_CONTENTS_ID_LENGTH	10
-#define GLOBAL_SF_CLASSNAME_LENGTH 6
-
-#define TYPE_OF_SERVICE_LENGTH				3
-#define IP_MASKED_SRC_ADDRESS_LENGTH			32
-#define IP_MASKED_DEST_ADDRESS_LENGTH		32
-#define PROTOCOL_SRC_PORT_RANGE_LENGTH		4
-#define PROTOCOL_DEST_PORT_RANGE_LENGTH		4
-#define ETHERNET_DEST_MAC_ADDR_LENGTH		12
-#define ETHERNET_SRC_MAC_ADDR_LENGTH		12
-#define NUM_ETHERTYPE_BYTES  3
-#define NUM_IPV6_FLOWLABLE_BYTES 3
-
-
-////////////////////////////////////////////////////////////////////////////////
-////////////////////////structure Definitions///////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////
-/// \brief class cCPacketClassificationRule
-struct _stCPacketClassificationRuleSI{
-
-	/**  16bit UserPriority Of The Service Flow*/
-    B_UINT16                        u16UserPriority;
-	/**  16bit VLANID Of The Service Flow*/
-    B_UINT16                        u16VLANID;
-	/**  16bit Packet Classification RuleIndex Of The Service Flow*/
-    B_UINT16                        u16PacketClassificationRuleIndex;
-	/**  8bit Classifier Rule Priority Of The Service Flow*/
-    B_UINT8                         u8ClassifierRulePriority;
-	/**  Length of IP TypeOfService field*/
-	B_UINT8                         u8IPTypeOfServiceLength;
-	/**  3bytes IP TypeOfService */
-    B_UINT8                         u8IPTypeOfService[TYPE_OF_SERVICE_LENGTH];
-	/** Protocol used in classification of Service Flow*/
-    B_UINT8                         u8Protocol;
-	/**  Length of IP Masked Source Address */
-    B_UINT8                         u8IPMaskedSourceAddressLength;
-	/**  IP Masked Source Address used in classification for the Service Flow*/
-    B_UINT8                         u8IPMaskedSourceAddress[IP_MASKED_SRC_ADDRESS_LENGTH];
-	/**  Length of IP Destination Address */
-    B_UINT8                         u8IPDestinationAddressLength;
-	/**  IP Destination Address used in classification for the Service Flow*/
-    B_UINT8                         u8IPDestinationAddress[IP_MASKED_DEST_ADDRESS_LENGTH];
-	/** Length of Protocol Source Port Range */
-    B_UINT8                         u8ProtocolSourcePortRangeLength;
-	/**  Protocol Source Port Range used in the Service Flow*/
-    B_UINT8                         u8ProtocolSourcePortRange[PROTOCOL_SRC_PORT_RANGE_LENGTH];
-	/** Length of Protocol Dest Port Range */
-    B_UINT8                         u8ProtocolDestPortRangeLength;
-	/**  Protocol Dest Port Range used in the Service Flow*/
-    B_UINT8                         u8ProtocolDestPortRange[PROTOCOL_DEST_PORT_RANGE_LENGTH];
-	/** Length of Ethernet Destination MAC Address  */
-    B_UINT8                         u8EthernetDestMacAddressLength;
-	/**  Ethernet Destination MAC Address  used in classification of the Service Flow*/
-    B_UINT8                         u8EthernetDestMacAddress[ETHERNET_DEST_MAC_ADDR_LENGTH];
-	/** Length of Ethernet Source MAC Address  */
-    B_UINT8                         u8EthernetSourceMACAddressLength;
-	/**  Ethernet Source MAC Address  used in classification of the Service Flow*/
-    B_UINT8                         u8EthernetSourceMACAddress[ETHERNET_SRC_MAC_ADDR_LENGTH];
-	/**  Length of Ethertype */
-	B_UINT8                         u8EthertypeLength;
-	/**  3bytes Ethertype Of The Service Flow*/
-    B_UINT8                         u8Ethertype[NUM_ETHERTYPE_BYTES];
-	/**  8bit Associated PHSI Of The Service Flow*/
-    B_UINT8                         u8AssociatedPHSI;
-	/** Length of Vendor Specific Classifier Param length Of The Service Flow*/
-    B_UINT8                         u8VendorSpecificClassifierParamLength;
-	/**  Vendor Specific Classifier Param Of The Service Flow*/
-    B_UINT8                         u8VendorSpecificClassifierParam[VENDOR_CLASSIFIER_PARAM_LENGTH];
-    /** Length Of IPv6 Flow Lable of the Service Flow*/
-    B_UINT8                         u8IPv6FlowLableLength;
-	/**  IPv6 Flow Lable Of The Service Flow*/
-    B_UINT8                         u8IPv6FlowLable[NUM_IPV6_FLOWLABLE_BYTES];
-	/**  Action associated with the classifier rule*/
-    B_UINT8							u8ClassifierActionRule;
-    B_UINT16							u16ValidityBitMap;
+#define DSA_REQ			11
+#define DSA_RSP			12
+#define DSA_ACK			13
+#define DSC_REQ			14
+#define DSC_RSP			15
+#define DSC_ACK			16
+#define DSD_REQ			17
+#define DSD_RSP			18
+#define DSD_ACK			19
+#define MAX_CLASSIFIERS_IN_SF	4
+
+#define MAX_STRING_LEN			20
+#define MAX_PHS_LENGTHS			255
+#define VENDOR_PHS_PARAM_LENGTH		10
+#define MAX_NUM_ACTIVE_BS		10
+#define AUTH_TOKEN_LENGTH		10
+#define NUM_HARQ_CHANNELS		16 /* Changed from 10 to 16 to accommodate all HARQ channels */
+#define VENDOR_CLASSIFIER_PARAM_LENGTH	1  /* Changed the size to 1 byte since we dnt use it */
+#define  VENDOR_SPECIF_QOS_PARAM	1
+#define VENDOR_PHS_PARAM_LENGTH		10
+#define MBS_CONTENTS_ID_LENGTH		10
+#define GLOBAL_SF_CLASSNAME_LENGTH	6
+
+#define TYPE_OF_SERVICE_LENGTH		3
+#define IP_MASKED_SRC_ADDRESS_LENGTH	32
+#define IP_MASKED_DEST_ADDRESS_LENGTH	32
+#define PROTOCOL_SRC_PORT_RANGE_LENGTH	4
+#define PROTOCOL_DEST_PORT_RANGE_LENGTH	4
+#define ETHERNET_DEST_MAC_ADDR_LENGTH	12
+#define ETHERNET_SRC_MAC_ADDR_LENGTH	12
+#define NUM_ETHERTYPE_BYTES		3
+#define NUM_IPV6_FLOWLABLE_BYTES	3
+
+struct bcm_packet_class_rules {
+	/* 16bit UserPriority Of The Service Flow */
+	B_UINT16 u16UserPriority;
+	/* 16bit VLANID Of The Service Flow */
+	B_UINT16 u16VLANID;
+	/* 16bit Packet Classification RuleIndex Of The Service Flow */
+	B_UINT16 u16PacketClassificationRuleIndex;
+	/* 8bit Classifier Rule Priority Of The Service Flow */
+	B_UINT8 u8ClassifierRulePriority;
+	/* Length of IP TypeOfService field */
+	B_UINT8 u8IPTypeOfServiceLength;
+	/* 3bytes IP TypeOfService */
+	B_UINT8 u8IPTypeOfService[TYPE_OF_SERVICE_LENGTH];
+	/* Protocol used in classification of Service Flow */
+	B_UINT8 u8Protocol;
+	/* Length of IP Masked Source Address */
+	B_UINT8 u8IPMaskedSourceAddressLength;
+	/* IP Masked Source Address used in classification for the Service Flow */
+	B_UINT8 u8IPMaskedSourceAddress[IP_MASKED_SRC_ADDRESS_LENGTH];
+	/* Length of IP Destination Address */
+	B_UINT8 u8IPDestinationAddressLength;
+	/* IP Destination Address used in classification for the Service Flow */
+	B_UINT8 u8IPDestinationAddress[IP_MASKED_DEST_ADDRESS_LENGTH];
+	/* Length of Protocol Source Port Range */
+	B_UINT8 u8ProtocolSourcePortRangeLength;
+	/* Protocol Source Port Range used in the Service Flow */
+	B_UINT8 u8ProtocolSourcePortRange[PROTOCOL_SRC_PORT_RANGE_LENGTH];
+	/* Length of Protocol Dest Port Range */
+	B_UINT8 u8ProtocolDestPortRangeLength;
+	/* Protocol Dest Port Range used in the Service Flow */
+	B_UINT8 u8ProtocolDestPortRange[PROTOCOL_DEST_PORT_RANGE_LENGTH];
+	/* Length of Ethernet Destination MAC Address */
+	B_UINT8 u8EthernetDestMacAddressLength;
+	/* Ethernet Destination MAC Address  used in classification of the Service Flow */
+	B_UINT8 u8EthernetDestMacAddress[ETHERNET_DEST_MAC_ADDR_LENGTH];
+	/* Length of Ethernet Source MAC Address */
+	B_UINT8 u8EthernetSourceMACAddressLength;
+	/* Ethernet Source MAC Address  used in classification of the Service Flow */
+	B_UINT8 u8EthernetSourceMACAddress[ETHERNET_SRC_MAC_ADDR_LENGTH];
+	/* Length of Ethertype */
+	B_UINT8 u8EthertypeLength;
+	/* 3bytes Ethertype Of The Service Flow */
+	B_UINT8 u8Ethertype[NUM_ETHERTYPE_BYTES];
+	/* 8bit Associated PHSI Of The Service Flow */
+	B_UINT8 u8AssociatedPHSI;
+	/* Length of Vendor Specific Classifier Param length Of The Service Flow */
+	B_UINT8 u8VendorSpecificClassifierParamLength;
+	/* Vendor Specific Classifier Param Of The Service Flow */
+	B_UINT8 u8VendorSpecificClassifierParam[VENDOR_CLASSIFIER_PARAM_LENGTH];
+	/* Length Of IPv6 Flow Lable of the Service Flow */
+	B_UINT8 u8IPv6FlowLableLength;
+	/* IPv6 Flow Lable Of The Service Flow */
+	B_UINT8 u8IPv6FlowLable[NUM_IPV6_FLOWLABLE_BYTES];
+	/* Action associated with the classifier rule */
+	B_UINT8 u8ClassifierActionRule;
+	B_UINT16 u16ValidityBitMap;
 };
-typedef struct _stCPacketClassificationRuleSI CCPacketClassificationRuleSI,stCPacketClassificationRuleSI, *pstCPacketClassificationRuleSI;
-
-/// \brief class CPhsRuleSI
-typedef struct _stPhsRuleSI {
-	/**  8bit PHS Index Of The Service Flow*/
-    B_UINT8                         u8PHSI;
-	/**  PHSF Length Of The Service Flow*/
-    B_UINT8                         u8PHSFLength;
-    /** String of bytes containing header information to be suppressed by the sending CS and reconstructed by the receiving CS*/
-    B_UINT8                         u8PHSF[MAX_PHS_LENGTHS];
-	/**  PHSM Length Of The Service Flow*/
-    B_UINT8                         u8PHSMLength;
-	/**  PHS Mask for the SF*/
-    B_UINT8                         u8PHSM[MAX_PHS_LENGTHS];
-	/**  8bit Total number of bytes to be suppressed for the Service Flow*/
-    B_UINT8                         u8PHSS;
-	/**  8bit Indicates whether or not Packet Header contents need to be verified prior to suppression */
-    B_UINT8                         u8PHSV;
-	/**  Vendor Specific PHS param Length Of The Service Flow*/
-    B_UINT8                         u8VendorSpecificPHSParamsLength;
-	/**  Vendor Specific PHS param Of The Service Flow*/
-    B_UINT8                         u8VendorSpecificPHSParams[VENDOR_PHS_PARAM_LENGTH];
-
-	B_UINT8                         u8Padding[2];
-}stPhsRuleSI,*pstPhsRuleSI;
-typedef stPhsRuleSI CPhsRuleSI;
 
-/// \brief structure cConvergenceSLTypes
-struct _stConvergenceSLTypes{
-	/**  8bit Phs Classfier Action Of The Service Flow*/
-    B_UINT8                         u8ClassfierDSCAction;
-	/**  8bit Phs DSC Action Of The Service Flow*/
-    B_UINT8                         u8PhsDSCAction;
-	/**   16bit Padding */
-    B_UINT8                         u8Padding[2];
-    /// \brief class cCPacketClassificationRule
-    stCPacketClassificationRuleSI     cCPacketClassificationRule;
-    /// \brief class CPhsRuleSI
-     struct _stPhsRuleSI		cPhsRule;
+struct bcm_phs_rules {
+	/* 8bit PHS Index Of The Service Flow */
+	B_UINT8 u8PHSI;
+	/* PHSF Length Of The Service Flow */
+	B_UINT8 u8PHSFLength;
+	/* String of bytes containing header information to be suppressed by the sending CS and reconstructed by the receiving CS */
+	B_UINT8 u8PHSF[MAX_PHS_LENGTHS];
+	/* PHSM Length Of The Service Flow */
+	B_UINT8 u8PHSMLength;
+	/* PHS Mask for the SF */
+	B_UINT8 u8PHSM[MAX_PHS_LENGTHS];
+	/* 8bit Total number of bytes to be suppressed for the Service Flow */
+	B_UINT8 u8PHSS;
+	/* 8bit Indicates whether or not Packet Header contents need to be verified prior to suppression */
+	B_UINT8 u8PHSV;
+	/* Vendor Specific PHS param Length Of The Service Flow */
+	B_UINT8 u8VendorSpecificPHSParamsLength;
+	/* Vendor Specific PHS param Of The Service Flow */
+	B_UINT8 u8VendorSpecificPHSParams[VENDOR_PHS_PARAM_LENGTH];
+	B_UINT8 u8Padding[2];
 };
-typedef struct _stConvergenceSLTypes stConvergenceSLTypes,CConvergenceSLTypes, *pstConvergenceSLTypes;
-
-
-/// \brief structure CServiceFlowParamSI
-typedef struct _stServiceFlowParamSI{
-
-     /**  32bitSFID Of The Service Flow*/
-    B_UINT32                        u32SFID;
-
-     /**  32bit Maximum Sustained Traffic Rate of the Service Flow*/
-    B_UINT32                        u32MaxSustainedTrafficRate;
-
-     /**  32bit Maximum Traffic Burst allowed for the Service Flow*/
-    B_UINT32                        u32MaxTrafficBurst;
-
-    /**  32bit Minimum Reserved Traffic Rate of the Service Flow*/
-    B_UINT32                        u32MinReservedTrafficRate;
-
-	/**  32bit Tolerated Jitter of the Service Flow*/
-    	B_UINT32                        u32ToleratedJitter;
-
-   /**  32bit Maximum Latency of the Service Flow*/
-    B_UINT32                        u32MaximumLatency;
-
-	/**  16bitCID Of The Service Flow*/
-    B_UINT16                        u16CID;
-
-     /**  16bit SAID on which the service flow being set up shall be mapped*/
-    B_UINT16                        u16TargetSAID;
-
-	/** 16bit  ARQ window size negotiated*/
-    B_UINT16                        u16ARQWindowSize;
-
-     /**  16bit Total Tx delay incl sending, receiving & processing delays 	*/
-    B_UINT16                        u16ARQRetryTxTimeOut;
-
-	/**  16bit Total Rx delay incl sending, receiving & processing delays 	*/
-    B_UINT16                        u16ARQRetryRxTimeOut;
-
-	/**  16bit ARQ block lifetime	*/
-    B_UINT16                        u16ARQBlockLifeTime;
-
-	/**  16bit ARQ Sync loss timeout*/
-    B_UINT16                        u16ARQSyncLossTimeOut;
-
-	 /**  16bit ARQ Purge timeout */
-    B_UINT16                        u16ARQRxPurgeTimeOut;
-//TODO::Remove this once we move to a new CORR2 driver
-    /// \brief Size of an ARQ block
-    B_UINT16                        u16ARQBlockSize;
-
-//#endif
-	/**  16bit Nominal interval b/w consecutive SDU arrivals at MAC SAP*/
-	B_UINT16                        u16SDUInterArrivalTime;
-
-	/**  16bit Specifies the time base for rate measurement 	*/
-	B_UINT16                        u16TimeBase;
-
-	 /** 16bit Interval b/w Successive Grant oppurtunities*/
-	B_UINT16                        u16UnsolicitedGrantInterval;
-
-	/** 16bit Interval b/w Successive Polling grant oppurtunities*/
-	B_UINT16						u16UnsolicitedPollingInterval;
-
-	 /**   internal var to get the overhead */
-	B_UINT16						u16MacOverhead;
-
-	 /**  MBS contents Identifier*/
-	B_UINT16						u16MBSContentsID[MBS_CONTENTS_ID_LENGTH];
-
-	/**  MBS contents Identifier length*/
-	B_UINT8							u8MBSContentsIDLength;
-
-	/**	 ServiceClassName Length Of The Service Flow*/
-    B_UINT8                         u8ServiceClassNameLength;
-
-	/**  32bytes ServiceClassName Of The Service Flow*/
-    B_UINT8                         u8ServiceClassName[32];
-
-	/**  8bit Indicates whether or not MBS service is requested for this Serivce Flow*/
-	B_UINT8							u8MBSService;
-
-    /**  8bit QOS Parameter Set specifies proper application of QoS paramters to Provisioned, Admitted and Active sets*/
-    B_UINT8                         u8QosParamSet;
-
-   /**  8bit Traffic Priority Of the Service Flow */
-    B_UINT8                         u8TrafficPriority;
-
-   /**  8bit Uplink Grant Scheduling Type of The Service Flow */
-    B_UINT8                         u8ServiceFlowSchedulingType;
-
-  /**  8bit Request transmission Policy of the Service Flow*/
-    B_UINT8							u8RequesttransmissionPolicy;
-
-	/**  8bit Specifies whether SDUs for this Service flow are of FixedLength or Variable length */
-    B_UINT8                         u8FixedLengthVSVariableLengthSDUIndicator;
-
-	/**  8bit Length of the SDU for a fixed length SDU service flow*/
-	B_UINT8                         u8SDUSize;
 
-	 /** 8bit Indicates whether or not ARQ is requested for this connection*/
-       B_UINT8                         u8ARQEnable;
-
-	/**<  8bit Indicates whether or not data has tobe delivered in order to higher layer*/
-       B_UINT8                         u8ARQDeliverInOrder;
-
-	/**  8bit Receiver ARQ ACK processing time */
-	B_UINT8                         u8RxARQAckProcessingTime;
-
-	/**  8bit Convergence Sublayer Specification Of The Service Flow*/
-       B_UINT8                         u8CSSpecification;
-
-	 /**  8 bit Type of data delivery service*/
-	B_UINT8                         u8TypeOfDataDeliveryService;
-
-	/** 8bit Specifies whether a service flow may generate Paging	*/
-	B_UINT8                         u8PagingPreference;
-
-	/**  8bit Indicates the MBS Zone through which the connection or virtual connection is valid	*/
-       B_UINT8                         u8MBSZoneIdentifierassignment;
-
-       /**  8bit Specifies whether traffic on SF should generate MOB_TRF_IND to MS in sleep mode*/
-	B_UINT8                         u8TrafficIndicationPreference;
-
-	/** 8bit Speciifes the length of predefined Global QoS parameter set encoding for this SF	*/
-	B_UINT8                         u8GlobalServicesClassNameLength;
-
-	 /**  6 byte Speciifes the predefined Global QoS parameter set encoding for this SF	*/
-	B_UINT8                         u8GlobalServicesClassName[GLOBAL_SF_CLASSNAME_LENGTH];
-
-	 /**  8bit Indicates whether or not SN feedback is enabled for the conn	*/
-	B_UINT8                         u8SNFeedbackEnabled;
-
-	 /**  Indicates the size of the Fragment Sequence Number for the connection	*/
-	B_UINT8                         u8FSNSize;
-
-	/** 8bit Number of CIDs in active BS list 	*/
-	B_UINT8							u8CIDAllocation4activeBSsLength;
-
-	/**  CIDs of BS in the active list	*/
-	B_UINT8							u8CIDAllocation4activeBSs[MAX_NUM_ACTIVE_BS];
-
-	 /**  Specifies if PDU extended subheader should be applied on every PDU on this conn*/
-	B_UINT8                         u8PDUSNExtendedSubheader4HarqReordering;
-
-	 /**  8bit Specifies whether the connection uses HARQ or not	*/
-	B_UINT8                         u8HARQServiceFlows;
-
-	/**  Specifies the length of Authorization token*/
-	B_UINT8							u8AuthTokenLength;
-
-	/**  Specifies the Authorization token*/
-	B_UINT8							u8AuthToken[AUTH_TOKEN_LENGTH];
-
-	/**  specifes Number of HARQ channels used to carry data length*/
-	B_UINT8							u8HarqChannelMappingLength;
-
-	 /**  specifes HARQ channels used to carry data*/
-	B_UINT8							u8HARQChannelMapping[NUM_HARQ_CHANNELS];
-
-	/**  8bit Length of Vendor Specific QoS Params */
-    B_UINT8                         u8VendorSpecificQoSParamLength;
-
-	/** 1byte  Vendor Specific QoS Param Of The Service Flow*/
-    B_UINT8                          u8VendorSpecificQoSParam[VENDOR_SPECIF_QOS_PARAM];
-
-	// indicates total classifiers in the SF
-	B_UINT8                         u8TotalClassifiers;  /**< Total number of valid classifiers*/
-	B_UINT8							bValid;	/**<  Validity flag */
-	B_UINT8				u8Padding;	 /**<  Padding byte*/
-
-/**
-Structure for Convergence SubLayer Types with a maximum of 4 classifiers
-*/
-	stConvergenceSLTypes		cConvergenceSLTypes[MAX_CLASSIFIERS_IN_SF];
-
-} stServiceFlowParamSI, *pstServiceFlowParamSI;
-typedef stServiceFlowParamSI CServiceFlowParamSI;
-
-/**
-structure stLocalSFAddRequest
-*/
-typedef struct _stLocalSFAddRequest{
-
-	B_UINT8                         u8Type;	/**<  Type*/
-	B_UINT8      eConnectionDir;		/**<  Connection direction*/
-	/// \brief 16 bit TID
-	B_UINT16                        u16TID;	/**<  16bit TID*/
-	/// \brief 16bitCID
-    	B_UINT16                        u16CID;	/**<  16bit CID*/
-	/// \brief 16bitVCID
-	B_UINT16                        u16VCID;	/**<  16bit VCID*/
-    /// \brief structure ParameterSet
-
-	stServiceFlowParamSI	*psfParameterSet;	/**<  structure ParameterSet*/
-
-}stLocalSFAddRequest, *pstLocalSFAddRequest;
-
-
-/**
-structure stLocalSFAddIndication
-*/
-typedef struct _stLocalSFAddIndication{
-
-	B_UINT8                         u8Type;	/**<  Type*/
-	B_UINT8      eConnectionDir;	/**<  Connection Direction*/
-	/// \brief 16 bit TID
-	B_UINT16                         u16TID;	/**<  TID*/
-    /// \brief 16bitCID
-    B_UINT16                        u16CID;		/**<  16bitCID*/
-    /// \brief 16bitVCID
-    B_UINT16                        u16VCID;	 /**<  16bitVCID*/
-
-
-    /// \brief structure AuthorizedSet
-    /// \brief structure AuthorizedSet
-    stServiceFlowParamSI              *psfAuthorizedSet;	/**<  AuthorizedSet of type stServiceFlowParamSI*/
-    /// \brief structure AdmittedSet
-    stServiceFlowParamSI              *psfAdmittedSet;	/**<  AdmittedSet of type stServiceFlowParamSI*/
-    /// \brief structure ActiveSet
-    stServiceFlowParamSI              *psfActiveSet;	/**<  sfActiveSet of type stServiceFlowParamSI*/
-	B_UINT8				   u8CC;	/**<  Confirmation Code*/
-	B_UINT8				   u8Padd;		/**<  8-bit Padding */
-
-    B_UINT16               u16Padd;	/**< 16 bit Padding */
-
-}stLocalSFAddIndication;
+struct bcm_convergence_types {
+	/* 8bit Phs Classfier Action Of The Service Flow */
+	B_UINT8 u8ClassfierDSCAction;
+	/* 8bit Phs DSC Action Of The Service Flow */
+	B_UINT8 u8PhsDSCAction;
+	/* 16bit Padding */
+	B_UINT8 u8Padding[2];
+	/* Packet classification rules structure */
+	struct bcm_packet_class_rules cCPacketClassificationRule;
+	/* Payload header suppression rules structure */
+	struct bcm_phs_rules cPhsRule;
+};
 
+struct bcm_connect_mgr_params {
+	/* 32bitSFID Of The Service Flow */
+	B_UINT32 u32SFID;
+	/* 32bit Maximum Sustained Traffic Rate of the Service Flow */
+	B_UINT32 u32MaxSustainedTrafficRate;
+	/* 32bit Maximum Traffic Burst allowed for the Service Flow */
+	B_UINT32 u32MaxTrafficBurst;
+	/* 32bit Minimum Reserved Traffic Rate of the Service Flow */
+	B_UINT32 u32MinReservedTrafficRate;
+	/* 32bit Tolerated Jitter of the Service Flow */
+	B_UINT32 u32ToleratedJitter;
+	/* 32bit Maximum Latency of the Service Flow */
+	B_UINT32 u32MaximumLatency;
+	/* 16bitCID Of The Service Flow */
+	B_UINT16 u16CID;
+	/* 16bit SAID on which the service flow being set up shall be mapped */
+	B_UINT16 u16TargetSAID;
+	/* 16bit  ARQ window size negotiated */
+	B_UINT16 u16ARQWindowSize;
+	/* 16bit Total Tx delay incl sending, receiving & processing delays */
+	B_UINT16 u16ARQRetryTxTimeOut;
+	/* 16bit Total Rx delay incl sending, receiving & processing delays */
+	B_UINT16 u16ARQRetryRxTimeOut;
+	/* 16bit ARQ block lifetime */
+	B_UINT16 u16ARQBlockLifeTime;
+	/* 16bit ARQ Sync loss timeout */
+	B_UINT16 u16ARQSyncLossTimeOut;
+	/* 16bit ARQ Purge timeout */
+	B_UINT16 u16ARQRxPurgeTimeOut;
+	/* TODO::Remove this once we move to a new CORR2 driver
+	 * brief Size of an ARQ block
+	 */
+	B_UINT16 u16ARQBlockSize;
+	/* #endif */
+	/* 16bit Nominal interval b/w consecutive SDU arrivals at MAC SAP */
+	B_UINT16 u16SDUInterArrivalTime;
+	/* 16bit Specifies the time base for rate measurement */
+	B_UINT16 u16TimeBase;
+	/* 16bit Interval b/w Successive Grant oppurtunities */
+	B_UINT16 u16UnsolicitedGrantInterval;
+	/* 16bit Interval b/w Successive Polling grant oppurtunities */
+	B_UINT16 u16UnsolicitedPollingInterval;
+	/* internal var to get the overhead */
+	B_UINT16 u16MacOverhead;
+	/* MBS contents Identifier */
+	B_UINT16 u16MBSContentsID[MBS_CONTENTS_ID_LENGTH];
+	/* MBS contents Identifier length */
+	B_UINT8 u8MBSContentsIDLength;
+	/* ServiceClassName Length Of The Service Flow */
+	B_UINT8 u8ServiceClassNameLength;
+	/* 32bytes ServiceClassName Of The Service Flow */
+	B_UINT8 u8ServiceClassName[32];
+	/* 8bit Indicates whether or not MBS service is requested for this Serivce Flow */
+	B_UINT8 u8MBSService;
+	/* 8bit QOS Parameter Set specifies proper application of QoS parameters to Provisioned, Admitted and Active sets */
+	B_UINT8 u8QosParamSet;
+	/* 8bit Traffic Priority Of the Service Flow */
+	B_UINT8 u8TrafficPriority;
+	/* 8bit Uplink Grant Scheduling Type of The Service Flow */
+	B_UINT8 u8ServiceFlowSchedulingType;
+	/* 8bit Request transmission Policy of the Service Flow */
+	B_UINT8 u8RequesttransmissionPolicy;
+	/* 8bit Specifies whether SDUs for this Service flow are of FixedLength or Variable length */
+	B_UINT8 u8FixedLengthVSVariableLengthSDUIndicator;
+	/* 8bit Length of the SDU for a fixed length SDU service flow */
+	B_UINT8 u8SDUSize;
+	/* 8bit Indicates whether or not ARQ is requested for this connection */
+	B_UINT8 u8ARQEnable;
+	/* < 8bit Indicates whether or not data has tobe delivered in order to higher layer */
+	B_UINT8 u8ARQDeliverInOrder;
+	/* 8bit Receiver ARQ ACK processing time */
+	B_UINT8 u8RxARQAckProcessingTime;
+	/* 8bit Convergence Sublayer Specification Of The Service Flow */
+	B_UINT8 u8CSSpecification;
+	/* 8 bit Type of data delivery service */
+	B_UINT8 u8TypeOfDataDeliveryService;
+	/* 8bit Specifies whether a service flow may generate Paging */
+	B_UINT8 u8PagingPreference;
+	/* 8bit Indicates the MBS Zone through which the connection or virtual connection is valid */
+	B_UINT8 u8MBSZoneIdentifierassignment;
+	/* 8bit Specifies whether traffic on SF should generate MOB_TRF_IND to MS in sleep mode */
+	B_UINT8 u8TrafficIndicationPreference;
+	/* 8bit Speciifes the length of predefined Global QoS parameter set encoding for this SF */
+	B_UINT8 u8GlobalServicesClassNameLength;
+	/* 6 byte Speciifes the predefined Global QoS parameter set encoding for this SF */
+	B_UINT8 u8GlobalServicesClassName[GLOBAL_SF_CLASSNAME_LENGTH];
+	/* 8bit Indicates whether or not SN feedback is enabled for the conn */
+	B_UINT8 u8SNFeedbackEnabled;
+	/* Indicates the size of the Fragment Sequence Number for the connection */
+	B_UINT8 u8FSNSize;
+	/* 8bit Number of CIDs in active BS list */
+	B_UINT8 u8CIDAllocation4activeBSsLength;
+	/* CIDs of BS in the active list */
+	B_UINT8 u8CIDAllocation4activeBSs[MAX_NUM_ACTIVE_BS];
+	/* Specifies if PDU extended subheader should be applied on every PDU on this conn */
+	B_UINT8 u8PDUSNExtendedSubheader4HarqReordering;
+	/* 8bit Specifies whether the connection uses HARQ or not */
+	B_UINT8 u8HARQServiceFlows;
+	/* Specifies the length of Authorization token */
+	B_UINT8 u8AuthTokenLength;
+	/* Specifies the Authorization token */
+	B_UINT8 u8AuthToken[AUTH_TOKEN_LENGTH];
+	/* specifes Number of HARQ channels used to carry data length */
+	B_UINT8 u8HarqChannelMappingLength;
+	/* specifes HARQ channels used to carry data */
+	B_UINT8 u8HARQChannelMapping[NUM_HARQ_CHANNELS];
+	/* 8bit Length of Vendor Specific QoS Params */
+	B_UINT8 u8VendorSpecificQoSParamLength;
+	/* 1byte  Vendor Specific QoS Param Of The Service Flow */
+	B_UINT8 u8VendorSpecificQoSParam[VENDOR_SPECIF_QOS_PARAM];
+	/* indicates total classifiers in the SF */
+	B_UINT8 u8TotalClassifiers;  /* < Total number of valid classifiers */
+	B_UINT8 bValid;	/* < Validity flag */
+	B_UINT8 u8Padding;	 /* < Padding byte */
+	/*
+	 * Structure for Convergence SubLayer Types with a maximum of 4 classifiers
+	 */
+	struct bcm_convergence_types cConvergenceSLTypes[MAX_CLASSIFIERS_IN_SF];
+};
 
-typedef struct _stLocalSFAddIndication *pstLocalSFAddIndication;
-/**
-structure stLocalSFChangeRequest is same as structure stLocalSFAddIndication
-*/
-typedef struct _stLocalSFAddIndication stLocalSFChangeRequest, *pstLocalSFChangeRequest;
-/**
-structure stLocalSFChangeIndication is same as structure stLocalSFAddIndication
-*/
-typedef struct _stLocalSFAddIndication stLocalSFChangeIndication, *pstLocalSFChangeIndication;
+struct bcm_add_request {
+	B_UINT8 u8Type;	/* < Type */
+	B_UINT8 eConnectionDir; /* < Connection direction */
+	/* brief 16 bit TID */
+	B_UINT16 u16TID; /* < 16bit TID */
+	/* brief 16bitCID */
+	B_UINT16 u16CID; /* < 16bit CID */
+	/* brief 16bitVCID */
+	B_UINT16 u16VCID; /* < 16bit VCID */
+	struct bcm_connect_mgr_params *psfParameterSet; /* < connection manager parameters */
+};
 
-/**
-structure stLocalSFDeleteRequest
-*/
-typedef struct _stLocalSFDeleteRequest{
-	B_UINT8                         u8Type;	 /**< Type*/
-	B_UINT8                         u8Padding;	 /**<  Padding byte*/
-	B_UINT16			u16TID;		 /**<  TID*/
-    /// \brief 32bitSFID
-    B_UINT32                        u32SFID;	 /**<  SFID*/
-}stLocalSFDeleteRequest, *pstLocalSFDeleteRequest;
+struct bcm_add_indication {
+	B_UINT8 u8Type;	/* < Type */
+	B_UINT8 eConnectionDir;	/* < Connection Direction */
+	/* brief 16 bit TID */
+	B_UINT16 u16TID; /* < TID */
+	/* brief 16bitCID */
+	B_UINT16 u16CID; /* < 16bitCID */
+	/* brief 16bitVCID */
+	B_UINT16 u16VCID; /* < 16bitVCID */
+	struct bcm_connect_mgr_params *psfAuthorizedSet; /* Authorized set of connection manager parameters */
+	struct bcm_connect_mgr_params *psfAdmittedSet; /* Admitted set of connection manager parameters */
+	struct bcm_connect_mgr_params *psfActiveSet; /* Activeset of connection manager parameters */
+	B_UINT8 u8CC; /* <Confirmation Code */
+	B_UINT8 u8Padd; /* < 8-bit Padding */
+	B_UINT16 u16Padd; /* < 16 bit Padding */
+};
 
-/**
-structure stLocalSFDeleteIndication
-*/
-typedef struct stLocalSFDeleteIndication{
-	B_UINT8                         u8Type;	/**< Type */
-	B_UINT8                         u8Padding;	/**< Padding  */
-	B_UINT16			u16TID;			/**< TID */
-       /// \brief 16bitCID
-    B_UINT16                        u16CID;			/**< CID */
-    /// \brief 16bitVCID
-    B_UINT16                        u16VCID;		/**< VCID */
-    /// \brief 32bitSFID
-    B_UINT32                        u32SFID; 		/**< SFID */
-	/// \brief 8bit Confirmation code
-	B_UINT8                         u8ConfirmationCode;	/**< Confirmation code */
-	B_UINT8                         u8Padding1[3];		/**< 3 byte Padding  */
-}stLocalSFDeleteIndication;
+struct bcm_del_request {
+	B_UINT8 u8Type; /* < Type */
+	B_UINT8 u8Padding; /* < Padding byte */
+	B_UINT16 u16TID; /* < TID */
+	/* brief 32bitSFID */
+	B_UINT32 u32SFID; /* < SFID */
+};
 
-typedef struct _stIM_SFHostNotify
-{
-	B_UINT32 	SFID;      //SFID of the service flow
-	B_UINT16 	newCID;   //the new/changed CID
-	B_UINT16 	VCID;             //Get new Vcid if the flow has been made active in CID update TLV, but was inactive earlier or the orig vcid
-	B_UINT8  	RetainSF;        //Indication to Host if the SF is to be retained or deleted; if TRUE-retain else delete
-	B_UINT8 	QoSParamSet; //QoS paramset of the retained SF
-	B_UINT16 	u16reserved;  //For byte alignment
+struct bcm_del_indication {
+	B_UINT8 u8Type;	/* < Type */
+	B_UINT8 u8Padding; /* < Padding */
+	B_UINT16 u16TID; /* < TID */
+	/* brief 16bitCID */
+	B_UINT16 u16CID; /* < CID */
+	/* brief 16bitVCID */
+	B_UINT16 u16VCID; /* < VCID */
+	/* brief 32bitSFID */
+	B_UINT32 u32SFID; /* < SFID */
+	/* brief 8bit Confirmation code */
+	B_UINT8 u8ConfirmationCode; /* < Confirmation code */
+	B_UINT8 u8Padding1[3]; /* < 3 byte Padding */
+};
 
-} stIM_SFHostNotify;
+struct bcm_stim_sfhostnotify {
+	B_UINT32 SFID; /* SFID of the service flow */
+	B_UINT16 newCID; /* the new/changed CID */
+	B_UINT16 VCID; /* Get new Vcid if the flow has been made active in CID update TLV, but was inactive earlier or the orig vcid */
+	B_UINT8 RetainSF; /* Indication to Host if the SF is to be retained or deleted; if TRUE-retain else delete */
+	B_UINT8 QoSParamSet; /* QoS paramset of the retained SF */
+	B_UINT16 u16reserved; /* For byte alignment */
+};
 
 #endif
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
index 08d13a4dfd70..10361bb35059 100644
--- a/drivers/staging/bcm/hostmibs.c
+++ b/drivers/staging/bcm/hostmibs.c
@@ -101,7 +101,7 @@ VOID GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *pstHostMibs, struct bcm_t
 	       sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
 }
 
-VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter, CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex)
+VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter, struct bcm_connect_mgr_params *psfLocalSet, UINT uiSearchRuleIndex)
 {
 	S_MIBS_EXTSERVICEFLOW_PARAMETERS *t = &Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
 
diff --git a/drivers/staging/bcm/nvm.c b/drivers/staging/bcm/nvm.c
index b179dbab93b5..b034eb5fa6b1 100644
--- a/drivers/staging/bcm/nvm.c
+++ b/drivers/staging/bcm/nvm.c
@@ -577,7 +577,7 @@ static int FlashSectorErase(struct bcm_mini_adapter *Adapter,
 			 * the sector erase cycle is 500 ms to 40000 msec. hence sleeping 10 ms
 			 * won't hamper performance in any case.
 			 */
-			udelay(10000);
+			mdelay(10);
 		} while ((uiStatus & 0x1) && (iRetries < 400));
 
 		if (uiStatus & 0x1) {
@@ -3932,7 +3932,7 @@ int validateFlash2xReadWrite(struct bcm_mini_adapter *Adapter, PFLASH2X_READWRIT
 				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2_PART3);
 		}
 
-		/* since this uiSectEndoffset is the size of iso Image. hence for calculating the vitual endoffset
+		/* since this uiSectEndoffset is the size of iso Image. hence for calculating the virtual endoffset
 		 * it should be added in startoffset. so that check done in last of this function can be valued.
 		 */
 		uiSectEndOffset = uiSectStartOffset + uiSectEndOffset;
diff --git a/drivers/staging/bcm/target_params.h b/drivers/staging/bcm/target_params.h
index 14876388b879..ad7ec0054938 100644
--- a/drivers/staging/bcm/target_params.h
+++ b/drivers/staging/bcm/target_params.h
@@ -32,7 +32,7 @@ typedef struct _TARGET_PARAMS
       B_UINT32 m_u32PowerSavingModesEnable; //bit 1: 1 Idlemode enable; bit2: 1 Sleepmode Enable
 	  /* PowerSaving Mode Options:
 	     bit 0 = 1: CPE mode - to keep pcmcia if alive;
-	     bit 1 = 1: CINR reporing in Idlemode Msg
+	     bit 1 = 1: CINR reporting in Idlemode Msg
 	     bit 2 = 1: Default PSC Enable in sleepmode*/
       B_UINT32 m_u32PowerSavingModeOptions;
 
diff --git a/drivers/staging/ccg/Kconfig b/drivers/staging/ccg/Kconfig
index df166652599f..8997a8c757aa 100644
--- a/drivers/staging/ccg/Kconfig
+++ b/drivers/staging/ccg/Kconfig
@@ -2,7 +2,7 @@ if USB_GADGET
 
 config USB_G_CCG
 	tristate "Configurable Composite Gadget (STAGING)"
-	depends on STAGING && BLOCK && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM
+	depends on STAGING && BLOCK && NET && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM
 	help
 	  The Configurable Composite Gadget supports multiple USB
 	  functions: acm, mass storage, rndis and FunctionFS.
diff --git a/drivers/staging/ccg/ccg.c b/drivers/staging/ccg/ccg.c
index 80feb95e27dc..93e1e2ffca0c 100644
--- a/drivers/staging/ccg/ccg.c
+++ b/drivers/staging/ccg/ccg.c
@@ -728,7 +728,7 @@ static int mass_storage_function_init(struct ccg_usb_function *f,
 	struct fsg_common *common;
 	int err;
 
-	memset(&fsg, 0, sizeof fsg);
+	memset(&fsg, 0, sizeof(fsg));
 	fsg.nluns = 1;
 	fsg.luns[0].removable = 1;
 	fsg.vendor_name = iManufacturer;
@@ -1101,13 +1101,7 @@ static struct device_attribute *ccg_usb_attributes[] = {
 static int ccg_bind_config(struct usb_configuration *c)
 {
 	struct ccg_dev *dev = _ccg_dev;
-	int ret = 0;
-
-	ret = ccg_bind_enabled_functions(dev, c);
-	if (ret)
-		return ret;
-
-	return 0;
+	return ccg_bind_enabled_functions(dev, c);
 }
 
 static void ccg_unbind_config(struct usb_configuration *c)
@@ -1255,8 +1249,10 @@ static int __init init(void)
 		return PTR_ERR(ccg_class);
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
+	if (!dev) {
+		class_destroy(ccg_class);
 		return -ENOMEM;
+	}
 
 	dev->functions = supported_functions;
 	INIT_LIST_HEAD(&dev->enabled_functions);
diff --git a/drivers/staging/ced1401/Kconfig b/drivers/staging/ced1401/Kconfig
new file mode 100644
index 000000000000..ae36d1b2ba99
--- /dev/null
+++ b/drivers/staging/ced1401/Kconfig
@@ -0,0 +1,6 @@
+config CED1401
+	tristate "Cambridge Electronic Design 1401 USB support"
+	depends on USB
+	help
+	  This driver supports the Cambridge Electronic Design 1401 USB device
+	  (whatever that is.)
diff --git a/drivers/staging/ced1401/Makefile b/drivers/staging/ced1401/Makefile
new file mode 100644
index 000000000000..f0c114b2b4b9
--- /dev/null
+++ b/drivers/staging/ced1401/Makefile
@@ -0,0 +1,3 @@
+
+obj-$(CONFIG_CED1401)	:= cedusb.o
+cedusb-objs	:= usb1401.o ced_ioc.o
diff --git a/drivers/staging/ced1401/TODO b/drivers/staging/ced1401/TODO
new file mode 100644
index 000000000000..9fd5630bdf4d
--- /dev/null
+++ b/drivers/staging/ced1401/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- coding syle fixes
+	- build warning fixups
+	- ioctl auditing
+	- usb api auditing
+	- proper USB minor number (it's stomping on an existing one right now.)
+
+Please send patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org> and Cc:
+Alois Schlögl <alois.schloegl@ist.ac.at>
+
diff --git a/drivers/staging/ced1401/ced_ioc.c b/drivers/staging/ced1401/ced_ioc.c
new file mode 100644
index 000000000000..c9492edaaddb
--- /dev/null
+++ b/drivers/staging/ced1401/ced_ioc.c
@@ -0,0 +1,1515 @@
+/* ced_ioc.c
+ ioctl part of the 1401 usb device driver for linux.
+ Copyright (C) 2010 Cambridge Electronic Design Ltd
+ Author Greg P Smith (greg@ced.co.uk)
+
+ This program is free software; you can redistribute it and/or
+ modify it under the terms of the GNU General Public License
+ as published by the Free Software Foundation; either version 2
+ of the License, or (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/uaccess.h>
+#include <linux/usb.h>
+#include <linux/mutex.h>
+#include <linux/page-flags.h>
+#include <linux/pagemap.h>
+#include <linux/jiffies.h>
+
+#include "usb1401.h"
+
+/****************************************************************************
+** FlushOutBuff
+**
+** Empties the Output buffer and sets int lines. Used from user level only
+****************************************************************************/
+void FlushOutBuff(DEVICE_EXTENSION * pdx)
+{
+	dev_dbg(&pdx->interface->dev, "%s currentState=%d", __func__,
+		pdx->sCurrentState);
+	if (pdx->sCurrentState == U14ERR_TIME)	/* Do nothing if hardware in trouble */
+		return;
+//    CharSend_Cancel(pdx);                   /* Kill off any pending I/O */
+	spin_lock_irq(&pdx->charOutLock);
+	pdx->dwNumOutput = 0;
+	pdx->dwOutBuffGet = 0;
+	pdx->dwOutBuffPut = 0;
+	spin_unlock_irq(&pdx->charOutLock);
+}
+
+/****************************************************************************
+**
+** FlushInBuff
+**
+** Empties the input buffer and sets int lines
+****************************************************************************/
+void FlushInBuff(DEVICE_EXTENSION * pdx)
+{
+	dev_dbg(&pdx->interface->dev, "%s currentState=%d", __func__,
+		pdx->sCurrentState);
+	if (pdx->sCurrentState == U14ERR_TIME)	/* Do nothing if hardware in trouble */
+		return;
+//    CharRead_Cancel(pDevObject);            /* Kill off any pending I/O */
+	spin_lock_irq(&pdx->charInLock);
+	pdx->dwNumInput = 0;
+	pdx->dwInBuffGet = 0;
+	pdx->dwInBuffPut = 0;
+	spin_unlock_irq(&pdx->charInLock);
+}
+
+/****************************************************************************
+** PutChars
+**
+** Utility routine to copy chars into the output buffer and fire them off.
+** called from user mode, holds charOutLock.
+****************************************************************************/
+static int PutChars(DEVICE_EXTENSION * pdx, const char *pCh,
+		    unsigned int uCount)
+{
+	int iReturn;
+	spin_lock_irq(&pdx->charOutLock);	// get the output spin lock
+	if ((OUTBUF_SZ - pdx->dwNumOutput) >= uCount) {
+		unsigned int u;
+		for (u = 0; u < uCount; u++) {
+			pdx->outputBuffer[pdx->dwOutBuffPut++] = pCh[u];
+			if (pdx->dwOutBuffPut >= OUTBUF_SZ)
+				pdx->dwOutBuffPut = 0;
+		}
+		pdx->dwNumOutput += uCount;
+		spin_unlock_irq(&pdx->charOutLock);
+		iReturn = SendChars(pdx);	// ...give a chance to transmit data
+	} else {
+		iReturn = U14ERR_NOOUT;	// no room at the out (ha-ha)
+		spin_unlock_irq(&pdx->charOutLock);
+	}
+	return iReturn;
+}
+
+/*****************************************************************************
+** Add the data in pData (local pointer) of length n to the output buffer, and
+** trigger an output transfer if this is appropriate. User mode.
+** Holds the io_mutex
+*****************************************************************************/
+int SendString(DEVICE_EXTENSION * pdx, const char __user * pData,
+	       unsigned int n)
+{
+	int iReturn = U14ERR_NOERROR;	// assume all will be well
+	char buffer[OUTBUF_SZ + 1];	// space in our address space for characters
+	if (n > OUTBUF_SZ)	// check space in local buffer...
+		return U14ERR_NOOUT;	// ...too many characters
+	if (copy_from_user(buffer, pData, n))
+		return -EFAULT;
+	buffer[n] = 0;		// terminate for debug purposes
+
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	if (n > 0)		// do nothing if nowt to do!
+	{
+		dev_dbg(&pdx->interface->dev, "%s n=%d>%s<", __func__, n,
+			buffer);
+		iReturn = PutChars(pdx, buffer, n);
+	}
+
+	Allowi(pdx, false);	// make sure we have input int
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** SendChar
+**
+** Sends a single character to the 1401. User mode, holds io_mutex.
+****************************************************************************/
+int SendChar(DEVICE_EXTENSION * pdx, char c)
+{
+	int iReturn;
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	iReturn = PutChars(pdx, &c, 1);
+	dev_dbg(&pdx->interface->dev, "SendChar >%c< (0x%02x)", c, c);
+	Allowi(pdx, false);	// Make sure char reads are running
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/***************************************************************************
+**
+** Get1401State
+**
+**  Retrieves state information from the 1401, adjusts the 1401 state held
+**  in the device extension to indicate the current 1401 type.
+**
+**  *state is updated with information about the 1401 state as returned by the
+**         1401. The low byte is a code for what 1401 is doing:
+**
+**  0       normal 1401 operation
+**  1       sending chars to host
+**  2       sending block data to host
+**  3       reading block data from host
+**  4       sending an escape sequence to the host
+**  0x80    1401 is executing self-test, in which case the upper word
+**          is the last error code seen (or zero for no new error).
+**
+** *error is updated with error information if a self-test error code
+**          is returned in the upper word of state.
+**
+**  both state and error are set to -1 if there are comms problems, and
+**  to zero if there is a simple failure.
+**
+** return error code (U14ERR_NOERROR for OK)
+*/
+int Get1401State(DEVICE_EXTENSION * pdx, __u32 * state, __u32 * error)
+{
+	int nGot;
+	dev_dbg(&pdx->interface->dev, "Get1401State() entry");
+
+	*state = 0xFFFFFFFF;	// Start off with invalid state
+	nGot = usb_control_msg(pdx->udev, usb_rcvctrlpipe(pdx->udev, 0),
+			       GET_STATUS, (D_TO_H | VENDOR | DEVREQ), 0, 0,
+			       pdx->statBuf, sizeof(pdx->statBuf), HZ);
+	if (nGot != sizeof(pdx->statBuf)) {
+		dev_err(&pdx->interface->dev,
+			"Get1401State() FAILED, return code %d", nGot);
+		pdx->sCurrentState = U14ERR_TIME;	// Indicate that things are very wrong indeed
+		*state = 0;	// Force status values to a known state
+		*error = 0;
+	} else {
+		int nDevice;
+		dev_dbg(&pdx->interface->dev,
+			"Get1401State() Success, state: 0x%x, 0x%x",
+			pdx->statBuf[0], pdx->statBuf[1]);
+
+		*state = pdx->statBuf[0];	// Return the state values to the calling code
+		*error = pdx->statBuf[1];
+
+		nDevice = pdx->udev->descriptor.bcdDevice >> 8;	// 1401 type code value
+		switch (nDevice)	// so we can clean up current state
+		{
+		case 0:
+			pdx->sCurrentState = U14ERR_U1401;
+			break;
+
+		default:	// allow lots of device codes for future 1401s
+			if ((nDevice >= 1) && (nDevice <= 23))
+				pdx->sCurrentState = (short)(nDevice + 6);
+			else
+				pdx->sCurrentState = U14ERR_ILL;
+			break;
+		}
+	}
+
+	return pdx->sCurrentState >= 0 ? U14ERR_NOERROR : pdx->sCurrentState;
+}
+
+/****************************************************************************
+** ReadWrite_Cancel
+**
+** Kills off staged read\write request from the USB if one is pending.
+****************************************************************************/
+int ReadWrite_Cancel(DEVICE_EXTENSION * pdx)
+{
+	dev_dbg(&pdx->interface->dev, "ReadWrite_Cancel entry %d",
+		pdx->bStagedUrbPending);
+#ifdef NOT_WRITTEN_YET
+	int ntStatus = STATUS_SUCCESS;
+	bool bResult = false;
+	unsigned int i;
+	// We can fill this in when we know how we will implement the staged transfer stuff
+	spin_lock_irq(&pdx->stagedLock);
+
+	if (pdx->bStagedUrbPending)	// anything to be cancelled? May need more...
+	{
+		dev_info(&pdx->interface - dev,
+			 "ReadWrite_Cancel about to cancel Urb");
+
+		//       KeClearEvent(&pdx->StagingDoneEvent);   // Clear the staging done flag
+		USB_ASSERT(pdx->pStagedIrp != NULL);
+
+		// Release the spinlock first otherwise the completion routine may hang
+		//  on the spinlock while this function hands waiting for the event.
+		spin_unlock_irq(&pdx->stagedLock);
+		bResult = IoCancelIrp(pdx->pStagedIrp);	// Actually do the cancel
+		if (bResult) {
+			LARGE_INTEGER timeout;
+			timeout.QuadPart = -10000000;	// Use a timeout of 1 second
+			dev_info(&pdx->interface - dev,
+				 "ReadWrite_Cancel about to wait till done");
+			ntStatus =
+			    KeWaitForSingleObject(&pdx->StagingDoneEvent,
+						  Executive, KernelMode, FALSE,
+						  &timeout);
+		} else {
+			dev_info(&pdx->interface - dev,
+				 "ReadWrite_Cancel, cancellation failed");
+			ntStatus = U14ERR_FAIL;
+		}
+		USB_KdPrint(DBGLVL_DEFAULT,
+			    ("ReadWrite_Cancel ntStatus = 0x%x decimal %d\n",
+			     ntStatus, ntStatus));
+	} else
+		spin_unlock_irq(&pdx->stagedLock);
+
+	dev_info(&pdx->interface - dev, "ReadWrite_Cancel  done");
+	return ntStatus;
+#else
+	return U14ERR_NOERROR;
+#endif
+
+}
+
+/***************************************************************************
+** InSelfTest - utility to check in self test. Return 1 for ST, 0 for not or
+** a -ve error code if we failed for some reason.
+***************************************************************************/
+static int InSelfTest(DEVICE_EXTENSION * pdx, unsigned int *pState)
+{
+	unsigned int state, error;
+	int iReturn = Get1401State(pdx, &state, &error);	// see if in self-test
+	if (iReturn == U14ERR_NOERROR)	// if all still OK
+		iReturn = (state == (unsigned int)-1) ||	// TX problem or...
+		    ((state & 0xff) == 0x80);	// ...self test
+	*pState = state;	// return actual state
+	return iReturn;
+}
+
+/***************************************************************************
+** Is1401 - ALWAYS CALLED HOLDING THE io_mutex
+**
+** Tests for the current state of the 1401. Sets sCurrentState:
+**
+**  U14ERR_NOIF  1401  i/f card not installed (not done here)
+**  U14ERR_OFF   1401  apparently not switched on
+**  U14ERR_NC    1401  appears to be not connected
+**  U14ERR_ILL   1401  if it is there its not very well at all
+**  U14ERR_TIME  1401  appears OK, but doesn't communicate - very bad
+**  U14ERR_STD   1401  OK and ready for use
+**  U14ERR_PLUS  1401+ OK and ready for use
+**  U14ERR_U1401 Micro1401 OK and ready for use
+**  U14ERR_POWER Power1401 OK and ready for use
+**  U14ERR_U14012 Micro1401 mkII OK and ready for use
+**
+**  Returns TRUE if a 1401 detected and OK, else FALSE
+****************************************************************************/
+bool Is1401(DEVICE_EXTENSION * pdx)
+{
+	int iReturn;
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	ced_draw_down(pdx);	// wait for, then kill outstanding Urbs
+	FlushInBuff(pdx);	// Clear out input buffer & pipe
+	FlushOutBuff(pdx);	// Clear output buffer & pipe
+
+	// The next call returns 0 if OK, but has returned 1 in the past, meaning that
+	// usb_unlock_device() is needed... now it always is
+	iReturn = usb_lock_device_for_reset(pdx->udev, pdx->interface);
+
+	// release the io_mutex because if we don't, we will deadlock due to system
+	// calls back into the driver.
+	mutex_unlock(&pdx->io_mutex);	// locked, so we will not get system calls
+	if (iReturn >= 0)	// if we failed
+	{
+		iReturn = usb_reset_device(pdx->udev);	// try to do the reset
+		usb_unlock_device(pdx->udev);	// undo the lock
+	}
+
+	mutex_lock(&pdx->io_mutex);	// hold stuff off while we wait
+	pdx->dwDMAFlag = MODE_CHAR;	// Clear DMA mode flag regardless!
+	if (iReturn == 0)	// if all is OK still
+	{
+		unsigned int state;
+		iReturn = InSelfTest(pdx, &state);	// see if likely in self test
+		if (iReturn > 0)	// do we need to wait for self-test?
+		{
+			unsigned long ulTimeOut = jiffies + 30 * HZ;	// when to give up
+			while ((iReturn > 0) && time_before(jiffies, ulTimeOut)) {
+				schedule();	// let other stuff run
+				iReturn = InSelfTest(pdx, &state);	// see if done yet
+			}
+		}
+
+		if (iReturn == 0)	// if all is OK...
+			iReturn = state == 0;	// then sucess is that the state is 0
+	} else
+		iReturn = 0;	// we failed
+	pdx->bForceReset = false;	// Clear forced reset flag now
+
+	return iReturn > 0;
+}
+
+/****************************************************************************
+** QuickCheck  - ALWAYS CALLED HOLDING THE io_mutex
+** This is used to test for a 1401. It will try to do a quick check if all is
+**  OK, that is the 1401 was OK the last time it was asked, and there is no DMA
+**  in progress, and if the bTestBuff flag is set, the character buffers must be
+**  empty too. If the quick check shows that the state is still the same, then
+**  all is OK.
+**
+** If any of the above conditions are not met, or if the state or type of the
+**  1401 has changed since the previous test, the full Is1401 test is done, but
+**  only if bCanReset is also TRUE.
+**
+** The return value is TRUE if a useable 1401 is found, FALSE if not
+*/
+bool QuickCheck(DEVICE_EXTENSION * pdx, bool bTestBuff, bool bCanReset)
+{
+	bool bRet = false;	// assume it will fail and we will reset
+	bool bShortTest;
+
+	bShortTest = ((pdx->dwDMAFlag == MODE_CHAR) &&	// no DMA running
+		      (!pdx->bForceReset) &&	// Not had a real reset forced
+		      (pdx->sCurrentState >= U14ERR_STD));	// No 1401 errors stored
+
+	dev_dbg(&pdx->interface->dev,
+		"%s DMAFlag:%d, state:%d, force:%d, testBuff:%d, short:%d",
+		__func__, pdx->dwDMAFlag, pdx->sCurrentState, pdx->bForceReset,
+		bTestBuff, bShortTest);
+
+	if ((bTestBuff) &&	// Buffer check requested, and...
+	    (pdx->dwNumInput || pdx->dwNumOutput))	// ...characters were in the buffer?
+	{
+		bShortTest = false;	// Then do the full test
+		dev_dbg(&pdx->interface->dev,
+			"%s will reset as buffers not empty", __func__);
+	}
+
+	if (bShortTest || !bCanReset)	// Still OK to try the short test?
+	{			// Always test if no reset - we want state update
+		unsigned int state, error;
+		dev_dbg(&pdx->interface->dev, "%s->Get1401State", __func__);
+		if (Get1401State(pdx, &state, &error) == U14ERR_NOERROR)	// Check on the 1401 state
+		{
+			if ((state & 0xFF) == 0)	// If call worked, check the status value
+				bRet = true;	// If that was zero, all is OK, no reset needed
+		}
+	}
+
+	if (!bRet && bCanReset)	// If all not OK, then
+	{
+		dev_info(&pdx->interface->dev, "%s->Is1401 %d %d %d %d",
+			 __func__, bShortTest, pdx->sCurrentState, bTestBuff,
+			 pdx->bForceReset);
+		bRet = Is1401(pdx);	//  do full test
+	}
+
+	return bRet;
+}
+
+/****************************************************************************
+** Reset1401
+**
+** Resets the 1401 and empties the i/o buffers
+*****************************************************************************/
+int Reset1401(DEVICE_EXTENSION * pdx)
+{
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	dev_dbg(&pdx->interface->dev, "ABout to call QuickCheck");
+	QuickCheck(pdx, true, true);	// Check 1401, reset if not OK
+	mutex_unlock(&pdx->io_mutex);
+	return U14ERR_NOERROR;
+}
+
+/****************************************************************************
+** GetChar
+**
+** Gets a single character from the 1401
+****************************************************************************/
+int GetChar(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = U14ERR_NOIN;	// assume we will get  nothing
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+
+	dev_dbg(&pdx->interface->dev, "GetChar");
+
+	Allowi(pdx, false);	// Make sure char reads are running
+	SendChars(pdx);		// and send any buffered chars
+
+	spin_lock_irq(&pdx->charInLock);
+	if (pdx->dwNumInput > 0)	// worth looking
+	{
+		iReturn = pdx->inputBuffer[pdx->dwInBuffGet++];
+		if (pdx->dwInBuffGet >= INBUF_SZ)
+			pdx->dwInBuffGet = 0;
+		pdx->dwNumInput--;
+	} else
+		iReturn = U14ERR_NOIN;	// no input data to read
+	spin_unlock_irq(&pdx->charInLock);
+
+	Allowi(pdx, false);	// Make sure char reads are running
+
+	mutex_unlock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	return iReturn;
+}
+
+/****************************************************************************
+** GetString
+**
+** Gets a string from the 1401. Returns chars up to the next CR or when
+** there are no more to read or nowhere to put them. CR is translated to
+** 0 and counted as a character. If the string does not end in a 0, we will
+** add one, if there is room, but it is not counted as a character.
+**
+** returns the count of characters (including the terminator, or 0 if none
+** or a negative error code.
+****************************************************************************/
+int GetString(DEVICE_EXTENSION * pdx, char __user * pUser, int n)
+{
+	int nAvailable;		// character in the buffer
+	int iReturn = U14ERR_NOIN;
+	if (n <= 0)
+		return -ENOMEM;
+
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	Allowi(pdx, false);	// Make sure char reads are running
+	SendChars(pdx);		// and send any buffered chars
+
+	spin_lock_irq(&pdx->charInLock);
+	nAvailable = pdx->dwNumInput;	// characters available now
+	if (nAvailable > n)	// read max of space in pUser...
+		nAvailable = n;	// ...or input characters
+
+	if (nAvailable > 0)	// worth looking?
+	{
+		char buffer[INBUF_SZ + 1];	// space for a linear copy of data
+		int nGot = 0;
+		int nCopyToUser;	// number to copy to user
+		char cData;
+		do {
+			cData = pdx->inputBuffer[pdx->dwInBuffGet++];
+			if (cData == CR_CHAR)	// replace CR with zero
+				cData = (char)0;
+
+			if (pdx->dwInBuffGet >= INBUF_SZ)
+				pdx->dwInBuffGet = 0;	// wrap buffer pointer
+
+			buffer[nGot++] = cData;	// save the output
+		}
+		while ((nGot < nAvailable) && cData);
+
+		nCopyToUser = nGot;	// what to copy...
+		if (cData)	// do we need null
+		{
+			buffer[nGot] = (char)0;	// make it tidy
+			if (nGot < n)	// if space in user buffer...
+				++nCopyToUser;	// ...copy the 0 as well.
+		}
+
+		pdx->dwNumInput -= nGot;
+		spin_unlock_irq(&pdx->charInLock);
+
+		dev_dbg(&pdx->interface->dev,
+			"GetString read %d characters >%s<", nGot, buffer);
+		if (copy_to_user(pUser, buffer, nCopyToUser))
+			iReturn = -EFAULT;
+		else
+			iReturn = nGot;		// report characters read
+	} else
+		spin_unlock_irq(&pdx->charInLock);
+
+	Allowi(pdx, false);	// Make sure char reads are running
+	mutex_unlock(&pdx->io_mutex);	// Protect disconnect from new i/o
+
+	return iReturn;
+}
+
+/*******************************************************************************
+** Get count of characters in the inout buffer.
+*******************************************************************************/
+int Stat1401(DEVICE_EXTENSION * pdx)
+{
+	int iReturn;
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	Allowi(pdx, false);	// make sure we allow pending chars
+	SendChars(pdx);		// in both directions
+	iReturn = pdx->dwNumInput;	// no lock as single read
+	mutex_unlock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	return iReturn;
+}
+
+/****************************************************************************
+** LineCount
+**
+** Returns the number of newline chars in the buffer. There is no need for
+** any fancy interlocks as we only read the interrupt routine data, and the
+** system is arranged so nothing can be destroyed.
+****************************************************************************/
+int LineCount(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = 0;	// will be count of line ends
+
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	Allowi(pdx, false);	// Make sure char reads are running
+	SendChars(pdx);		// and send any buffered chars
+	spin_lock_irq(&pdx->charInLock);	// Get protection
+
+	if (pdx->dwNumInput > 0)	// worth looking?
+	{
+		unsigned int dwIndex = pdx->dwInBuffGet;	// start at first available
+		unsigned int dwEnd = pdx->dwInBuffPut;	// Position for search end
+		do {
+			if (pdx->inputBuffer[dwIndex++] == CR_CHAR)
+				++iReturn;	// inc count if CR
+
+			if (dwIndex >= INBUF_SZ)	// see if we fall off buff
+				dwIndex = 0;
+		}
+		while (dwIndex != dwEnd);	// go to last avaliable
+	}
+
+	spin_unlock_irq(&pdx->charInLock);
+	dev_dbg(&pdx->interface->dev, "LineCount returned %d", iReturn);
+	mutex_unlock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	return iReturn;
+}
+
+/****************************************************************************
+** GetOutBufSpace
+**
+** Gets the space in the output buffer. Called from user code.
+*****************************************************************************/
+int GetOutBufSpace(DEVICE_EXTENSION * pdx)
+{
+	int iReturn;
+	mutex_lock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	SendChars(pdx);		// send any buffered chars
+	iReturn = (int)(OUTBUF_SZ - pdx->dwNumOutput);	// no lock needed for single read
+	dev_dbg(&pdx->interface->dev, "OutBufSpace %d", iReturn);
+	mutex_unlock(&pdx->io_mutex);	// Protect disconnect from new i/o
+	return iReturn;
+}
+
+/****************************************************************************
+**
+** ClearArea
+**
+** Clears up a transfer area. This is always called in the context of a user
+** request, never from a call-back.
+****************************************************************************/
+int ClearArea(DEVICE_EXTENSION * pdx, int nArea)
+{
+	int iReturn = U14ERR_NOERROR;
+
+	if ((nArea < 0) || (nArea >= MAX_TRANSAREAS)) {
+		iReturn = U14ERR_BADAREA;
+		dev_err(&pdx->interface->dev, "%s Attempt to clear area %d",
+			__func__, nArea);
+	} else {
+		TRANSAREA *pTA = &pdx->rTransDef[nArea];	// to save typing
+		if (!pTA->bUsed)	// if not used...
+			iReturn = U14ERR_NOTSET;	// ...nothing to be done
+		else {
+			// We must save the memory we return as we shouldn't mess with memory while
+			// holding a spin lock.
+			struct page **pPages = 0;	// save page address list
+			int nPages = 0;	// and number of pages
+			int np;
+
+			dev_dbg(&pdx->interface->dev, "%s area %d", __func__,
+				nArea);
+			spin_lock_irq(&pdx->stagedLock);
+			if ((pdx->StagedId == nArea)
+			    && (pdx->dwDMAFlag > MODE_CHAR)) {
+				iReturn = U14ERR_UNLOCKFAIL;	// cannot delete as in use
+				dev_err(&pdx->interface->dev,
+					"%s call on area %d while active",
+					__func__, nArea);
+			} else {
+				pPages = pTA->pPages;	// save page address list
+				nPages = pTA->nPages;	// and page count
+				if (pTA->dwEventSz)	// if events flagging in use
+					wake_up_interruptible(&pTA->wqEvent);	// release anything that was waiting
+
+				if (pdx->bXFerWaiting
+				    && (pdx->rDMAInfo.wIdent == nArea))
+					pdx->bXFerWaiting = false;	// Cannot have pending xfer if area cleared
+
+				// Clean out the TRANSAREA except for the wait queue, which is at the end
+				// This sets bUsed to false and dwEventSz to 0 to say area not used and no events.
+				memset(pTA, 0,
+				       sizeof(TRANSAREA) -
+				       sizeof(wait_queue_head_t));
+			}
+			spin_unlock_irq(&pdx->stagedLock);
+
+			if (pPages)	// if we decided to release the memory
+			{
+				// Now we must undo the pinning down of the pages. We will assume the worst and mark
+				// all the pages as dirty. Don't be tempted to move this up above as you must not be
+				// holding a spin lock to do this stuff as it is not atomic.
+				dev_dbg(&pdx->interface->dev, "%s nPages=%d",
+					__func__, nPages);
+
+				for (np = 0; np < nPages; ++np) {
+					if (pPages[np]) {
+						SetPageDirty(pPages[np]);
+						page_cache_release(pPages[np]);
+					}
+				}
+
+				kfree(pPages);
+				dev_dbg(&pdx->interface->dev,
+					"%s kfree(pPages) done", __func__);
+			}
+		}
+	}
+
+	return iReturn;
+}
+
+/****************************************************************************
+** SetArea
+**
+** Sets up a transfer area - the functional part. Called by both
+** SetTransfer and SetCircular.
+****************************************************************************/
+static int SetArea(DEVICE_EXTENSION * pdx, int nArea, char __user * puBuf,
+		   unsigned int dwLength, bool bCircular, bool bCircToHost)
+{
+	// Start by working out the page aligned start of the area and the size
+	// of the area in pages, allowing for the start not being aligned and the
+	// end needing to be rounded up to a page boundary.
+	unsigned long ulStart = ((unsigned long)puBuf) & PAGE_MASK;
+	unsigned int ulOffset = ((unsigned long)puBuf) & (PAGE_SIZE - 1);
+	int len = (dwLength + ulOffset + PAGE_SIZE - 1) >> PAGE_SHIFT;
+
+	TRANSAREA *pTA = &pdx->rTransDef[nArea];	// to save typing
+	struct page **pPages = 0;	// space for page tables
+	int nPages = 0;		// and number of pages
+
+	int iReturn = ClearArea(pdx, nArea);	// see if OK to use this area
+	if ((iReturn != U14ERR_NOTSET) &&	// if not area unused and...
+	    (iReturn != U14ERR_NOERROR))	// ...not all OK, then...
+		return iReturn;	// ...we cannot use this area
+
+	if (!access_ok(VERIFY_WRITE, puBuf, dwLength))	// if we cannot access the memory...
+		return -EFAULT;	// ...then we are done
+
+	// Now allocate space to hold the page pointer and virtual address pointer tables
+	pPages =
+	    (struct page **)kmalloc(len * sizeof(struct page *), GFP_KERNEL);
+	if (!pPages) {
+		iReturn = U14ERR_NOMEMORY;
+		goto error;
+	}
+	dev_dbg(&pdx->interface->dev, "%s %p, length=%06x, circular %d",
+		__func__, puBuf, dwLength, bCircular);
+
+	// To pin down user pages we must first acquire the mapping semaphore.
+	down_read(&current->mm->mmap_sem);	// get memory map semaphore
+	nPages =
+	    get_user_pages(current, current->mm, ulStart, len, 1, 0, pPages, 0);
+	up_read(&current->mm->mmap_sem);	// release the semaphore
+	dev_dbg(&pdx->interface->dev, "%s nPages = %d", __func__, nPages);
+
+	if (nPages > 0)		// if we succeeded
+	{
+		// If you are tempted to use page_address (form LDD3), forget it. You MUST use
+		// kmap() or kmap_atomic() to get a virtual address. page_address will give you
+		// (null) or at least it does in this context with an x86 machine.
+		spin_lock_irq(&pdx->stagedLock);
+		pTA->lpvBuff = puBuf;	// keep start of region (user address)
+		pTA->dwBaseOffset = ulOffset;	// save offset in first page to start of xfer
+		pTA->dwLength = dwLength;	// Size if the region in bytes
+		pTA->pPages = pPages;	// list of pages that are used by buffer
+		pTA->nPages = nPages;	// number of pages
+
+		pTA->bCircular = bCircular;
+		pTA->bCircToHost = bCircToHost;
+
+		pTA->aBlocks[0].dwOffset = 0;
+		pTA->aBlocks[0].dwSize = 0;
+		pTA->aBlocks[1].dwOffset = 0;
+		pTA->aBlocks[1].dwSize = 0;
+		pTA->bUsed = true;	// This is now a used block
+
+		spin_unlock_irq(&pdx->stagedLock);
+		iReturn = U14ERR_NOERROR;	// say all was well
+	} else {
+		iReturn = U14ERR_LOCKFAIL;
+		goto error;
+	}
+
+	return iReturn;
+
+error:
+	kfree(pPages);
+	return iReturn;
+}
+
+/****************************************************************************
+** SetTransfer
+**
+** Sets up a transfer area record. If the area is already set, we attempt to
+** unset it. Unsetting will fail if the area is booked, and a transfer to that
+** area is in progress. Otherwise, we will release the area and re-assign it.
+****************************************************************************/
+int SetTransfer(DEVICE_EXTENSION * pdx, TRANSFERDESC __user * pTD)
+{
+	int iReturn;
+	TRANSFERDESC td;
+
+	if (copy_from_user(&td, pTD, sizeof(td)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s area:%d, size:%08x", __func__,
+		td.wAreaNum, td.dwLength);
+	// The strange cast is done so that we don't get warnings in 32-bit linux about the size of the
+	// pointer. The pointer is always passed as a 64-bit object so that we don't have problems using
+	// a 32-bit program on a 64-bit system. unsigned long is 64-bits on a 64-bit system.
+	iReturn =
+	    SetArea(pdx, td.wAreaNum,
+		    (char __user *)((unsigned long)td.lpvBuff), td.dwLength,
+		    false, false);
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/****************************************************************************
+** UnSetTransfer
+** Erases a transfer area record
+****************************************************************************/
+int UnsetTransfer(DEVICE_EXTENSION * pdx, int nArea)
+{
+	int iReturn;
+	mutex_lock(&pdx->io_mutex);
+	iReturn = ClearArea(pdx, nArea);
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/****************************************************************************
+** SetEvent
+** Creates an event that we can test for based on a transfer to/from an area.
+** The area must be setup for a transfer. We attempt to simulate the Windows
+** driver behavior for events (as we don't actually use them), which is to
+** pretend that whatever the user asked for was achieved, so we return 1 if
+** try to create one, and 0 if they ask to remove (assuming all else was OK).
+****************************************************************************/
+int SetEvent(DEVICE_EXTENSION * pdx, TRANSFEREVENT __user * pTE)
+{
+	int iReturn = U14ERR_NOERROR;
+	TRANSFEREVENT te;
+
+	// get a local copy of the data
+	if (copy_from_user(&te, pTE, sizeof(te)))
+		return -EFAULT;
+
+	if (te.wAreaNum >= MAX_TRANSAREAS)	// the area must exist
+		return U14ERR_BADAREA;
+	else {
+		TRANSAREA *pTA = &pdx->rTransDef[te.wAreaNum];
+		mutex_lock(&pdx->io_mutex);	// make sure we have no competitor
+		spin_lock_irq(&pdx->stagedLock);
+		if (pTA->bUsed)	// area must be in use
+		{
+			pTA->dwEventSt = te.dwStart;	// set area regions
+			pTA->dwEventSz = te.dwLength;	// set size (0 cancels it)
+			pTA->bEventToHost = te.wFlags & 1;	// set the direction
+			pTA->iWakeUp = 0;	// zero the wake up count
+		} else
+			iReturn = U14ERR_NOTSET;
+		spin_unlock_irq(&pdx->stagedLock);
+		mutex_unlock(&pdx->io_mutex);
+	}
+	return iReturn ==
+	    U14ERR_NOERROR ? (te.iSetEvent ? 1 : U14ERR_NOERROR) : iReturn;
+}
+
+/****************************************************************************
+** WaitEvent
+** Sleep the process with a timeout waiting for an event. Returns the number
+** of times that a block met the event condition since we last cleared it or
+** 0 if timed out, or -ve error (bad area or not set, or signal).
+****************************************************************************/
+int WaitEvent(DEVICE_EXTENSION * pdx, int nArea, int msTimeOut)
+{
+	int iReturn;
+	if ((unsigned)nArea >= MAX_TRANSAREAS)
+		return U14ERR_BADAREA;
+	else {
+		int iWait;
+		TRANSAREA *pTA = &pdx->rTransDef[nArea];
+		msTimeOut = (msTimeOut * HZ + 999) / 1000;	// convert timeout to jiffies
+
+		// We cannot wait holding the mutex, but we check the flags while holding
+		// it. This may well be pointless as another thread could get in between
+		// releasing it and the wait call. However, this would have to clear the
+		// iWakeUp flag. However, the !pTA-bUsed may help us in this case.
+		mutex_lock(&pdx->io_mutex);	// make sure we have no competitor
+		if (!pTA->bUsed || !pTA->dwEventSz)	// check something to wait for...
+			return U14ERR_NOTSET;	// ...else we do nothing
+		mutex_unlock(&pdx->io_mutex);
+
+		if (msTimeOut)
+			iWait =
+			    wait_event_interruptible_timeout(pTA->wqEvent,
+							     pTA->iWakeUp
+							     || !pTA->bUsed,
+							     msTimeOut);
+		else
+			iWait =
+			    wait_event_interruptible(pTA->wqEvent, pTA->iWakeUp
+						     || !pTA->bUsed);
+		if (iWait)
+			iReturn = -ERESTARTSYS;	// oops - we have had a SIGNAL
+		else
+			iReturn = pTA->iWakeUp;	// else the wakeup count
+
+		spin_lock_irq(&pdx->stagedLock);
+		pTA->iWakeUp = 0;	// clear the flag
+		spin_unlock_irq(&pdx->stagedLock);
+	}
+	return iReturn;
+}
+
+/****************************************************************************
+** TestEvent
+** Test the event to see if a WaitEvent would return immediately. Returns the
+** number of times a block completed since the last call, or 0 if none or a
+** negative error.
+****************************************************************************/
+int TestEvent(DEVICE_EXTENSION * pdx, int nArea)
+{
+	int iReturn;
+	if ((unsigned)nArea >= MAX_TRANSAREAS)
+		iReturn = U14ERR_BADAREA;
+	else {
+		TRANSAREA *pTA = &pdx->rTransDef[nArea];
+		mutex_lock(&pdx->io_mutex);	// make sure we have no competitor
+		spin_lock_irq(&pdx->stagedLock);
+		iReturn = pTA->iWakeUp;	// get wakeup count since last call
+		pTA->iWakeUp = 0;	// clear the count
+		spin_unlock_irq(&pdx->stagedLock);
+		mutex_unlock(&pdx->io_mutex);
+	}
+	return iReturn;
+}
+
+/****************************************************************************
+** GetTransferInfo
+** Puts the current state of the 1401 in a TGET_TX_BLOCK.
+*****************************************************************************/
+int GetTransfer(DEVICE_EXTENSION * pdx, TGET_TX_BLOCK __user * pTX)
+{
+	int iReturn = U14ERR_NOERROR;
+	unsigned int dwIdent;
+
+	mutex_lock(&pdx->io_mutex);
+	dwIdent = pdx->StagedId;	// area ident for last xfer
+	if (dwIdent >= MAX_TRANSAREAS)
+		iReturn = U14ERR_BADAREA;
+	else {
+		// Return the best information we have - we don't have physical addresses
+		TGET_TX_BLOCK tx;
+		memset(&tx, 0, sizeof(tx));	// clean out local work structure
+		tx.size = pdx->rTransDef[dwIdent].dwLength;
+		tx.linear = (long long)((long)pdx->rTransDef[dwIdent].lpvBuff);
+		tx.avail = GET_TX_MAXENTRIES;	// how many blocks we could return
+		tx.used = 1;	// number we actually return
+		tx.entries[0].physical =
+		    (long long)(tx.linear + pdx->StagedOffset);
+		tx.entries[0].size = tx.size;
+
+		if (copy_to_user(pTX, &tx, sizeof(tx)))
+			iReturn = -EFAULT;
+	}
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/****************************************************************************
+** KillIO1401
+**
+** Empties the host i/o buffers
+****************************************************************************/
+int KillIO1401(DEVICE_EXTENSION * pdx)
+{
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+	mutex_lock(&pdx->io_mutex);
+	FlushOutBuff(pdx);
+	FlushInBuff(pdx);
+	mutex_unlock(&pdx->io_mutex);
+	return U14ERR_NOERROR;
+}
+
+/****************************************************************************
+** BlkTransState
+** Returns a 0 or a 1 for whether DMA is happening. No point holding a mutex
+** for this as it only does one read.
+*****************************************************************************/
+int BlkTransState(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = pdx->dwDMAFlag != MODE_CHAR;
+	dev_dbg(&pdx->interface->dev, "%s = %d", __func__, iReturn);
+	return iReturn;
+}
+
+/****************************************************************************
+** StateOf1401
+**
+** Puts the current state of the 1401 in the Irp return buffer.
+*****************************************************************************/
+int StateOf1401(DEVICE_EXTENSION * pdx)
+{
+	int iReturn;
+	mutex_lock(&pdx->io_mutex);
+
+	QuickCheck(pdx, false, false);	// get state up to date, no reset
+	iReturn = pdx->sCurrentState;
+
+	mutex_unlock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s = %d", __func__, iReturn);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** StartSelfTest
+**
+** Initiates a self-test cycle. The assumption is that we have no interrupts
+** active, so we should make sure that this is the case.
+*****************************************************************************/
+int StartSelfTest(DEVICE_EXTENSION * pdx)
+{
+	int nGot;
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	ced_draw_down(pdx);	// wait for, then kill outstanding Urbs
+	FlushInBuff(pdx);	// Clear out input buffer & pipe
+	FlushOutBuff(pdx);	// Clear output buffer & pipe
+//    ReadWrite_Cancel(pDeviceObject);        /* so things stay tidy */
+	pdx->dwDMAFlag = MODE_CHAR;	/* Clear DMA mode flags here */
+
+	nGot = usb_control_msg(pdx->udev, usb_rcvctrlpipe(pdx->udev, 0), DB_SELFTEST, (H_TO_D | VENDOR | DEVREQ), 0, 0, 0, 0, HZ);	// allow 1 second timeout
+	pdx->ulSelfTestTime = jiffies + HZ * 30;	// 30 seconds into the future
+
+	mutex_unlock(&pdx->io_mutex);
+	if (nGot < 0)
+		dev_err(&pdx->interface->dev, "%s err=%d", __func__, nGot);
+	return nGot < 0 ? U14ERR_FAIL : U14ERR_NOERROR;
+}
+
+/****************************************************************************
+** CheckSelfTest
+**
+** Check progress of a self-test cycle
+****************************************************************************/
+int CheckSelfTest(DEVICE_EXTENSION * pdx, TGET_SELFTEST __user * pGST)
+{
+	unsigned int state, error;
+	int iReturn;
+	TGET_SELFTEST gst;	// local work space
+	memset(&gst, 0, sizeof(gst));	// clear out the space (sets code 0)
+
+	mutex_lock(&pdx->io_mutex);
+
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+	iReturn = Get1401State(pdx, &state, &error);
+	if (iReturn == U14ERR_NOERROR)	// Only accept zero if it happens twice
+		iReturn = Get1401State(pdx, &state, &error);
+
+	if (iReturn != U14ERR_NOERROR)	// Self-test can cause comms errors
+	{			// so we assume still testing
+		dev_err(&pdx->interface->dev,
+			"%s Get1401State=%d, assuming still testing", __func__,
+			iReturn);
+		state = 0x80;	// Force still-testing, no error
+		error = 0;
+		iReturn = U14ERR_NOERROR;
+	}
+
+	if ((state == -1) && (error == -1))	// If Get1401State had problems
+	{
+		dev_err(&pdx->interface->dev,
+			"%s Get1401State failed, assuming still testing",
+			__func__);
+		state = 0x80;	// Force still-testing, no error
+		error = 0;
+	}
+
+	if ((state & 0xFF) == 0x80)	// If we are still in self-test
+	{
+		if (state & 0x00FF0000)	// Have we got an error?
+		{
+			gst.code = (state & 0x00FF0000) >> 16;	// read the error code
+			gst.x = error & 0x0000FFFF;	// Error data X
+			gst.y = (error & 0xFFFF0000) >> 16;	// and data Y
+			dev_dbg(&pdx->interface->dev, "Self-test error code %d",
+				gst.code);
+		} else		// No error, check for timeout
+		{
+			unsigned long ulNow = jiffies;	// get current time
+			if (time_after(ulNow, pdx->ulSelfTestTime)) {
+				gst.code = -2;	// Flag the timeout
+				dev_dbg(&pdx->interface->dev,
+					"Self-test timed-out");
+			} else
+				dev_dbg(&pdx->interface->dev,
+					"Self-test on-going");
+		}
+	} else {
+		gst.code = -1;	// Flag the test is done
+		dev_dbg(&pdx->interface->dev, "Self-test done");
+	}
+
+	if (gst.code < 0)	// If we have a problem or finished
+	{			// If using the 2890 we should reset properly
+		if ((pdx->nPipes == 4) && (pdx->s1401Type <= TYPEPOWER))
+			Is1401(pdx);	// Get 1401 reset and OK
+		else
+			QuickCheck(pdx, true, true);	// Otherwise check without reset unless problems
+	}
+	mutex_unlock(&pdx->io_mutex);
+
+	if (copy_to_user(pGST, &gst, sizeof(gst)))
+		return -EFAULT;
+
+	return iReturn;
+}
+
+/****************************************************************************
+** TypeOf1401
+**
+** Returns code for standard, plus, micro1401, power1401 or none
+****************************************************************************/
+int TypeOf1401(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = TYPEUNKNOWN;
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	switch (pdx->s1401Type) {
+	case TYPE1401:
+		iReturn = U14ERR_STD;
+		break;		// Handle these types directly
+	case TYPEPLUS:
+		iReturn = U14ERR_PLUS;
+		break;
+	case TYPEU1401:
+		iReturn = U14ERR_U1401;
+		break;
+	default:
+		if ((pdx->s1401Type >= TYPEPOWER) && (pdx->s1401Type <= 25))
+			iReturn = pdx->s1401Type + 4;	// We can calculate types
+		else		//  for up-coming 1401 designs
+			iReturn = TYPEUNKNOWN;	// Don't know or not there
+	}
+	dev_dbg(&pdx->interface->dev, "%s %d", __func__, iReturn);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** TransferFlags
+**
+** Returns flags on block transfer abilities
+****************************************************************************/
+int TransferFlags(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = U14TF_MULTIA | U14TF_DIAG |	// we always have multiple DMA area
+	    U14TF_NOTIFY | U14TF_CIRCTH;	// diagnostics, notify and circular
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+	mutex_lock(&pdx->io_mutex);
+	if (pdx->bIsUSB2)	// Set flag for USB2 if appropriate
+		iReturn |= U14TF_USB2;
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/***************************************************************************
+** DbgCmd1401
+** Issues a debug\diagnostic command to the 1401 along with a 32-bit datum
+** This is a utility command used for dbg operations.
+*/
+static int DbgCmd1401(DEVICE_EXTENSION * pdx, unsigned char cmd,
+		      unsigned int data)
+{
+	int iReturn;
+	dev_dbg(&pdx->interface->dev, "%s entry", __func__);
+	iReturn = usb_control_msg(pdx->udev, usb_sndctrlpipe(pdx->udev, 0), cmd, (H_TO_D | VENDOR | DEVREQ), (unsigned short)data, (unsigned short)(data >> 16), 0, 0, HZ);	// allow 1 second timeout
+	if (iReturn < 0)
+		dev_err(&pdx->interface->dev, "%s fail code=%d", __func__,
+			iReturn);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgPeek
+**
+** Execute the diagnostic peek operation. Uses address, width and repeats.
+****************************************************************************/
+int DbgPeek(DEVICE_EXTENSION * pdx, TDBGBLOCK __user * pDB)
+{
+	int iReturn;
+	TDBGBLOCK db;
+
+	if (copy_from_user(&db, pDB, sizeof(db)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s @ %08x", __func__, db.iAddr);
+
+	iReturn = DbgCmd1401(pdx, DB_SETADD, db.iAddr);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_WIDTH, db.iWidth);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_REPEATS, db.iRepeats);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_PEEK, 0);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgPoke
+**
+** Execute the diagnostic poke operation. Parameters are in the CSBLOCK struct
+** in order address, size, repeats and value to poke.
+****************************************************************************/
+int DbgPoke(DEVICE_EXTENSION * pdx, TDBGBLOCK __user * pDB)
+{
+	int iReturn;
+	TDBGBLOCK db;
+
+	if (copy_from_user(&db, pDB, sizeof(db)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s @ %08x", __func__, db.iAddr);
+
+	iReturn = DbgCmd1401(pdx, DB_SETADD, db.iAddr);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_WIDTH, db.iWidth);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_REPEATS, db.iRepeats);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_POKE, db.iData);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgRampData
+**
+** Execute the diagnostic ramp data operation. Parameters are in the CSBLOCK struct
+** in order address, default, enable mask, size and repeats.
+****************************************************************************/
+int DbgRampData(DEVICE_EXTENSION * pdx, TDBGBLOCK __user * pDB)
+{
+	int iReturn;
+	TDBGBLOCK db;
+
+	if (copy_from_user(&db, pDB, sizeof(db)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s @ %08x", __func__, db.iAddr);
+
+	iReturn = DbgCmd1401(pdx, DB_SETADD, db.iAddr);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_SETDEF, db.iDefault);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_SETMASK, db.iMask);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_WIDTH, db.iWidth);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_REPEATS, db.iRepeats);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_RAMPD, 0);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgRampAddr
+**
+** Execute the diagnostic ramp address operation
+****************************************************************************/
+int DbgRampAddr(DEVICE_EXTENSION * pdx, TDBGBLOCK __user * pDB)
+{
+	int iReturn;
+	TDBGBLOCK db;
+
+	if (copy_from_user(&db, pDB, sizeof(db)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	iReturn = DbgCmd1401(pdx, DB_SETDEF, db.iDefault);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_SETMASK, db.iMask);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_WIDTH, db.iWidth);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_REPEATS, db.iRepeats);
+	if (iReturn == U14ERR_NOERROR)
+		iReturn = DbgCmd1401(pdx, DB_RAMPA, 0);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgGetData
+**
+** Retrieve the data resulting from the last debug Peek operation
+****************************************************************************/
+int DbgGetData(DEVICE_EXTENSION * pdx, TDBGBLOCK __user * pDB)
+{
+	int iReturn;
+	TDBGBLOCK db;
+	memset(&db, 0, sizeof(db));	// fill returned block with 0s
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	// Read back the last peeked value from the 1401.
+	iReturn = usb_control_msg(pdx->udev, usb_rcvctrlpipe(pdx->udev, 0),
+				  DB_DATA, (D_TO_H | VENDOR | DEVREQ), 0, 0,
+				  &db.iData, sizeof(db.iData), HZ);
+	if (iReturn == sizeof(db.iData)) {
+		if (copy_to_user(pDB, &db, sizeof(db)))
+			iReturn = -EFAULT;
+		else
+			iReturn = U14ERR_NOERROR;
+	} else
+		dev_err(&pdx->interface->dev, "%s failed, code %d", __func__,
+			iReturn);
+
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** DbgStopLoop
+**
+** Stop any never-ending debug loop, we just call Get1401State for USB
+**
+****************************************************************************/
+int DbgStopLoop(DEVICE_EXTENSION * pdx)
+{
+	int iReturn;
+	unsigned int uState, uErr;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+	iReturn = Get1401State(pdx, &uState, &uErr);
+	mutex_unlock(&pdx->io_mutex);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** SetCircular
+**
+** Sets up a transfer area record for circular transfers. If the area is
+** already set, we attempt to unset it. Unsetting will fail if the area is
+** booked and a transfer to that area is in progress. Otherwise, we will
+** release the area and re-assign it.
+****************************************************************************/
+int SetCircular(DEVICE_EXTENSION * pdx, TRANSFERDESC __user * pTD)
+{
+	int iReturn;
+	bool bToHost;
+	TRANSFERDESC td;
+
+	if (copy_from_user(&td, pTD, sizeof(td)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s area:%d, size:%08x", __func__,
+		td.wAreaNum, td.dwLength);
+	bToHost = td.eSize != 0;	// this is used as the tohost flag
+
+	// The strange cast is done so that we don't get warnings in 32-bit linux about the size of the
+	// pointer. The pointer is always passed as a 64-bit object so that we don't have problems using
+	// a 32-bit program on a 64-bit system. unsigned long is 64-bits on a 64-bit system.
+	iReturn =
+	    SetArea(pdx, td.wAreaNum,
+		    (char __user *)((unsigned long)td.lpvBuff), td.dwLength,
+		    true, bToHost);
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/****************************************************************************
+** GetCircBlock
+**
+** Return the next available block of circularly-transferred data.
+****************************************************************************/
+int GetCircBlock(DEVICE_EXTENSION * pdx, TCIRCBLOCK __user * pCB)
+{
+	int iReturn = U14ERR_NOERROR;
+	unsigned int nArea;
+	TCIRCBLOCK cb;
+
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	if (copy_from_user(&cb, pCB, sizeof(cb)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+
+	nArea = cb.nArea;	// Retrieve parameters first
+	cb.dwOffset = 0;	// set default result (nothing)
+	cb.dwSize = 0;
+
+	if (nArea < MAX_TRANSAREAS)	// The area number must be OK
+	{
+		TRANSAREA *pArea = &pdx->rTransDef[nArea];	// Pointer to relevant info
+		spin_lock_irq(&pdx->stagedLock);	// Lock others out
+
+		if ((pArea->bUsed) && (pArea->bCircular) &&	// Must be circular area
+		    (pArea->bCircToHost))	// For now at least must be to host
+		{
+			if (pArea->aBlocks[0].dwSize > 0)	// Got anything?
+			{
+				cb.dwOffset = pArea->aBlocks[0].dwOffset;
+				cb.dwSize = pArea->aBlocks[0].dwSize;
+				dev_dbg(&pdx->interface->dev,
+					"%s return block 0: %d bytes at %d",
+					__func__, cb.dwSize, cb.dwOffset);
+			}
+		} else
+			iReturn = U14ERR_NOTSET;
+
+		spin_unlock_irq(&pdx->stagedLock);
+	} else
+		iReturn = U14ERR_BADAREA;
+
+	if (copy_to_user(pCB, &cb, sizeof(cb)))
+		iReturn = -EFAULT;
+
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
+
+/****************************************************************************
+** FreeCircBlock
+**
+** Frees a block of circularly-transferred data and returns the next one.
+****************************************************************************/
+int FreeCircBlock(DEVICE_EXTENSION * pdx, TCIRCBLOCK __user * pCB)
+{
+	int iReturn = U14ERR_NOERROR;
+	unsigned int nArea, uStart, uSize;
+	TCIRCBLOCK cb;
+
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	if (copy_from_user(&cb, pCB, sizeof(cb)))
+		return -EFAULT;
+
+	mutex_lock(&pdx->io_mutex);
+
+	nArea = cb.nArea;	// Retrieve parameters first
+	uStart = cb.dwOffset;
+	uSize = cb.dwSize;
+	cb.dwOffset = 0;	// then set default result (nothing)
+	cb.dwSize = 0;
+
+	if (nArea < MAX_TRANSAREAS)	// The area number must be OK
+	{
+		TRANSAREA *pArea = &pdx->rTransDef[nArea];	// Pointer to relevant info
+		spin_lock_irq(&pdx->stagedLock);	// Lock others out
+
+		if ((pArea->bUsed) && (pArea->bCircular) &&	// Must be circular area
+		    (pArea->bCircToHost))	// For now at least must be to host
+		{
+			bool bWaiting = false;
+
+			if ((pArea->aBlocks[0].dwSize >= uSize) &&	// Got anything?
+			    (pArea->aBlocks[0].dwOffset == uStart))	// Must be legal data
+			{
+				pArea->aBlocks[0].dwSize -= uSize;
+				pArea->aBlocks[0].dwOffset += uSize;
+				if (pArea->aBlocks[0].dwSize == 0)	// Have we emptied this block?
+				{
+					if (pArea->aBlocks[1].dwSize)	// Is there a second block?
+					{
+						pArea->aBlocks[0] = pArea->aBlocks[1];	// Copy down block 2 data
+						pArea->aBlocks[1].dwSize = 0;	// and mark the second block as unused
+						pArea->aBlocks[1].dwOffset = 0;
+					} else
+						pArea->aBlocks[0].dwOffset = 0;
+				}
+
+				dev_dbg(&pdx->interface->dev,
+					"%s free %d bytes at %d, return %d bytes at %d, wait=%d",
+					__func__, uSize, uStart,
+					pArea->aBlocks[0].dwSize,
+					pArea->aBlocks[0].dwOffset,
+					pdx->bXFerWaiting);
+
+				// Return the next available block of memory as well
+				if (pArea->aBlocks[0].dwSize > 0)	// Got anything?
+				{
+					cb.dwOffset =
+					    pArea->aBlocks[0].dwOffset;
+					cb.dwSize = pArea->aBlocks[0].dwSize;
+				}
+
+				bWaiting = pdx->bXFerWaiting;
+				if (bWaiting && pdx->bStagedUrbPending) {
+					dev_err(&pdx->interface->dev,
+						"%s ERROR: waiting xfer and staged Urb pending!",
+						__func__);
+					bWaiting = false;
+				}
+			} else {
+				dev_err(&pdx->interface->dev,
+					"%s ERROR: freeing %d bytes at %d, block 0 is %d bytes at %d",
+					__func__, uSize, uStart,
+					pArea->aBlocks[0].dwSize,
+					pArea->aBlocks[0].dwOffset);
+				iReturn = U14ERR_NOMEMORY;
+			}
+
+			// If we have one, kick off pending transfer
+			if (bWaiting)	// Got a block xfer waiting?
+			{
+				int RWMStat =
+				    ReadWriteMem(pdx, !pdx->rDMAInfo.bOutWard,
+						 pdx->rDMAInfo.wIdent,
+						 pdx->rDMAInfo.dwOffset,
+						 pdx->rDMAInfo.dwSize);
+				if (RWMStat != U14ERR_NOERROR)
+					dev_err(&pdx->interface->dev,
+						"%s rw setup failed %d",
+						__func__, RWMStat);
+			}
+		} else
+			iReturn = U14ERR_NOTSET;
+
+		spin_unlock_irq(&pdx->stagedLock);
+	} else
+		iReturn = U14ERR_BADAREA;
+
+	if (copy_to_user(pCB, &cb, sizeof(cb)))
+		return -EFAULT;
+
+	mutex_unlock(&pdx->io_mutex);
+	return iReturn;
+}
diff --git a/drivers/staging/ced1401/ced_ioctl.h b/drivers/staging/ced1401/ced_ioctl.h
new file mode 100644
index 000000000000..0895c9414b4f
--- /dev/null
+++ b/drivers/staging/ced1401/ced_ioctl.h
@@ -0,0 +1,345 @@
+/*
+ * IOCTL calls for the CED1401 driver
+ * Copyright (C) 2010 Cambridge Electronic Design Ltd
+ * Author Greg P Smith (greg@ced.co.uk)
+ *
+ * This program is free software; you can redistribute 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 __CED_IOCTL_H__
+#define __CED_IOCTL_H__
+
+#include <linux/ioctl.h>
+
+/* dma modes, only MODE_CHAR and MODE_LINEAR are used in this driver */
+#define MODE_CHAR		0
+#define MODE_LINEAR		1
+
+/****************************************************************************
+** TypeDefs
+*****************************************************************************/
+
+typedef unsigned short TBLOCKENTRY;	/* index the blk transfer table 0-7 */
+
+typedef struct TransferDesc {
+	long long lpvBuff;	/* address of transfer area (for 64 or 32 bit) */
+	unsigned int dwLength;	/* length of the area */
+	TBLOCKENTRY wAreaNum;	/* number of transfer area to set up */
+	short eSize;		/* element size - is tohost flag for circular */
+} TRANSFERDESC;
+
+typedef TRANSFERDESC * LPTRANSFERDESC;
+
+typedef struct TransferEvent {
+	unsigned int dwStart;		/* offset into the area */
+	unsigned int dwLength;		/* length of the region */
+	unsigned short wAreaNum;	/* the area number */
+	unsigned short wFlags;		/* bit 0 set for toHost */
+	int iSetEvent;			/* could be dummy in LINUX */
+} TRANSFEREVENT;
+
+#define MAX_TRANSFER_SIZE	0x4000		/* Maximum data bytes per IRP */
+#define MAX_AREA_LENGTH		0x100000	/* Maximum size of transfer area */
+#define MAX_TRANSAREAS		8		/* definitions for dma set up  */
+
+typedef struct TGetSelfTest {
+	int code;			/* self-test error code */
+	int x, y;			/* additional information */
+} TGET_SELFTEST;
+
+/* Debug block used for several commands. Not all fields are used for all commands. */
+typedef struct TDbgBlock {
+	int iAddr;			/* the address in the 1401 */
+	int iRepeats;			/* number of repeats */
+	int iWidth;			/* width in bytes 1, 2, 4 */
+	int iDefault;			/* default value */
+	int iMask;			/* mask to apply */
+	int iData;			/* data for poke, result for peek */
+} TDBGBLOCK;
+
+/* Used to collect information about a circular block from the device driver */
+typedef struct TCircBlock {
+	unsigned int nArea;		/* the area to collect information from */
+	unsigned int dwOffset;		/* offset into the area to the available block */
+	unsigned int dwSize;		/* size of the area */
+} TCIRCBLOCK;
+
+/* Used to clollect the 1401 status */
+typedef struct TCSBlock {
+	unsigned int uiState;
+	unsigned int uiError;
+} TCSBLOCK;
+
+/*
+ * As seen by the user, an ioctl call looks like: int ioctl(int fd, unsigned
+ * long cmd, char* argp); We will then have all sorts of variants on this that
+ * can be used to pass stuff to our driver. We will generate macros for each
+ * type of call so as to provide some sort of type safety in the calling:
+ */
+#define CED_MAGIC_IOC 0xce
+
+/* NBNB: READ and WRITE are from the point of view of the device, not user. */
+typedef struct ced_ioc_string {
+	int nChars;
+	char buffer[256];
+} CED_IOC_STRING;
+
+#define IOCTL_CED_SENDSTRING(n)		_IOC(_IOC_WRITE, CED_MAGIC_IOC, 2, n)
+
+#define IOCTL_CED_RESET1401		_IO(CED_MAGIC_IOC, 3)
+#define IOCTL_CED_GETCHAR		_IO(CED_MAGIC_IOC, 4)
+#define IOCTL_CED_SENDCHAR		_IO(CED_MAGIC_IOC, 5)
+#define IOCTL_CED_STAT1401		_IO(CED_MAGIC_IOC, 6)
+#define IOCTL_CED_LINECOUNT		_IO(CED_MAGIC_IOC, 7)
+#define IOCTL_CED_GETSTRING(nMax)	_IOC(_IOC_READ, CED_MAGIC_IOC, 8, nMax)
+
+#define IOCTL_CED_SETTRANSFER		_IOW(CED_MAGIC_IOC, 11, TRANSFERDESC)
+#define IOCTL_CED_UNSETTRANSFER		_IO(CED_MAGIC_IOC, 12)
+#define IOCTL_CED_SETEVENT		_IOW(CED_MAGIC_IOC, 13, TRANSFEREVENT)
+#define IOCTL_CED_GETOUTBUFSPACE	_IO(CED_MAGIC_IOC, 14)
+#define IOCTL_CED_GETBASEADDRESS	_IO(CED_MAGIC_IOC, 15)
+#define IOCTL_CED_GETDRIVERREVISION	_IO(CED_MAGIC_IOC, 16)
+
+#define IOCTL_CED_GETTRANSFER		_IOR(CED_MAGIC_IOC, 17, TGET_TX_BLOCK)
+#define IOCTL_CED_KILLIO1401		_IO(CED_MAGIC_IOC, 18)
+#define IOCTL_CED_BLKTRANSSTATE		_IO(CED_MAGIC_IOC, 19)
+
+#define IOCTL_CED_STATEOF1401		_IO(CED_MAGIC_IOC, 23)
+#define IOCTL_CED_GRAB1401		_IO(CED_MAGIC_IOC, 25)
+#define IOCTL_CED_FREE1401		_IO(CED_MAGIC_IOC, 26)
+#define IOCTL_CED_STARTSELFTEST		_IO(CED_MAGIC_IOC, 31)
+#define IOCTL_CED_CHECKSELFTEST		_IOR(CED_MAGIC_IOC, 32, TGET_SELFTEST)
+#define IOCTL_CED_TYPEOF1401		_IO(CED_MAGIC_IOC, 33)
+#define IOCTL_CED_TRANSFERFLAGS		_IO(CED_MAGIC_IOC, 34)
+
+#define IOCTL_CED_DBGPEEK		_IOW(CED_MAGIC_IOC, 35, TDBGBLOCK)
+#define IOCTL_CED_DBGPOKE		_IOW(CED_MAGIC_IOC, 36, TDBGBLOCK)
+#define IOCTL_CED_DBGRAMPDATA		_IOW(CED_MAGIC_IOC, 37, TDBGBLOCK)
+#define IOCTL_CED_DBGRAMPADDR		_IOW(CED_MAGIC_IOC, 38, TDBGBLOCK)
+#define IOCTL_CED_DBGGETDATA		_IOR(CED_MAGIC_IOC, 39, TDBGBLOCK)
+#define IOCTL_CED_DBGSTOPLOOP		_IO(CED_MAGIC_IOC, 40)
+#define IOCTL_CED_FULLRESET		_IO(CED_MAGIC_IOC, 41)
+#define IOCTL_CED_SETCIRCULAR		_IOW(CED_MAGIC_IOC, 42, TRANSFERDESC)
+#define IOCTL_CED_GETCIRCBLOCK		_IOWR(CED_MAGIC_IOC, 43, TCIRCBLOCK)
+#define IOCTL_CED_FREECIRCBLOCK		_IOWR(CED_MAGIC_IOC, 44, TCIRCBLOCK)
+#define IOCTL_CED_WAITEVENT		_IO(CED_MAGIC_IOC, 45)
+#define IOCTL_CED_TESTEVENT		_IO(CED_MAGIC_IOC, 46)
+
+#ifndef __KERNEL__
+/*
+ * If nothing said about return value, it is a U14ERR_... error code
+ * (U14ERR_NOERROR for none)
+ */
+inline int CED_SendString(int fh, const char *szText, int n)
+{
+	return ioctl(fh, IOCTL_CED_SENDSTRING(n), szText);
+}
+
+inline int CED_Reset1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_RESET1401);
+}
+
+/* Return the singe character or a -ve error code. */
+inline int CED_GetChar(int fh)
+{
+	return ioctl(fh, IOCTL_CED_GETCHAR);
+}
+
+/* Return character count in input buffer */
+inline int CED_Stat1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_STAT1401);
+}
+
+inline int CED_SendChar(int fh, char c)
+{
+	return ioctl(fh, IOCTL_CED_SENDCHAR, c);
+}
+
+inline int CED_LineCount(int fh)
+{
+	return ioctl(fh, IOCTL_CED_LINECOUNT);
+}
+
+/*
+ * return the count of characters returned. If the string was terminated by CR
+ * or 0, then the 0 is part of the count. Otherwise, we will add a zero if
+ * there is room, but it is not included in the count.  The return value is 0
+ * if there was nothing to read.
+ */
+inline int CED_GetString(int fh, char *szText, int nMax)
+{
+	return ioctl(fh, IOCTL_CED_GETSTRING(nMax), szText);
+}
+
+/* returns space in the output buffer. */
+inline int CED_GetOutBufSpace(int fh)
+{
+	return ioctl(fh, IOCTL_CED_GETOUTBUFSPACE);
+}
+
+/* This always returns -1 as not implemented. */
+inline int CED_GetBaseAddress(int fh)
+{
+	return ioctl(fh, IOCTL_CED_GETBASEADDRESS);
+}
+
+/* returns the major revision <<16 | minor revision. */
+inline int CED_GetDriverRevision(int fh)
+{
+	return ioctl(fh, IOCTL_CED_GETDRIVERREVISION);
+}
+
+inline int CED_SetTransfer(int fh, TRANSFERDESC *pTD)
+{
+	return ioctl(fh, IOCTL_CED_SETTRANSFER, pTD);
+}
+
+inline int CED_UnsetTransfer(int fh, int nArea)
+{
+	return ioctl(fh, IOCTL_CED_UNSETTRANSFER, nArea);
+}
+
+inline int CED_SetEvent(int fh, TRANSFEREVENT *pTE)
+{
+	return ioctl(fh, IOCTL_CED_SETEVENT, pTE);
+}
+
+inline int CED_GetTransfer(int fh, TGET_TX_BLOCK *pTX)
+{
+	return ioctl(fh, IOCTL_CED_GETTRANSFER, pTX);
+}
+
+inline int CED_KillIO1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_KILLIO1401);
+}
+
+/* returns 0 if no active DMA, 1 if active */
+inline int CED_BlkTransState(int fh)
+{
+	return ioctl(fh, IOCTL_CED_BLKTRANSSTATE);
+}
+
+inline int CED_StateOf1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_STATEOF1401);
+}
+
+inline int CED_Grab1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_GRAB1401);
+}
+
+inline int CED_Free1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_FREE1401);
+}
+
+inline int CED_StartSelfTest(int fh)
+{
+	return ioctl(fh, IOCTL_CED_STARTSELFTEST);
+}
+
+inline int CED_CheckSelfTest(int fh, TGET_SELFTEST *pGST)
+{
+	return ioctl(fh, IOCTL_CED_CHECKSELFTEST, pGST);
+}
+
+inline int CED_TypeOf1401(int fh)
+{
+	return ioctl(fh, IOCTL_CED_TYPEOF1401);
+}
+
+inline int CED_TransferFlags(int fh)
+{
+	return ioctl(fh, IOCTL_CED_TRANSFERFLAGS);
+}
+
+inline int CED_DbgPeek(int fh, TDBGBLOCK *pDB)
+{
+	return ioctl(fh, IOCTL_CED_DBGPEEK, pDB);
+}
+
+inline int CED_DbgPoke(int fh, TDBGBLOCK *pDB)
+{
+	return ioctl(fh, IOCTL_CED_DBGPOKE, pDB);
+}
+
+inline int CED_DbgRampData(int fh, TDBGBLOCK *pDB)
+{
+	return ioctl(fh, IOCTL_CED_DBGRAMPDATA, pDB);
+}
+
+inline int CED_DbgRampAddr(int fh, TDBGBLOCK *pDB)
+{
+	return ioctl(fh, IOCTL_CED_DBGRAMPADDR, pDB);
+}
+
+inline int CED_DbgGetData(int fh, TDBGBLOCK *pDB)
+{
+	return ioctl(fh, IOCTL_CED_DBGGETDATA, pDB);
+}
+
+inline int CED_DbgStopLoop(int fh)
+{
+	return ioctl(fh, IOCTL_CED_DBGSTOPLOOP);
+}
+
+inline int CED_FullReset(int fh)
+{
+	return ioctl(fh, IOCTL_CED_FULLRESET);
+}
+
+inline int CED_SetCircular(int fh, TRANSFERDESC *pTD)
+{
+	return ioctl(fh, IOCTL_CED_SETCIRCULAR, pTD);
+}
+
+inline int CED_GetCircBlock(int fh, TCIRCBLOCK *pCB)
+{
+	return ioctl(fh, IOCTL_CED_GETCIRCBLOCK, pCB);
+}
+
+inline int CED_FreeCircBlock(int fh, TCIRCBLOCK *pCB)
+{
+	return ioctl(fh, IOCTL_CED_FREECIRCBLOCK, pCB);
+}
+
+inline int CED_WaitEvent(int fh, int nArea, int msTimeOut)
+{
+	return ioctl(fh, IOCTL_CED_WAITEVENT, (nArea & 0xff)|(msTimeOut << 8));
+}
+
+inline int CED_TestEvent(int fh, int nArea)
+{
+	return ioctl(fh, IOCTL_CED_TESTEVENT, nArea);
+}
+#endif
+
+#ifdef NOTWANTEDYET
+#define IOCTL_CED_REGCALLBACK		_IO(CED_MAGIC_IOC, 9)	/* Not used */
+#define IOCTL_CED_GETMONITORBUF		_IO(CED_MAGIC_IOC, 10)	/* Not used */
+
+#define IOCTL_CED_BYTECOUNT		_IO(CED_MAGIC_IOC, 20)	/* Not used */
+#define IOCTL_CED_ZEROBLOCKCOUNT	_IO(CED_MAGIC_IOC, 21)	/* Not used */
+#define IOCTL_CED_STOPCIRCULAR		_IO(CED_MAGIC_IOC, 22)	/* Not used */
+
+#define IOCTL_CED_REGISTERS1401		_IO(CED_MAGIC_IOC, 24)	/* Not used */
+#define IOCTL_CED_STEP1401		_IO(CED_MAGIC_IOC, 27)	/* Not used */
+#define IOCTL_CED_SET1401REGISTERS	_IO(CED_MAGIC_IOC, 28)	/* Not used */
+#define IOCTL_CED_STEPTILL1401		_IO(CED_MAGIC_IOC, 29)	/* Not used */
+#define IOCTL_CED_SETORIN		_IO(CED_MAGIC_IOC, 30)	/* Not used */
+
+#endif
+
+/* __CED_IOCTL_H__ */
+#endif
diff --git a/drivers/staging/ced1401/machine.h b/drivers/staging/ced1401/machine.h
new file mode 100644
index 000000000000..af073790b942
--- /dev/null
+++ b/drivers/staging/ced1401/machine.h
@@ -0,0 +1,127 @@
+/*****************************************************************************
+**
+** machine.h
+**
+** Copyright (c) Cambridge Electronic Design Limited 1991,1992,2010
+**
+** This program is free software; you can redistribute it and/or
+** modify it under the terms of the GNU General Public License
+** as published by the Free Software Foundation; either version 2
+** of the License, or (at your option) any later version.
+**
+** This program is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+** GNU General Public License for more details.
+**
+** You should have received a copy of the GNU General Public License
+** along with this program; if not, write to the Free Software
+** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+**
+** Contact CED: Cambridge Electronic Design Limited, Science Park, Milton Road
+**              Cambridge, CB6 0FE.
+**              www.ced.co.uk
+**              greg@ced.co.uk
+**
+** This file is included at the start of 'C' or 'C++' source file to define
+** things for cross-platform/compiler interoperability. This used to deal with
+** MSDOS/16-bit stuff, but this was all removed in Decemeber 2010. There are
+** three things to consider: Windows, LINUX, mac OSX (BSD Unix) and 32 vs 64
+** bit. At the time of writing (DEC 2010) there is a consensus on the following
+** and their unsigned equivalents:
+**
+** type       bits
+** char         8
+** short       16
+** int         32
+** long long   64
+**
+** long is a problem as it is always 64 bits on linux/unix and is always 32 bits
+** on windows.
+** On windows, we define _IS_WINDOWS_ and one of WIN32 or WIN64.
+** On linux we define LINUX
+** On Max OSX we define MACOSX
+**
+*/
+
+#ifndef __MACHINE_H__
+#define __MACHINE_H__
+#ifndef __KERNEL__
+#include <float.h>
+#include <limits.h>
+#endif
+
+/*
+** The initial section is to identify the operating system
+*/
+#if (defined(__linux__) || defined(_linux) || defined(__linux)) && !defined(LINUX)
+#define LINUX 1
+#endif
+
+#if (defined(__WIN32__) || defined(_WIN32)) && !defined(WIN32)
+#define WIN32 1
+#endif
+
+#if defined(__APPLE__)
+#define MACOSX
+#endif
+
+#if defined(_WIN64)
+#undef WIN32
+#undef WIN64
+#define WIN64 1
+#endif
+
+#if defined(WIN32) || defined(WIN64)
+#define _IS_WINDOWS_ 1
+#endif
+
+#if defined(LINUX) || defined(MAXOSX)
+    #define FAR
+
+    typedef int BOOL;       // To match Windows
+    typedef char * LPSTR;
+    typedef const char * LPCSTR;
+    typedef unsigned short WORD;
+    typedef unsigned int  DWORD;
+    typedef unsigned char  BYTE;
+    typedef BYTE  BOOLEAN;
+    typedef unsigned char UCHAR;
+    #define __packed __attribute__((packed))
+    typedef BYTE * LPBYTE;
+    #define HIWORD(x) (WORD)(((x)>>16) & 0xffff)
+    #define LOWORD(x) (WORD)((x) & 0xffff)
+#endif
+
+#ifdef _IS_WINDOWS_
+#include <windows.h>
+#define __packed
+#endif
+
+/*
+** Sort out the DllExport and DllImport macros. The GCC compiler has its own
+** syntax for this, though it also supports the MS specific __declspec() as
+** a synonym.
+*/
+#ifdef GNUC
+    #define DllExport __attribute__((dllexport))
+    #define DllImport __attribute__((dllimport))
+#endif
+
+#ifndef DllExport
+#ifdef _IS_WINDOWS_
+    #define DllExport __declspec(dllexport)
+    #define DllImport __declspec(dllimport)
+#else
+    #define DllExport
+    #define DllImport
+#endif
+#endif /* _IS_WINDOWS_ */
+
+    
+#ifndef TRUE
+   #define TRUE 1
+   #define FALSE 0
+#endif
+
+#endif
diff --git a/drivers/staging/ced1401/usb1401.c b/drivers/staging/ced1401/usb1401.c
new file mode 100644
index 000000000000..6ba0ef652561
--- /dev/null
+++ b/drivers/staging/ced1401/usb1401.c
@@ -0,0 +1,1637 @@
+/***********************************************************************************
+ CED1401 usb driver. This basic loading is based on the usb-skeleton.c code that is:
+ Copyright (C) 2001-2004 Greg Kroah-Hartman (greg@kroah.com)
+ Copyright (C) 2012 Alois Schloegl <alois.schloegl@ist.ac.at>
+ There is not a great deal of the skeleton left.
+
+ All the remainder dealing specifically with the CED1401 is based on drivers written
+ by CED for other systems (mainly Windows) and is:
+ Copyright (C) 2010 Cambridge Electronic Design Ltd
+ Author Greg P Smith (greg@ced.co.uk)
+
+ This program is free software; you can redistribute it and/or
+ modify it under the terms of the GNU General Public License
+ as published by the Free Software Foundation; either version 2
+ of the License, or (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+Endpoints
+*********
+There are 4 endpoints plus the control endpoint in the standard interface
+provided by most 1401s. The control endpoint is used for standard USB requests,
+plus various CED-specific transactions such as start self test, debug and get
+the 1401 status. The other endpoints are:
+
+ 1 Characters to the 1401
+ 2 Characters from the 1401
+ 3 Block data to the 1401
+ 4 Block data to the host.
+
+inside the driver these are indexed as an array from 0 to 3, transactions
+over the control endpoint are carried out using a separate mechanism. The
+use of the endpoints is mostly straightforward, with the driver issuing
+IO request packets (IRPs) as required to transfer data to and from the 1401.
+The handling of endpoint 2 is different because it is used for characters
+from the 1401, which can appear spontaneously and without any other driver
+activity - for example to repeatedly request DMA transfers in Spike2. The
+desired effect is achieved by using an interrupt endpoint which can be
+polled to see if it has data available, and writing the driver so that it
+always maintains a pending read IRP from that endpoint which will read the
+character data and terminate as soon as the 1401 makes data available. This
+works very well, some care is taken with when you kick off this character
+read IRP to avoid it being active when it is not wanted but generally it
+is running all the time.
+
+In the 2270, there are only three endpoints plus the control endpoint. In
+addition to the transactions mentioned above, the control endpoint is used
+to transfer character data to the 1401. The other endpoints are used as:
+
+ 1 Characters from the 1401
+ 2 Block data to the 1401
+ 3 Block data to the host.
+
+The type of interface available is specified by the interface subclass field
+in the interface descriptor provided by the 1401. See the USB_INT_ constants
+for the values that this field can hold.
+
+****************************************************************************
+Linux implementation
+
+Although Linux Device Drivers (3rd Edition) was a major source of information,
+it is very out of date. A lot of information was gleaned from the latest
+usb_skeleton.c code (you need to download the kernel sources to get this).
+
+To match the Windows version, everything is done using ioctl calls. All the
+device state is held in the DEVICE_EXTENSION (named to match Windows use).
+Block transfers are done by using get_user_pages() to pin down a list of
+pages that we hold a pointer to in the device driver. We also allocate a
+coherent transfer buffer of size STAGED_SZ (this must be a multiple of the
+bulk endpoint size so that the 1401 does not realise that we break large
+transfers down into smaller pieces). We use kmap_atomic() to get a kernel
+va for each page, as it is required, for copying; see CopyUserSpace().
+
+All character and data transfers are done using asynchronous IO. All Urbs are
+tracked by anchoring them. Status and debug ioctls are implemented with the
+synchronous non-Urb based transfers.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/usb.h>
+#include <linux/mutex.h>
+#include <linux/mm.h>
+#include <linux/highmem.h>
+#include <linux/version.h>
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35) )
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/uaccess.h>
+#endif
+
+#include "usb1401.h"
+
+/* Define these values to match your devices */
+#define USB_CED_VENDOR_ID	0x0525
+#define USB_CED_PRODUCT_ID	0xa0f0
+
+/* table of devices that work with this driver */
+static const struct usb_device_id ced_table[] = {
+	{USB_DEVICE(USB_CED_VENDOR_ID, USB_CED_PRODUCT_ID)},
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, ced_table);
+
+/* Get a minor range for your devices from the usb maintainer */
+#define USB_CED_MINOR_BASE	192
+
+/* our private defines. if this grows any larger, use your own .h file */
+#define MAX_TRANSFER		(PAGE_SIZE - 512)
+/* MAX_TRANSFER is chosen so that the VM is not stressed by
+   allocations > PAGE_SIZE and the number of packets in a page
+   is an integer 512 is the largest possible packet on EHCI */
+#define WRITES_IN_FLIGHT	8
+/* arbitrarily chosen */
+
+/* 
+The cause for these errors is that the driver makes use of the functions usb_buffer_alloc() and usb_buffer_free() which got renamed in kernel 2.6.35. This is stated in the Changelog:   USB: rename usb_buffer_alloc() and usb_buffer_free() users
+    For more clearance what the functions actually do,
+      usb_buffer_alloc() is renamed to usb_alloc_coherent()
+      usb_buffer_free()  is renamed to usb_free_coherent()
+   This is needed on Debian 2.6.32-5-amd64
+*/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) )
+#define usb_alloc_coherent usb_buffer_alloc
+#define usb_free_coherent  usb_buffer_free
+#define noop_llseek NULL
+#endif
+
+static struct usb_driver ced_driver;
+
+static void ced_delete(struct kref *kref)
+{
+	DEVICE_EXTENSION *pdx = to_DEVICE_EXTENSION(kref);
+
+	// Free up the output buffer, then free the output urb. Note that the interface member
+	// of pdx will probably be NULL, so cannot be used to get to dev.
+	usb_free_coherent(pdx->udev, OUTBUF_SZ, pdx->pCoherCharOut,
+			  pdx->pUrbCharOut->transfer_dma);
+	usb_free_urb(pdx->pUrbCharOut);
+
+	// Do the same for chan input
+	usb_free_coherent(pdx->udev, INBUF_SZ, pdx->pCoherCharIn,
+			  pdx->pUrbCharIn->transfer_dma);
+	usb_free_urb(pdx->pUrbCharIn);
+
+	// Do the same for the block transfers
+	usb_free_coherent(pdx->udev, STAGED_SZ, pdx->pCoherStagedIO,
+			  pdx->pStagedUrb->transfer_dma);
+	usb_free_urb(pdx->pStagedUrb);
+
+	usb_put_dev(pdx->udev);
+	kfree(pdx);
+}
+
+// This is the driver end of the open() call from user space.
+static int ced_open(struct inode *inode, struct file *file)
+{
+	DEVICE_EXTENSION *pdx;
+	int retval = 0;
+	int subminor = iminor(inode);
+	struct usb_interface *interface =
+	    usb_find_interface(&ced_driver, subminor);
+	if (!interface) {
+		pr_err("%s - error, can't find device for minor %d", __func__,
+		       subminor);
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	pdx = usb_get_intfdata(interface);
+	if (!pdx) {
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	dev_dbg(&interface->dev, "%s got pdx", __func__);
+
+	/* increment our usage count for the device */
+	kref_get(&pdx->kref);
+
+	/* lock the device to allow correctly handling errors
+	 * in resumption */
+	mutex_lock(&pdx->io_mutex);
+
+	if (!pdx->open_count++) {
+		retval = usb_autopm_get_interface(interface);
+		if (retval) {
+			pdx->open_count--;
+			mutex_unlock(&pdx->io_mutex);
+			kref_put(&pdx->kref, ced_delete);
+			goto exit;
+		}
+	} else {		//uncomment this block if you want exclusive open
+		dev_err(&interface->dev, "%s fail: already open", __func__);
+		retval = -EBUSY;
+		pdx->open_count--;
+		mutex_unlock(&pdx->io_mutex);
+		kref_put(&pdx->kref, ced_delete);
+		goto exit;
+	}
+	/* prevent the device from being autosuspended */
+
+	/* save our object in the file's private structure */
+	file->private_data = pdx;
+	mutex_unlock(&pdx->io_mutex);
+
+exit:
+	return retval;
+}
+
+static int ced_release(struct inode *inode, struct file *file)
+{
+	DEVICE_EXTENSION *pdx = file->private_data;
+	if (pdx == NULL)
+		return -ENODEV;
+
+	dev_dbg(&pdx->interface->dev, "%s called", __func__);
+	mutex_lock(&pdx->io_mutex);
+	if (!--pdx->open_count && pdx->interface)	// Allow autosuspend
+		usb_autopm_put_interface(pdx->interface);
+	mutex_unlock(&pdx->io_mutex);
+
+	kref_put(&pdx->kref, ced_delete);	// decrement the count on our device
+	return 0;
+}
+
+static int ced_flush(struct file *file, fl_owner_t id)
+{
+	int res;
+	DEVICE_EXTENSION *pdx = file->private_data;
+	if (pdx == NULL)
+		return -ENODEV;
+
+	dev_dbg(&pdx->interface->dev, "%s char in pend=%d", __func__,
+		pdx->bReadCharsPending);
+
+	/* wait for io to stop */
+	mutex_lock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s got io_mutex", __func__);
+	ced_draw_down(pdx);
+
+	/* read out errors, leave subsequent opens a clean slate */
+	spin_lock_irq(&pdx->err_lock);
+	res = pdx->errors ? (pdx->errors == -EPIPE ? -EPIPE : -EIO) : 0;
+	pdx->errors = 0;
+	spin_unlock_irq(&pdx->err_lock);
+
+	mutex_unlock(&pdx->io_mutex);
+	dev_dbg(&pdx->interface->dev, "%s exit reached", __func__);
+
+	return res;
+}
+
+/***************************************************************************
+** CanAcceptIoRequests
+** If the device is removed, interface is set NULL. We also clear our pointer
+** from the interface, so we should make sure that pdx is not NULL. This will
+** not help with a device extension held by a file.
+** return true if can accept new io requests, else false
+*/
+static bool CanAcceptIoRequests(DEVICE_EXTENSION * pdx)
+{
+	return pdx && pdx->interface;	// Can we accept IO requests
+}
+
+/****************************************************************************
+** Callback routine to complete writes. This may need to fire off another
+** urb to complete the transfer.
+****************************************************************************/
+static void ced_writechar_callback(struct urb *pUrb)
+{
+	DEVICE_EXTENSION *pdx = pUrb->context;
+	int nGot = pUrb->actual_length;	// what we transferred
+
+	if (pUrb->status) {	// sync/async unlink faults aren't errors
+		if (!
+		    (pUrb->status == -ENOENT || pUrb->status == -ECONNRESET
+		     || pUrb->status == -ESHUTDOWN)) {
+			dev_err(&pdx->interface->dev,
+				"%s - nonzero write bulk status received: %d",
+				__func__, pUrb->status);
+		}
+
+		spin_lock(&pdx->err_lock);
+		pdx->errors = pUrb->status;
+		spin_unlock(&pdx->err_lock);
+		nGot = 0;	//  and tidy up again if so
+
+		spin_lock(&pdx->charOutLock);	// already at irq level
+		pdx->dwOutBuffGet = 0;	// Reset the output buffer
+		pdx->dwOutBuffPut = 0;
+		pdx->dwNumOutput = 0;	// Clear the char count
+		pdx->bPipeError[0] = 1;	// Flag an error for later
+		pdx->bSendCharsPending = false;	// Allow other threads again
+		spin_unlock(&pdx->charOutLock);	// already at irq level
+		dev_dbg(&pdx->interface->dev,
+			"%s - char out done, 0 chars sent", __func__);
+	} else {
+		dev_dbg(&pdx->interface->dev,
+			"%s - char out done, %d chars sent", __func__, nGot);
+		spin_lock(&pdx->charOutLock);	// already at irq level
+		pdx->dwNumOutput -= nGot;	// Now adjust the char send buffer
+		pdx->dwOutBuffGet += nGot;	// to match what we did
+		if (pdx->dwOutBuffGet >= OUTBUF_SZ)	// Can't do this any earlier as data could be overwritten
+			pdx->dwOutBuffGet = 0;
+
+		if (pdx->dwNumOutput > 0)	// if more to be done...
+		{
+			int nPipe = 0;	// The pipe number to use
+			int iReturn;
+			char *pDat = &pdx->outputBuffer[pdx->dwOutBuffGet];
+			unsigned int dwCount = pdx->dwNumOutput;	// maximum to send
+			if ((pdx->dwOutBuffGet + dwCount) > OUTBUF_SZ)	// does it cross buffer end?
+				dwCount = OUTBUF_SZ - pdx->dwOutBuffGet;
+			spin_unlock(&pdx->charOutLock);	// we are done with stuff that changes
+			memcpy(pdx->pCoherCharOut, pDat, dwCount);	// copy output data to the buffer
+			usb_fill_bulk_urb(pdx->pUrbCharOut, pdx->udev,
+					  usb_sndbulkpipe(pdx->udev,
+							  pdx->epAddr[0]),
+					  pdx->pCoherCharOut, dwCount,
+					  ced_writechar_callback, pdx);
+			pdx->pUrbCharOut->transfer_flags |=
+			    URB_NO_TRANSFER_DMA_MAP;
+			usb_anchor_urb(pdx->pUrbCharOut, &pdx->submitted);	// in case we need to kill it
+			iReturn = usb_submit_urb(pdx->pUrbCharOut, GFP_ATOMIC);
+			dev_dbg(&pdx->interface->dev, "%s n=%d>%s<", __func__,
+				dwCount, pDat);
+			spin_lock(&pdx->charOutLock);	// grab lock for errors
+			if (iReturn) {
+				pdx->bPipeError[nPipe] = 1;	// Flag an error to be handled later
+				pdx->bSendCharsPending = false;	// Allow other threads again
+				usb_unanchor_urb(pdx->pUrbCharOut);
+				dev_err(&pdx->interface->dev,
+					"%s usb_submit_urb() returned %d",
+					__func__, iReturn);
+			}
+		} else
+			pdx->bSendCharsPending = false;	// Allow other threads again
+		spin_unlock(&pdx->charOutLock);	// already at irq level
+	}
+}
+
+/****************************************************************************
+** SendChars
+** Transmit the characters in the output buffer to the 1401. This may need
+** breaking down into multiple transfers.
+****************************************************************************/
+int SendChars(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = U14ERR_NOERROR;
+
+	spin_lock_irq(&pdx->charOutLock);	// Protect ourselves
+
+	if ((!pdx->bSendCharsPending) &&	// Not currently sending
+	    (pdx->dwNumOutput > 0) &&	//  has characters to output
+	    (CanAcceptIoRequests(pdx)))	//  and current activity is OK
+	{
+		unsigned int dwCount = pdx->dwNumOutput;	// Get a copy of the character count
+		pdx->bSendCharsPending = true;	// Set flag to lock out other threads
+
+		dev_dbg(&pdx->interface->dev,
+			"Send %d chars to 1401, EP0 flag %d\n", dwCount,
+			pdx->nPipes == 3);
+		// If we have only 3 end points we must send the characters to the 1401 using EP0.
+		if (pdx->nPipes == 3) {
+			// For EP0 character transmissions to the 1401, we have to hang about until they
+			// are gone, as otherwise without more character IO activity they will never go.
+			unsigned int count = dwCount;	// Local char counter
+			unsigned int index = 0;	// The index into the char buffer
+
+			spin_unlock_irq(&pdx->charOutLock);	// Free spinlock as we call USBD
+
+			while ((count > 0) && (iReturn == U14ERR_NOERROR)) {
+				// We have to break the transfer up into 64-byte chunks because of a 2270 problem
+				int n = count > 64 ? 64 : count;	// Chars for this xfer, max of 64
+				int nSent = usb_control_msg(pdx->udev,
+							    usb_sndctrlpipe(pdx->udev, 0),	// use end point 0
+							    DB_CHARS,	// bRequest
+							    (H_TO_D | VENDOR | DEVREQ),	// to the device, vendor request to the device
+							    0, 0,	// value and index are both 0
+							    &pdx->outputBuffer[index],	// where to send from
+							    n,	// how much to send
+							    1000);	// timeout in jiffies
+				if (nSent <= 0) {
+					iReturn = nSent ? nSent : -ETIMEDOUT;	// if 0 chars says we timed out
+					dev_err(&pdx->interface->dev,
+						"Send %d chars by EP0 failed: %d",
+						n, iReturn);
+				} else {
+					dev_dbg(&pdx->interface->dev,
+						"Sent %d chars by EP0", n);
+					count -= nSent;
+					index += nSent;
+				}
+			}
+
+			spin_lock_irq(&pdx->charOutLock);	// Protect pdx changes, released by general code
+			pdx->dwOutBuffGet = 0;	// so reset the output buffer
+			pdx->dwOutBuffPut = 0;
+			pdx->dwNumOutput = 0;	// and clear the buffer count
+			pdx->bSendCharsPending = false;	// Allow other threads again
+		} else {	// Here for sending chars normally - we hold the spin lock
+			int nPipe = 0;	// The pipe number to use
+			char *pDat = &pdx->outputBuffer[pdx->dwOutBuffGet];
+
+			if ((pdx->dwOutBuffGet + dwCount) > OUTBUF_SZ)	// does it cross buffer end?
+				dwCount = OUTBUF_SZ - pdx->dwOutBuffGet;
+			spin_unlock_irq(&pdx->charOutLock);	// we are done with stuff that changes
+			memcpy(pdx->pCoherCharOut, pDat, dwCount);	// copy output data to the buffer
+			usb_fill_bulk_urb(pdx->pUrbCharOut, pdx->udev,
+					  usb_sndbulkpipe(pdx->udev,
+							  pdx->epAddr[0]),
+					  pdx->pCoherCharOut, dwCount,
+					  ced_writechar_callback, pdx);
+			pdx->pUrbCharOut->transfer_flags |=
+			    URB_NO_TRANSFER_DMA_MAP;
+			usb_anchor_urb(pdx->pUrbCharOut, &pdx->submitted);
+			iReturn = usb_submit_urb(pdx->pUrbCharOut, GFP_KERNEL);
+			spin_lock_irq(&pdx->charOutLock);	// grab lock for errors
+			if (iReturn) {
+				pdx->bPipeError[nPipe] = 1;	// Flag an error to be handled later
+				pdx->bSendCharsPending = false;	// Allow other threads again
+				usb_unanchor_urb(pdx->pUrbCharOut);	// remove from list of active urbs
+			}
+		}
+	} else if (pdx->bSendCharsPending && (pdx->dwNumOutput > 0))
+		dev_dbg(&pdx->interface->dev,
+			"SendChars bSendCharsPending:true");
+
+	dev_dbg(&pdx->interface->dev, "SendChars exit code: %d", iReturn);
+	spin_unlock_irq(&pdx->charOutLock);	// Now let go of the spinlock
+	return iReturn;
+}
+
+/***************************************************************************
+** CopyUserSpace
+** This moves memory between pinned down user space and the pCoherStagedIO
+** memory buffer we use for transfers. Copy n bytes in the directions that
+** is defined by pdx->StagedRead. The user space is determined by the area
+** in pdx->StagedId and the offset in pdx->StagedDone. The user
+** area may well not start on a page boundary, so allow for that.
+**
+** We have a table of physical pages that describe the area, so we can use
+** this to get a virtual address that the kernel can use.
+**
+** pdx  Is our device extension which holds all we know about the transfer.
+** n    The number of bytes to move one way or the other.
+***************************************************************************/
+static void CopyUserSpace(DEVICE_EXTENSION * pdx, int n)
+{
+	unsigned int nArea = pdx->StagedId;
+	if (nArea < MAX_TRANSAREAS) {
+		TRANSAREA *pArea = &pdx->rTransDef[nArea];	// area to be used
+		unsigned int dwOffset =
+		    pdx->StagedDone + pdx->StagedOffset + pArea->dwBaseOffset;
+		char *pCoherBuf = pdx->pCoherStagedIO;	// coherent buffer
+		if (!pArea->bUsed) {
+			dev_err(&pdx->interface->dev, "%s area %d unused",
+				__func__, nArea);
+			return;
+		}
+
+		while (n) {
+			int nPage = dwOffset >> PAGE_SHIFT;	// page number in table
+			if (nPage < pArea->nPages) {
+				char *pvAddress =
+				    (char *)kmap_atomic(pArea->pPages[nPage]);
+				if (pvAddress) {
+					unsigned int uiPageOff = dwOffset & (PAGE_SIZE - 1);	// offset into the page
+					size_t uiXfer = PAGE_SIZE - uiPageOff;	// max to transfer on this page
+					if (uiXfer > n)	// limit byte count if too much
+						uiXfer = n;	// for the page
+					if (pdx->StagedRead)
+						memcpy(pvAddress + uiPageOff,
+						       pCoherBuf, uiXfer);
+					else
+						memcpy(pCoherBuf,
+						       pvAddress + uiPageOff,
+						       uiXfer);
+					kunmap_atomic(pvAddress);
+					dwOffset += uiXfer;
+					pCoherBuf += uiXfer;
+					n -= uiXfer;
+				} else {
+					dev_err(&pdx->interface->dev,
+						"%s did not map page %d",
+						__func__, nPage);
+					return;
+				}
+
+			} else {
+				dev_err(&pdx->interface->dev,
+					"%s exceeded pages %d", __func__,
+					nPage);
+				return;
+			}
+		}
+	} else
+		dev_err(&pdx->interface->dev, "%s bad area %d", __func__,
+			nArea);
+}
+
+// Forward declarations for stuff used circularly
+static int StageChunk(DEVICE_EXTENSION * pdx);
+/***************************************************************************
+** ReadWrite_Complete
+**
+**  Completion routine for our staged read/write Irps
+*/
+static void staged_callback(struct urb *pUrb)
+{
+	DEVICE_EXTENSION *pdx = pUrb->context;
+	unsigned int nGot = pUrb->actual_length;	// what we transferred
+	bool bCancel = false;
+	bool bRestartCharInput;	// used at the end
+
+	spin_lock(&pdx->stagedLock);	// stop ReadWriteMem() action while this routine is running
+	pdx->bStagedUrbPending = false;	// clear the flag for staged IRP pending
+
+	if (pUrb->status) {	// sync/async unlink faults aren't errors
+		if (!
+		    (pUrb->status == -ENOENT || pUrb->status == -ECONNRESET
+		     || pUrb->status == -ESHUTDOWN)) {
+			dev_err(&pdx->interface->dev,
+				"%s - nonzero write bulk status received: %d",
+				__func__, pUrb->status);
+		} else
+			dev_info(&pdx->interface->dev,
+				 "%s - staged xfer cancelled", __func__);
+
+		spin_lock(&pdx->err_lock);
+		pdx->errors = pUrb->status;
+		spin_unlock(&pdx->err_lock);
+		nGot = 0;	//  and tidy up again if so
+		bCancel = true;
+	} else {
+		dev_dbg(&pdx->interface->dev, "%s %d chars xferred", __func__,
+			nGot);
+		if (pdx->StagedRead)	// if reading, save to user space
+			CopyUserSpace(pdx, nGot);	// copy from buffer to user
+		if (nGot == 0)
+			dev_dbg(&pdx->interface->dev, "%s ZLP", __func__);
+	}
+
+	// Update the transfer length based on the TransferBufferLength value in the URB
+	pdx->StagedDone += nGot;
+
+	dev_dbg(&pdx->interface->dev, "%s, done %d bytes of %d", __func__,
+		pdx->StagedDone, pdx->StagedLength);
+
+	if ((pdx->StagedDone == pdx->StagedLength) ||	// If no more to do
+	    (bCancel))		// or this IRP was cancelled
+	{
+		TRANSAREA *pArea = &pdx->rTransDef[pdx->StagedId];	// Transfer area info
+		dev_dbg(&pdx->interface->dev,
+			"%s transfer done, bytes %d, cancel %d", __func__,
+			pdx->StagedDone, bCancel);
+
+		// Here is where we sort out what to do with this transfer if using a circular buffer. We have
+		//  a completed transfer that can be assumed to fit into the transfer area. We should be able to
+		//  add this to the end of a growing block or to use it to start a new block unless the code
+		//  that calculates the offset to use (in ReadWriteMem) is totally duff.
+		if ((pArea->bCircular) && (pArea->bCircToHost) && (!bCancel) &&	// Time to sort out circular buffer info?
+		    (pdx->StagedRead))	// Only for tohost transfers for now
+		{
+			if (pArea->aBlocks[1].dwSize > 0)	// If block 1 is in use we must append to it
+			{
+				if (pdx->StagedOffset ==
+				    (pArea->aBlocks[1].dwOffset +
+				     pArea->aBlocks[1].dwSize)) {
+					pArea->aBlocks[1].dwSize +=
+					    pdx->StagedLength;
+					dev_dbg(&pdx->interface->dev,
+						"RWM_Complete, circ block 1 now %d bytes at %d",
+						pArea->aBlocks[1].dwSize,
+						pArea->aBlocks[1].dwOffset);
+				} else {
+					// Here things have gone very, very, wrong, but I cannot see how this can actually be achieved
+					pArea->aBlocks[1].dwOffset =
+					    pdx->StagedOffset;
+					pArea->aBlocks[1].dwSize =
+					    pdx->StagedLength;
+					dev_err(&pdx->interface->dev,
+						"%s ERROR, circ block 1 re-started %d bytes at %d",
+						__func__,
+						pArea->aBlocks[1].dwSize,
+						pArea->aBlocks[1].dwOffset);
+				}
+			} else	// If block 1 is not used, we try to add to block 0
+			{
+				if (pArea->aBlocks[0].dwSize > 0)	// Got stored block 0 information?
+				{	// Must append onto the existing block 0
+					if (pdx->StagedOffset ==
+					    (pArea->aBlocks[0].dwOffset +
+					     pArea->aBlocks[0].dwSize)) {
+						pArea->aBlocks[0].dwSize += pdx->StagedLength;	// Just add this transfer in
+						dev_dbg(&pdx->interface->dev,
+							"RWM_Complete, circ block 0 now %d bytes at %d",
+							pArea->aBlocks[0].
+							dwSize,
+							pArea->aBlocks[0].
+							dwOffset);
+					} else	// If it doesn't append, put into new block 1
+					{
+						pArea->aBlocks[1].dwOffset =
+						    pdx->StagedOffset;
+						pArea->aBlocks[1].dwSize =
+						    pdx->StagedLength;
+						dev_dbg(&pdx->interface->dev,
+							"RWM_Complete, circ block 1 started %d bytes at %d",
+							pArea->aBlocks[1].
+							dwSize,
+							pArea->aBlocks[1].
+							dwOffset);
+					}
+				} else	// No info stored yet, just save in block 0
+				{
+					pArea->aBlocks[0].dwOffset =
+					    pdx->StagedOffset;
+					pArea->aBlocks[0].dwSize =
+					    pdx->StagedLength;
+					dev_dbg(&pdx->interface->dev,
+						"RWM_Complete, circ block 0 started %d bytes at %d",
+						pArea->aBlocks[0].dwSize,
+						pArea->aBlocks[0].dwOffset);
+				}
+			}
+		}
+
+		if (!bCancel)	// Don't generate an event if cancelled
+		{
+			dev_dbg(&pdx->interface->dev,
+				"RWM_Complete,  bCircular %d, bToHost %d, eStart %d, eSize %d",
+				pArea->bCircular, pArea->bEventToHost,
+				pArea->dwEventSt, pArea->dwEventSz);
+			if ((pArea->dwEventSz) &&	// Set a user-mode event...
+			    (pdx->StagedRead == pArea->bEventToHost))	// ...on transfers in this direction?
+			{
+				int iWakeUp = 0;	// assume
+				// If we have completed the right sort of DMA transfer then set the event to notify
+				//   the user code to wake up anyone that is waiting.
+				if ((pArea->bCircular) &&	// Circular areas use a simpler test
+				    (pArea->bCircToHost))	// only in supported direction
+				{	// Is total data waiting up to size limit?
+					unsigned int dwTotal =
+					    pArea->aBlocks[0].dwSize +
+					    pArea->aBlocks[1].dwSize;
+					iWakeUp = (dwTotal >= pArea->dwEventSz);
+				} else {
+					unsigned int transEnd =
+					    pdx->StagedOffset +
+					    pdx->StagedLength;
+					unsigned int eventEnd =
+					    pArea->dwEventSt + pArea->dwEventSz;
+					iWakeUp = (pdx->StagedOffset < eventEnd)
+					    && (transEnd > pArea->dwEventSt);
+				}
+
+				if (iWakeUp) {
+					dev_dbg(&pdx->interface->dev,
+						"About to set event to notify app");
+					wake_up_interruptible(&pArea->wqEvent);	// wake up waiting processes
+					++pArea->iWakeUp;	// increment wakeup count
+				}
+			}
+		}
+
+		pdx->dwDMAFlag = MODE_CHAR;	// Switch back to char mode before ReadWriteMem call
+
+		if (!bCancel)	// Don't look for waiting transfer if cancelled
+		{
+			// If we have a transfer waiting, kick it off
+			if (pdx->bXFerWaiting)	// Got a block xfer waiting?
+			{
+				int iReturn;
+				dev_info(&pdx->interface->dev,
+					 "*** RWM_Complete *** pending transfer will now be set up!!!");
+				iReturn =
+				    ReadWriteMem(pdx, !pdx->rDMAInfo.bOutWard,
+						 pdx->rDMAInfo.wIdent,
+						 pdx->rDMAInfo.dwOffset,
+						 pdx->rDMAInfo.dwSize);
+
+				if (iReturn)
+					dev_err(&pdx->interface->dev,
+						"RWM_Complete rw setup failed %d",
+						iReturn);
+			}
+		}
+
+	} else			// Here for more to do
+		StageChunk(pdx);	// fire off the next bit
+
+	// While we hold the stagedLock, see if we should reallow character input ints
+	// Don't allow if cancelled, or if a new block has started or if there is a waiting block.
+	// This feels wrong as we should ask which spin lock protects dwDMAFlag.
+	bRestartCharInput = !bCancel && (pdx->dwDMAFlag == MODE_CHAR)
+	    && !pdx->bXFerWaiting;
+
+	spin_unlock(&pdx->stagedLock);	// Finally release the lock again
+
+	// This is not correct as dwDMAFlag is protected by the staged lock, but it is treated
+	// in Allowi as if it were protected by the char lock. In any case, most systems will
+	// not be upset by char input during DMA... sigh. Needs sorting out.
+	if (bRestartCharInput)	// may be out of date, but...
+		Allowi(pdx, true);	// ...Allowi tests a lock too.
+	dev_dbg(&pdx->interface->dev, "%s done", __func__);
+}
+
+/****************************************************************************
+** StageChunk
+**
+** Generates the next chunk of data making up a staged transfer.
+**
+** The calling code must have acquired the staging spinlock before calling
+**  this function, and is responsible for releasing it. We are at callback level.
+****************************************************************************/
+static int StageChunk(DEVICE_EXTENSION * pdx)
+{
+	int iReturn = U14ERR_NOERROR;
+	unsigned int ChunkSize;
+	int nPipe = pdx->StagedRead ? 3 : 2;	// The pipe number to use for reads or writes
+	if (pdx->nPipes == 3)
+		nPipe--;	// Adjust for the 3-pipe case
+	if (nPipe < 0)		// and trap case that should never happen
+		return U14ERR_FAIL;
+
+	if (!CanAcceptIoRequests(pdx))	// got sudden remove?
+	{
+		dev_info(&pdx->interface->dev, "%s sudden remove, giving up",
+			 __func__);
+		return U14ERR_FAIL;	// could do with a better error
+	}
+
+	ChunkSize = (pdx->StagedLength - pdx->StagedDone);	// transfer length remaining
+	if (ChunkSize > STAGED_SZ)	// make sure to keep legal
+		ChunkSize = STAGED_SZ;	//  limit to max allowed
+
+	if (!pdx->StagedRead)	// if writing...
+		CopyUserSpace(pdx, ChunkSize);	// ...copy data into the buffer
+
+	usb_fill_bulk_urb(pdx->pStagedUrb, pdx->udev,
+			  pdx->StagedRead ? usb_rcvbulkpipe(pdx->udev,
+							    pdx->
+							    epAddr[nPipe]) :
+			  usb_sndbulkpipe(pdx->udev, pdx->epAddr[nPipe]),
+			  pdx->pCoherStagedIO, ChunkSize, staged_callback, pdx);
+	pdx->pStagedUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	usb_anchor_urb(pdx->pStagedUrb, &pdx->submitted);	// in case we need to kill it
+	iReturn = usb_submit_urb(pdx->pStagedUrb, GFP_ATOMIC);
+	if (iReturn) {
+		usb_unanchor_urb(pdx->pStagedUrb);	// kill it
+		pdx->bPipeError[nPipe] = 1;	// Flag an error to be handled later
+		dev_err(&pdx->interface->dev, "%s submit urb failed, code %d",
+			__func__, iReturn);
+	} else
+		pdx->bStagedUrbPending = true;	// Set the flag for staged URB pending
+	dev_dbg(&pdx->interface->dev, "%s done so far:%d, this size:%d",
+		__func__, pdx->StagedDone, ChunkSize);
+
+	return iReturn;
+}
+
+/***************************************************************************
+** ReadWriteMem
+**
+** This routine is used generally for block read and write operations.
+** Breaks up a read or write in to specified sized chunks, as specified by pipe
+** information on maximum transfer size.
+**
+** Any code that calls this must be holding the stagedLock
+**
+** Arguments:
+**    DeviceObject - pointer to our FDO (Functional Device Object)
+**    Read - TRUE for read, FALSE for write. This is from POV of the driver
+**    wIdent - the transfer area number - defines memory area and more.
+**    dwOffs - the start offset within the transfer area of the start of this
+**             transfer.
+**    dwLen - the number of bytes to transfer.
+*/
+int ReadWriteMem(DEVICE_EXTENSION * pdx, bool Read, unsigned short wIdent,
+		 unsigned int dwOffs, unsigned int dwLen)
+{
+	TRANSAREA *pArea = &pdx->rTransDef[wIdent];	// Transfer area info
+
+	if (!CanAcceptIoRequests(pdx))	// Are we in a state to accept new requests?
+	{
+		dev_err(&pdx->interface->dev, "%s can't accept requests",
+			__func__);
+		return U14ERR_FAIL;
+	}
+
+	dev_dbg(&pdx->interface->dev,
+		"%s xfer %d bytes to %s, offset %d, area %d", __func__, dwLen,
+		Read ? "host" : "1401", dwOffs, wIdent);
+
+	// Amazingly, we can get an escape sequence back before the current staged Urb is done, so we
+	//  have to check for this situation and, if so, wait until all is OK.
+	if (pdx->bStagedUrbPending) {
+		pdx->bXFerWaiting = true;	// Flag we are waiting
+		dev_info(&pdx->interface->dev,
+			 "%s xfer is waiting, as previous staged pending",
+			 __func__);
+		return U14ERR_NOERROR;
+	}
+
+	if (dwLen == 0)		// allow 0-len read or write; just return success
+	{
+		dev_dbg(&pdx->interface->dev,
+			"%s OK; zero-len read/write request", __func__);
+		return U14ERR_NOERROR;
+	}
+
+	if ((pArea->bCircular) &&	// Circular transfer?
+	    (pArea->bCircToHost) && (Read))	// In a supported direction
+	{			// If so, we sort out offset ourself
+		bool bWait = false;	// Flag for transfer having to wait
+
+		dev_dbg(&pdx->interface->dev,
+			"Circular buffers are %d at %d and %d at %d",
+			pArea->aBlocks[0].dwSize, pArea->aBlocks[0].dwOffset,
+			pArea->aBlocks[1].dwSize, pArea->aBlocks[1].dwOffset);
+		if (pArea->aBlocks[1].dwSize > 0)	// Using the second block already?
+		{
+			dwOffs = pArea->aBlocks[1].dwOffset + pArea->aBlocks[1].dwSize;	// take offset from that
+			bWait = (dwOffs + dwLen) > pArea->aBlocks[0].dwOffset;	// Wait if will overwrite block 0?
+			bWait |= (dwOffs + dwLen) > pArea->dwLength;	// or if it overflows the buffer
+		} else		// Area 1 not in use, try to use area 0
+		{
+			if (pArea->aBlocks[0].dwSize == 0)	// Reset block 0 if not in use
+				pArea->aBlocks[0].dwOffset = 0;
+			dwOffs =
+			    pArea->aBlocks[0].dwOffset +
+			    pArea->aBlocks[0].dwSize;
+			if ((dwOffs + dwLen) > pArea->dwLength)	// Off the end of the buffer?
+			{
+				pArea->aBlocks[1].dwOffset = 0;	// Set up to use second block
+				dwOffs = 0;
+				bWait = (dwOffs + dwLen) > pArea->aBlocks[0].dwOffset;	// Wait if will overwrite block 0?
+				bWait |= (dwOffs + dwLen) > pArea->dwLength;	// or if it overflows the buffer
+			}
+		}
+
+		if (bWait)	// This transfer will have to wait?
+		{
+			pdx->bXFerWaiting = true;	// Flag we are waiting
+			dev_dbg(&pdx->interface->dev,
+				"%s xfer waiting for circular buffer space",
+				__func__);
+			return U14ERR_NOERROR;
+		}
+
+		dev_dbg(&pdx->interface->dev,
+			"%s circular xfer, %d bytes starting at %d", __func__,
+			dwLen, dwOffs);
+	}
+	// Save the parameters for the read\write transfer
+	pdx->StagedRead = Read;	// Save the parameters for this read
+	pdx->StagedId = wIdent;	// ID allows us to get transfer area info
+	pdx->StagedOffset = dwOffs;	// The area within the transfer area
+	pdx->StagedLength = dwLen;
+	pdx->StagedDone = 0;	// Initialise the byte count
+	pdx->dwDMAFlag = MODE_LINEAR;	// Set DMA mode flag at this point
+	pdx->bXFerWaiting = false;	// Clearly not a transfer waiting now
+
+//    KeClearEvent(&pdx->StagingDoneEvent);           // Clear the transfer done event
+	StageChunk(pdx);	// fire off the first chunk
+
+	return U14ERR_NOERROR;
+}
+
+/****************************************************************************
+**
+** ReadChar
+**
+** Reads a character a buffer. If there is no more
+**  data we return FALSE. Used as part of decoding a DMA request.
+**
+****************************************************************************/
+static bool ReadChar(unsigned char *pChar, char *pBuf, unsigned int *pdDone,
+		     unsigned int dGot)
+{
+	bool bRead = false;
+	unsigned int dDone = *pdDone;
+
+	if (dDone < dGot)	// If there is more data
+	{
+		*pChar = (unsigned char)pBuf[dDone];	// Extract the next char
+		dDone++;	// Increment the done count
+		*pdDone = dDone;
+		bRead = true;	// and flag success
+	}
+
+	return bRead;
+}
+
+#ifdef NOTUSED
+/****************************************************************************
+**
+** ReadWord
+**
+** Reads a word from the 1401, just uses ReadChar twice; passes on any error
+**
+*****************************************************************************/
+static bool ReadWord(unsigned short *pWord, char *pBuf, unsigned int *pdDone,
+		     unsigned int dGot)
+{
+	if (ReadChar((unsigned char *)pWord, pBuf, pdDone, dGot))
+		return ReadChar(((unsigned char *)pWord) + 1, pBuf, pdDone,
+				dGot);
+	else
+		return false;
+}
+#endif
+
+/****************************************************************************
+** ReadHuff
+**
+** Reads a coded number in and returns it, Code is:
+** If data is in range 0..127 we recieve 1 byte. If data in range 128-16383
+** we recieve two bytes, top bit of first indicates another on its way. If
+** data in range 16383-4194303 we get three bytes, top two bits of first set
+** to indicate three byte total.
+**
+*****************************************************************************/
+static bool ReadHuff(volatile unsigned int *pDWord, char *pBuf,
+		     unsigned int *pdDone, unsigned int dGot)
+{
+	unsigned char ucData;	/* for each read to ReadChar */
+	bool bReturn = true;	/* assume we will succeed */
+	unsigned int dwData = 0;	/* Accumulator for the data */
+
+	if (ReadChar(&ucData, pBuf, pdDone, dGot)) {
+		dwData = ucData;	/* copy the data */
+		if ((dwData & 0x00000080) != 0) {	/* Bit set for more data ? */
+			dwData &= 0x0000007F;	/* Clear the relevant bit */
+			if (ReadChar(&ucData, pBuf, pdDone, dGot)) {
+				dwData = (dwData << 8) | ucData;
+				if ((dwData & 0x00004000) != 0) {	/* three byte sequence ? */
+					dwData &= 0x00003FFF;	/* Clear the relevant bit */
+					if (ReadChar
+					    (&ucData, pBuf, pdDone, dGot))
+						dwData = (dwData << 8) | ucData;
+					else
+						bReturn = false;
+				}
+			} else
+				bReturn = false;	/* couldn't read data */
+		}
+	} else
+		bReturn = false;
+
+	*pDWord = dwData;	/* return the data */
+	return bReturn;
+}
+
+/***************************************************************************
+**
+** ReadDMAInfo
+**
+** Tries to read info about the dma request from the 1401 and decode it into
+** the dma descriptor block. We have at this point had the escape character
+** from the 1401 and now we must read in the rest of the information about
+** the transfer request. Returns FALSE if 1401 fails to respond or obselete
+** code from 1401 or bad parameters.
+**
+** The pBuf char pointer does not include the initial escape character, so
+**  we start handling the data at offset zero.
+**
+*****************************************************************************/
+static bool ReadDMAInfo(volatile DMADESC * pDmaDesc, DEVICE_EXTENSION * pdx,
+			char *pBuf, unsigned int dwCount)
+{
+	bool bResult = false;	// assume we won't succeed
+	unsigned char ucData;
+	unsigned int dDone = 0;	// We haven't parsed anything so far
+
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	if (ReadChar(&ucData, pBuf, &dDone, dwCount)) {
+		unsigned char ucTransCode = (ucData & 0x0F);	// get code for transfer type
+		unsigned short wIdent = ((ucData >> 4) & 0x07);	// and area identifier
+
+		// fill in the structure we were given
+		pDmaDesc->wTransType = ucTransCode;	// type of transfer
+		pDmaDesc->wIdent = wIdent;	// area to use
+		pDmaDesc->dwSize = 0;	// initialise other bits
+		pDmaDesc->dwOffset = 0;
+
+		dev_dbg(&pdx->interface->dev, "%s type: %d ident: %d", __func__,
+			pDmaDesc->wTransType, pDmaDesc->wIdent);
+
+		pDmaDesc->bOutWard = (ucTransCode != TM_EXTTOHOST);	// set transfer direction
+
+		switch (ucTransCode) {
+		case TM_EXTTOHOST:	// Extended linear transfer modes (the only ones!)
+		case TM_EXTTO1401:
+			{
+				bResult =
+				    ReadHuff(&(pDmaDesc->dwOffset), pBuf,
+					     &dDone, dwCount)
+				    && ReadHuff(&(pDmaDesc->dwSize), pBuf,
+						&dDone, dwCount);
+				if (bResult) {
+					dev_dbg(&pdx->interface->dev,
+						"%s xfer offset & size %d %d",
+						__func__, pDmaDesc->dwOffset,
+						pDmaDesc->dwSize);
+
+					if ((wIdent >= MAX_TRANSAREAS) ||	// Illegal area number, or...
+					    (!pdx->rTransDef[wIdent].bUsed) ||	// area not set up, or...
+					    (pDmaDesc->dwOffset > pdx->rTransDef[wIdent].dwLength) ||	// range/size
+					    ((pDmaDesc->dwOffset +
+					      pDmaDesc->dwSize) >
+					     (pdx->rTransDef[wIdent].
+					      dwLength))) {
+						bResult = false;	// bad parameter(s)
+						dev_dbg(&pdx->interface->dev,
+							"%s bad param - id %d, bUsed %d, offset %d, size %d, area length %d",
+							__func__, wIdent,
+							pdx->rTransDef[wIdent].
+							bUsed,
+							pDmaDesc->dwOffset,
+							pDmaDesc->dwSize,
+							pdx->rTransDef[wIdent].
+							dwLength);
+					}
+				}
+				break;
+			}
+		default:
+			break;
+		}
+	} else
+		bResult = false;
+
+	if (!bResult)		// now check parameters for validity
+		dev_err(&pdx->interface->dev, "%s error reading Esc sequence",
+			__func__);
+
+	return bResult;
+}
+
+/****************************************************************************
+**
+** Handle1401Esc
+**
+** Deals with an escape sequence coming from the 1401. This can either be
+**  a DMA transfer request of various types or a response to an escape sequence
+**  sent to the 1401. This is called from a callback.
+**
+** Parameters are
+**
+** dwCount - the number of characters in the device extension char in buffer,
+**           this is known to be at least 2 or we will not be called.
+**
+****************************************************************************/
+static int Handle1401Esc(DEVICE_EXTENSION * pdx, char *pCh,
+			 unsigned int dwCount)
+{
+	int iReturn = U14ERR_FAIL;
+
+	// I have no idea what this next test is about. '?' is 0x3f, which is area 3, code
+	// 15. At the moment, this is not used, so it does no harm, but unless someone can
+	// tell me what this is for, it should be removed from this and the Windows driver.
+	if (pCh[0] == '?')	// Is this an information response
+	{			// Parse and save the information
+	} else {
+		spin_lock(&pdx->stagedLock);	// Lock others out
+
+		if (ReadDMAInfo(&pdx->rDMAInfo, pdx, pCh, dwCount))	// Get DMA parameters
+		{
+			unsigned short wTransType = pdx->rDMAInfo.wTransType;	// check transfer type
+
+			dev_dbg(&pdx->interface->dev,
+				"%s xfer to %s, offset %d, length %d", __func__,
+				pdx->rDMAInfo.bOutWard ? "1401" : "host",
+				pdx->rDMAInfo.dwOffset, pdx->rDMAInfo.dwSize);
+
+			if (pdx->bXFerWaiting)	// Check here for badly out of kilter...
+			{	// This can never happen, really
+				dev_err(&pdx->interface->dev,
+					"ERROR: DMA setup while transfer still waiting");
+				spin_unlock(&pdx->stagedLock);
+			} else {
+				if ((wTransType == TM_EXTTOHOST)
+				    || (wTransType == TM_EXTTO1401)) {
+					iReturn =
+					    ReadWriteMem(pdx,
+							 !pdx->rDMAInfo.
+							 bOutWard,
+							 pdx->rDMAInfo.wIdent,
+							 pdx->rDMAInfo.dwOffset,
+							 pdx->rDMAInfo.dwSize);
+					if (iReturn != U14ERR_NOERROR)
+						dev_err(&pdx->interface->dev,
+							"%s ReadWriteMem() failed %d",
+							__func__, iReturn);
+				} else	// This covers non-linear transfer setup
+					dev_err(&pdx->interface->dev,
+						"%s Unknown block xfer type %d",
+						__func__, wTransType);
+			}
+		} else		// Failed to read parameters
+			dev_err(&pdx->interface->dev, "%s ReadDMAInfo() fail",
+				__func__);
+
+		spin_unlock(&pdx->stagedLock);	// OK here
+	}
+
+	dev_dbg(&pdx->interface->dev, "%s returns %d", __func__, iReturn);
+
+	return iReturn;
+}
+
+/****************************************************************************
+** Callback for the character read complete or error
+****************************************************************************/
+static void ced_readchar_callback(struct urb *pUrb)
+{
+	DEVICE_EXTENSION *pdx = pUrb->context;
+	int nGot = pUrb->actual_length;	// what we transferred
+
+	if (pUrb->status)	// Do we have a problem to handle?
+	{
+		int nPipe = pdx->nPipes == 4 ? 1 : 0;	// The pipe number to use for error
+		// sync/async unlink faults aren't errors... just saying device removed or stopped
+		if (!
+		    (pUrb->status == -ENOENT || pUrb->status == -ECONNRESET
+		     || pUrb->status == -ESHUTDOWN)) {
+			dev_err(&pdx->interface->dev,
+				"%s - nonzero write bulk status received: %d",
+				__func__, pUrb->status);
+		} else
+			dev_dbg(&pdx->interface->dev,
+				"%s - 0 chars pUrb->status=%d (shutdown?)",
+				__func__, pUrb->status);
+
+		spin_lock(&pdx->err_lock);
+		pdx->errors = pUrb->status;
+		spin_unlock(&pdx->err_lock);
+		nGot = 0;	//  and tidy up again if so
+
+		spin_lock(&pdx->charInLock);	// already at irq level
+		pdx->bPipeError[nPipe] = 1;	// Flag an error for later
+	} else {
+		if ((nGot > 1) && ((pdx->pCoherCharIn[0] & 0x7f) == 0x1b))	// Esc sequence?
+		{
+			Handle1401Esc(pdx, &pdx->pCoherCharIn[1], nGot - 1);	// handle it
+			spin_lock(&pdx->charInLock);	// already at irq level
+		} else {
+			spin_lock(&pdx->charInLock);	// already at irq level
+			if (nGot > 0) {
+				unsigned int i;
+				if (nGot < INBUF_SZ) {
+					pdx->pCoherCharIn[nGot] = 0;	// tidy the string
+					dev_dbg(&pdx->interface->dev,
+						"%s got %d chars >%s<",
+						__func__, nGot,
+						pdx->pCoherCharIn);
+				}
+				// We know that whatever we read must fit in the input buffer
+				for (i = 0; i < nGot; i++) {
+					pdx->inputBuffer[pdx->dwInBuffPut++] =
+					    pdx->pCoherCharIn[i] & 0x7F;
+					if (pdx->dwInBuffPut >= INBUF_SZ)
+						pdx->dwInBuffPut = 0;
+				}
+
+				if ((pdx->dwNumInput + nGot) <= INBUF_SZ)
+					pdx->dwNumInput += nGot;	// Adjust the buffer count accordingly
+			} else
+				dev_dbg(&pdx->interface->dev, "%s read ZLP",
+					__func__);
+		}
+	}
+
+	pdx->bReadCharsPending = false;	// No longer have a pending read
+	spin_unlock(&pdx->charInLock);	// already at irq level
+
+	Allowi(pdx, true);	// see if we can do the next one
+}
+
+/****************************************************************************
+** Allowi
+**
+** This is used to make sure that there is always a pending input transfer so
+** we can pick up any inward transfers. This can be called in multiple contexts
+** so we use the irqsave version of the spinlock.
+****************************************************************************/
+int Allowi(DEVICE_EXTENSION * pdx, bool bInCallback)
+{
+	int iReturn = U14ERR_NOERROR;
+	unsigned long flags;
+	spin_lock_irqsave(&pdx->charInLock, flags);	// can be called in multiple contexts
+
+	// We don't want char input running while DMA is in progress as we know that this
+	//  can cause sequencing problems for the 2270. So don't. It will also allow the
+	//  ERR response to get back to the host code too early on some PCs, even if there
+	//  is no actual driver failure, so we don't allow this at all.
+	if (!pdx->bInDrawDown &&	// stop input if
+	    !pdx->bReadCharsPending &&	// If no read request outstanding
+	    (pdx->dwNumInput < (INBUF_SZ / 2)) &&	//  and there is some space
+	    (pdx->dwDMAFlag == MODE_CHAR) &&	//  not doing any DMA
+	    (!pdx->bXFerWaiting) &&	//  no xfer waiting to start
+	    (CanAcceptIoRequests(pdx)))	//  and activity is generally OK
+	{			//  then off we go
+		unsigned int nMax = INBUF_SZ - pdx->dwNumInput;	// max we could read
+		int nPipe = pdx->nPipes == 4 ? 1 : 0;	// The pipe number to use
+
+		dev_dbg(&pdx->interface->dev, "%s %d chars in input buffer",
+			__func__, pdx->dwNumInput);
+
+		usb_fill_int_urb(pdx->pUrbCharIn, pdx->udev,
+				 usb_rcvintpipe(pdx->udev, pdx->epAddr[nPipe]),
+				 pdx->pCoherCharIn, nMax, ced_readchar_callback,
+				 pdx, pdx->bInterval);
+		pdx->pUrbCharIn->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;	// short xfers are OK by default
+		usb_anchor_urb(pdx->pUrbCharIn, &pdx->submitted);	// in case we need to kill it
+		iReturn =
+		    usb_submit_urb(pdx->pUrbCharIn,
+				   bInCallback ? GFP_ATOMIC : GFP_KERNEL);
+		if (iReturn) {
+			usb_unanchor_urb(pdx->pUrbCharIn);	// remove from list of active Urbs
+			pdx->bPipeError[nPipe] = 1;	// Flag an error to be handled later
+			dev_err(&pdx->interface->dev,
+				"%s submit urb failed: %d", __func__, iReturn);
+		} else
+			pdx->bReadCharsPending = true;	// Flag that we are active here
+	}
+
+	spin_unlock_irqrestore(&pdx->charInLock, flags);
+
+	return iReturn;
+
+}
+
+/*****************************************************************************
+** The ioctl entry point to the driver that is used by us to talk to it.
+** inode    The device node (no longer in 3.0.0 kernels)
+** file     The file that is open, which holds our pdx pointer
+** ulArg    The argument passed in. Note that long is 64-bits in 64-bit system, i.e. it is big
+**          enough for a 64-bit pointer.
+*****************************************************************************/
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+static long ced_ioctl(struct file *file, unsigned int cmd, unsigned long ulArg)
+#else
+static int ced_ioctl(struct inode *node, struct file *file, unsigned int cmd,
+		     unsigned long ulArg)
+#endif
+{
+	int err = 0;
+	DEVICE_EXTENSION *pdx = file->private_data;
+	if (!CanAcceptIoRequests(pdx))	// check we still exist
+		return -ENODEV;
+
+	// Check that access is allowed, where is is needed. Anything that would have an indeterminate
+	// size will be checked by the specific command.
+	if (_IOC_DIR(cmd) & _IOC_READ)	// read from point of view of user...
+		err = !access_ok(VERIFY_WRITE, (void __user *)ulArg, _IOC_SIZE(cmd));	// is kernel write
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)	// and write from point of view of user...
+		err = !access_ok(VERIFY_READ, (void __user *)ulArg, _IOC_SIZE(cmd));	// is kernel read
+	if (err)
+		return -EFAULT;
+
+	switch (_IOC_NR(cmd)) {
+	case _IOC_NR(IOCTL_CED_SENDSTRING(0)):
+		return SendString(pdx, (const char __user *)ulArg,
+				  _IOC_SIZE(cmd));
+
+	case _IOC_NR(IOCTL_CED_RESET1401):
+		return Reset1401(pdx);
+
+	case _IOC_NR(IOCTL_CED_GETCHAR):
+		return GetChar(pdx);
+
+	case _IOC_NR(IOCTL_CED_SENDCHAR):
+		return SendChar(pdx, (char)ulArg);
+
+	case _IOC_NR(IOCTL_CED_STAT1401):
+		return Stat1401(pdx);
+
+	case _IOC_NR(IOCTL_CED_LINECOUNT):
+		return LineCount(pdx);
+
+	case _IOC_NR(IOCTL_CED_GETSTRING(0)):
+		return GetString(pdx, (char __user *)ulArg, _IOC_SIZE(cmd));
+
+	case _IOC_NR(IOCTL_CED_SETTRANSFER):
+		return SetTransfer(pdx, (TRANSFERDESC __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_UNSETTRANSFER):
+		return UnsetTransfer(pdx, (int)ulArg);
+
+	case _IOC_NR(IOCTL_CED_SETEVENT):
+		return SetEvent(pdx, (TRANSFEREVENT __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_GETOUTBUFSPACE):
+		return GetOutBufSpace(pdx);
+
+	case _IOC_NR(IOCTL_CED_GETBASEADDRESS):
+		return -1;
+
+	case _IOC_NR(IOCTL_CED_GETDRIVERREVISION):
+		return (2 << 24) | (DRIVERMAJREV << 16) | DRIVERMINREV;	// USB | MAJOR | MINOR
+
+	case _IOC_NR(IOCTL_CED_GETTRANSFER):
+		return GetTransfer(pdx, (TGET_TX_BLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_KILLIO1401):
+		return KillIO1401(pdx);
+
+	case _IOC_NR(IOCTL_CED_STATEOF1401):
+		return StateOf1401(pdx);
+
+	case _IOC_NR(IOCTL_CED_GRAB1401):
+	case _IOC_NR(IOCTL_CED_FREE1401):
+		return U14ERR_NOERROR;
+
+	case _IOC_NR(IOCTL_CED_STARTSELFTEST):
+		return StartSelfTest(pdx);
+
+	case _IOC_NR(IOCTL_CED_CHECKSELFTEST):
+		return CheckSelfTest(pdx, (TGET_SELFTEST __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_TYPEOF1401):
+		return TypeOf1401(pdx);
+
+	case _IOC_NR(IOCTL_CED_TRANSFERFLAGS):
+		return TransferFlags(pdx);
+
+	case _IOC_NR(IOCTL_CED_DBGPEEK):
+		return DbgPeek(pdx, (TDBGBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_DBGPOKE):
+		return DbgPoke(pdx, (TDBGBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_DBGRAMPDATA):
+		return DbgRampData(pdx, (TDBGBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_DBGRAMPADDR):
+		return DbgRampAddr(pdx, (TDBGBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_DBGGETDATA):
+		return DbgGetData(pdx, (TDBGBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_DBGSTOPLOOP):
+		return DbgStopLoop(pdx);
+
+	case _IOC_NR(IOCTL_CED_FULLRESET):
+		pdx->bForceReset = true;	// Set a flag for a full reset
+		break;
+
+	case _IOC_NR(IOCTL_CED_SETCIRCULAR):
+		return SetCircular(pdx, (TRANSFERDESC __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_GETCIRCBLOCK):
+		return GetCircBlock(pdx, (TCIRCBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_FREECIRCBLOCK):
+		return FreeCircBlock(pdx, (TCIRCBLOCK __user *) ulArg);
+
+	case _IOC_NR(IOCTL_CED_WAITEVENT):
+		return WaitEvent(pdx, (int)(ulArg & 0xff), (int)(ulArg >> 8));
+
+	case _IOC_NR(IOCTL_CED_TESTEVENT):
+		return TestEvent(pdx, (int)ulArg);
+
+	default:
+		return U14ERR_NO_SUCH_FN;
+	}
+	return U14ERR_NOERROR;
+}
+
+static const struct file_operations ced_fops = {
+	.owner = THIS_MODULE,
+	.open = ced_open,
+	.release = ced_release,
+	.flush = ced_flush,
+	.llseek = noop_llseek,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+	.unlocked_ioctl = ced_ioctl,
+#else
+	.ioctl = ced_ioctl,
+#endif
+};
+
+/*
+ * usb class driver info in order to get a minor number from the usb core,
+ * and to have the device registered with the driver core
+ */
+static struct usb_class_driver ced_class = {
+	.name = "cedusb%d",
+	.fops = &ced_fops,
+	.minor_base = USB_CED_MINOR_BASE,
+};
+
+// Check that the device that matches a 1401 vendor and product ID is OK to use and
+// initialise our DEVICE_EXTENSION.
+static int ced_probe(struct usb_interface *interface,
+		     const struct usb_device_id *id)
+{
+	DEVICE_EXTENSION *pdx;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i, bcdDevice;
+	int retval = -ENOMEM;
+
+	// allocate memory for our device extension and initialize it
+	pdx = kzalloc(sizeof(*pdx), GFP_KERNEL);
+	if (!pdx) {
+		dev_err(&interface->dev, "Out of memory\n");
+		goto error;
+	}
+
+	for (i = 0; i < MAX_TRANSAREAS; ++i)	// Initialise the wait queues
+	{
+		init_waitqueue_head(&pdx->rTransDef[i].wqEvent);
+	}
+
+	// Put initialises for our stuff here. Note that all of *pdx is zero, so
+	// no need to explicitly zero it.
+	spin_lock_init(&pdx->charOutLock);
+	spin_lock_init(&pdx->charInLock);
+	spin_lock_init(&pdx->stagedLock);
+
+	// Initialises from the skeleton stuff
+	kref_init(&pdx->kref);
+	mutex_init(&pdx->io_mutex);
+	spin_lock_init(&pdx->err_lock);
+	init_usb_anchor(&pdx->submitted);
+
+	pdx->udev = usb_get_dev(interface_to_usbdev(interface));
+	pdx->interface = interface;
+
+	// Attempt to identify the device
+	bcdDevice = pdx->udev->descriptor.bcdDevice;
+	i = (bcdDevice >> 8);
+	if (i == 0)
+		pdx->s1401Type = TYPEU1401;
+	else if ((i >= 1) && (i <= 23))
+		pdx->s1401Type = i + 2;
+	else {
+		dev_err(&interface->dev, "%s Unknown device. bcdDevice = %d",
+			__func__, bcdDevice);
+		goto error;
+	}
+	// set up the endpoint information. We only care about the number of EP as
+	// we know that we are dealing with a 1401 device.
+	iface_desc = interface->cur_altsetting;
+	pdx->nPipes = iface_desc->desc.bNumEndpoints;
+	dev_info(&interface->dev, "1401Type=%d with %d End Points",
+		 pdx->s1401Type, pdx->nPipes);
+	if ((pdx->nPipes < 3) || (pdx->nPipes > 4))
+		goto error;
+
+	// Allocate the URBs we hold for performing transfers
+	pdx->pUrbCharOut = usb_alloc_urb(0, GFP_KERNEL);	// character output URB
+	pdx->pUrbCharIn = usb_alloc_urb(0, GFP_KERNEL);	// character input URB
+	pdx->pStagedUrb = usb_alloc_urb(0, GFP_KERNEL);	// block transfer URB
+	if (!pdx->pUrbCharOut || !pdx->pUrbCharIn || !pdx->pStagedUrb) {
+		dev_err(&interface->dev, "%s URB alloc failed", __func__);
+		goto error;
+	}
+
+	pdx->pCoherStagedIO =
+	    usb_alloc_coherent(pdx->udev, STAGED_SZ, GFP_KERNEL,
+			       &pdx->pStagedUrb->transfer_dma);
+	pdx->pCoherCharOut =
+	    usb_alloc_coherent(pdx->udev, OUTBUF_SZ, GFP_KERNEL,
+			       &pdx->pUrbCharOut->transfer_dma);
+	pdx->pCoherCharIn =
+	    usb_alloc_coherent(pdx->udev, INBUF_SZ, GFP_KERNEL,
+			       &pdx->pUrbCharIn->transfer_dma);
+	if (!pdx->pCoherCharOut || !pdx->pCoherCharIn || !pdx->pCoherStagedIO) {
+		dev_err(&interface->dev, "%s Coherent buffer alloc failed",
+			__func__);
+		goto error;
+	}
+
+	for (i = 0; i < pdx->nPipes; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		pdx->epAddr[i] = endpoint->bEndpointAddress;
+		dev_info(&interface->dev, "Pipe %d, ep address %02x", i,
+			 pdx->epAddr[i]);
+		if (((pdx->nPipes == 3) && (i == 0)) ||	// if char input end point
+		    ((pdx->nPipes == 4) && (i == 1))) {
+			pdx->bInterval = endpoint->bInterval;	// save the endpoint interrupt interval
+			dev_info(&interface->dev, "Pipe %d, bInterval = %d", i,
+				 pdx->bInterval);
+		}
+		// Detect USB2 by checking last ep size (64 if USB1)
+		if (i == pdx->nPipes - 1)	// if this is the last ep (bulk)
+		{
+			pdx->bIsUSB2 =
+			    le16_to_cpu(endpoint->wMaxPacketSize) > 64;
+			dev_info(&pdx->interface->dev, "USB%d",
+				 pdx->bIsUSB2 + 1);
+		}
+	}
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, pdx);
+
+	/* we can register the device now, as it is ready */
+	retval = usb_register_dev(interface, &ced_class);
+	if (retval) {
+		/* something prevented us from registering this driver */
+		dev_err(&interface->dev,
+			"Not able to get a minor for this device.\n");
+		usb_set_intfdata(interface, NULL);
+		goto error;
+	}
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&interface->dev,
+		 "USB CEDUSB device now attached to cedusb #%d",
+		 interface->minor);
+	return 0;
+
+error:
+	if (pdx)
+		kref_put(&pdx->kref, ced_delete);	// frees allocated memory
+	return retval;
+}
+
+static void ced_disconnect(struct usb_interface *interface)
+{
+	DEVICE_EXTENSION *pdx = usb_get_intfdata(interface);
+	int minor = interface->minor;	// save for message at the end
+	int i;
+
+	usb_set_intfdata(interface, NULL);	// remove the pdx from the interface
+	usb_deregister_dev(interface, &ced_class);	// give back our minor device number
+
+	mutex_lock(&pdx->io_mutex);	// stop more I/O starting while...
+	ced_draw_down(pdx);	// ...wait for then kill any io
+	for (i = 0; i < MAX_TRANSAREAS; ++i) {
+		int iErr = ClearArea(pdx, i);	// ...release any used memory
+		if (iErr == U14ERR_UNLOCKFAIL)
+			dev_err(&pdx->interface->dev, "%s Area %d was in used",
+				__func__, i);
+	}
+	pdx->interface = NULL;	// ...we kill off link to interface
+	mutex_unlock(&pdx->io_mutex);
+
+	usb_kill_anchored_urbs(&pdx->submitted);
+
+	kref_put(&pdx->kref, ced_delete);	// decrement our usage count
+
+	dev_info(&interface->dev, "USB cedusb #%d now disconnected", minor);
+}
+
+// Wait for all the urbs we know of to be done with, then kill off any that
+// are left. NBNB we will need to have a mechanism to stop circular xfers
+// from trying to fire off more urbs. We will wait up to 3 seconds for Urbs
+// to be done.
+void ced_draw_down(DEVICE_EXTENSION * pdx)
+{
+	int time;
+	dev_dbg(&pdx->interface->dev, "%s called", __func__);
+
+	pdx->bInDrawDown = true;
+	time = usb_wait_anchor_empty_timeout(&pdx->submitted, 3000);
+	if (!time)		// if we timed out we kill the urbs
+	{
+		usb_kill_anchored_urbs(&pdx->submitted);
+		dev_err(&pdx->interface->dev, "%s timed out", __func__);
+	}
+	pdx->bInDrawDown = false;
+}
+
+static int ced_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	DEVICE_EXTENSION *pdx = usb_get_intfdata(intf);
+	if (!pdx)
+		return 0;
+	ced_draw_down(pdx);
+
+	dev_dbg(&pdx->interface->dev, "%s called", __func__);
+	return 0;
+}
+
+static int ced_resume(struct usb_interface *intf)
+{
+	DEVICE_EXTENSION *pdx = usb_get_intfdata(intf);
+	if (!pdx)
+		return 0;
+	dev_dbg(&pdx->interface->dev, "%s called", __func__);
+	return 0;
+}
+
+static int ced_pre_reset(struct usb_interface *intf)
+{
+	DEVICE_EXTENSION *pdx = usb_get_intfdata(intf);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+	mutex_lock(&pdx->io_mutex);
+	ced_draw_down(pdx);
+	return 0;
+}
+
+static int ced_post_reset(struct usb_interface *intf)
+{
+	DEVICE_EXTENSION *pdx = usb_get_intfdata(intf);
+	dev_dbg(&pdx->interface->dev, "%s", __func__);
+
+	/* we are sure no URBs are active - no locking needed */
+	pdx->errors = -EPIPE;
+	mutex_unlock(&pdx->io_mutex);
+
+	return 0;
+}
+
+static struct usb_driver ced_driver = {
+	.name = "cedusb",
+	.probe = ced_probe,
+	.disconnect = ced_disconnect,
+	.suspend = ced_suspend,
+	.resume = ced_resume,
+	.pre_reset = ced_pre_reset,
+	.post_reset = ced_post_reset,
+	.id_table = ced_table,
+	.supports_autosuspend = 1,
+};
+
+module_usb_driver(ced_driver);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/ced1401/usb1401.h b/drivers/staging/ced1401/usb1401.h
new file mode 100644
index 000000000000..331ca9859829
--- /dev/null
+++ b/drivers/staging/ced1401/usb1401.h
@@ -0,0 +1,249 @@
+/* usb1401.h
+ Header file for the CED 1401 USB device driver for Linux
+ Copyright (C) 2010 Cambridge Electronic Design Ltd
+ Author Greg P Smith (greg@ced.co.uk)
+
+ This program is free software; you can redistribute it and/or
+ modify it under the terms of the GNU General Public License
+ as published by the Free Software Foundation; either version 2
+ of the License, or (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+#ifndef __USB1401_H__
+#define __USB1401_H__
+#include "use1401.h"
+#include "ced_ioctl.h"
+
+#ifndef UINT
+#define UINT unsigned int
+#endif
+
+/// Device type codes, but these don't need to be extended - a succession is assumed
+/// These are set for usb from the bcdDevice field (suitably mangled). Future devices
+/// will be added in order of device creation to the list, so the names here are just
+/// to help use remember which device is which. The U14ERR_... values follow the same
+/// pattern for modern devices.
+#define TYPEUNKNOWN        -1             // dont know
+#define TYPE1401           0              // standard 1401
+#define TYPEPLUS           1              // 1401 plus
+#define TYPEU1401          2              // u1401
+#define TYPEPOWER          3              // Power1401
+#define TYPEU14012         4              // u1401 mkII
+#define TYPEPOWER2         5              // Power1401 mk II
+#define TYPEMICRO3         6              // Micro1401-3
+#define TYPEPOWER3         7              // Power1401-3
+
+/// Some useful defines of constants. DONT FORGET to change the version in the
+/// resources whenever you change it here!.
+#define DRIVERMAJREV      2             // driver revision level major (match windows)
+#define DRIVERMINREV      0             // driver revision level minor
+
+/// Definitions of the various block transfer command codes
+#define TM_EXTTOHOST    8               // extended tohost
+#define TM_EXTTO1401    9               // extended to1401
+
+/// Definitions of values in usbReqtype. Used in sorting out setup actions
+#define H_TO_D 0x00
+#define D_TO_H 0x80
+#define VENDOR 0x40
+#define DEVREQ 0x00
+#define INTREQ 0x01
+#define ENDREQ 0x02
+
+/// Definition of values in usbRequest, again used to sort out setup
+#define GET_STATUS      0x00
+#define CLEAR_FEATURE   0x01
+#define SET_FEATURE     0x03
+#define SET_ADDRESS     0x05
+#define GET_DESC        0x06
+#define SET_DESC        0x07
+#define GET_CONF        0x08
+#define SET_CONF        0x09
+#define GET_INTERFACE   0x0a
+#define SET_INTERFACE   0x0b
+#define SYNCH_FRAME     0x0c
+
+/// Definitions of the various debug command codes understood by the 1401. These
+/// are used in various vendor-specific commands to achieve the desired effect
+#define DB_GRAB         0x50            /* Grab is a NOP for USB */
+#define DB_FREE         0x51            /* Free is a NOP for the USB */
+#define DB_SETADD       0x52            /* Set debug address (double) */
+#define DB_SELFTEST     0x53            /* Start self test */
+#define DB_SETMASK      0x54            /* Set enable mask (double) */
+#define DB_SETDEF       0x55            /* Set default mask (double) */
+#define DB_PEEK         0x56            /* Peek address, save result */
+#define DB_POKE         0x57            /* Poke address with data (double) */
+#define DB_RAMPD        0x58            /* Ramp data at debug address */
+#define DB_RAMPA        0x59            /* Ramp address bus */
+#define DB_REPEATS      0x5A            /* Set repeats for operations (double) */
+#define DB_WIDTH        0x5B            /* Set width for operations (byte) */
+#define DB_DATA         0x5C            /* Get 4-byte data read by PEEK */
+#define DB_CHARS        0x5D            /* Send chars via EP0 control write */
+
+#define CR_CHAR          0x0D           /* The carriage return character */
+#define CR_CHAR_80       0x8d           /*  and with bit 7 set */
+
+/// A structure holding information about a block of memory for use in circular transfers
+typedef struct circBlk
+{
+    volatile UINT dwOffset;             /* Offset within area of block start */
+    volatile UINT dwSize;               /* Size of the block, in bytes (0 = unused) */
+} CIRCBLK;
+
+/// A structure holding all of the information about a transfer area - an area of
+///  memory set up for use either as a source or destination in DMA transfers.
+typedef struct transarea
+{
+    void*       lpvBuff;                // User address of xfer area saved for completeness
+    UINT        dwBaseOffset;           // offset to start of xfer area in first page
+    UINT        dwLength;               // Length of xfer area, in bytes
+    struct page **pPages;               // Points at array of locked down pages
+    int         nPages;                 // number of pages that are locked down
+    bool        bUsed;                  // Is this structure in use?
+    bool        bCircular;              // Is this area for circular transfers?
+    bool        bCircToHost;            // Flag for direction of circular transfer
+    bool        bEventToHost;           // Set event on transfer to host?
+    int         iWakeUp;                // Set 1 on event, cleared by TestEvent()
+    UINT        dwEventSt;              // Defines section within xfer area for...
+    UINT        dwEventSz;              // ...notification by the event SZ is 0 if unset
+    CIRCBLK     aBlocks[2];             // Info on a pair of circular blocks
+    wait_queue_head_t wqEvent;          // The wait queue for events in this area MUST BE LAST
+} TRANSAREA;
+
+/// The DMADESC structure is used to hold information on the transfer in progress. It
+/// is set up by ReadDMAInfo, using information sent by the 1401 in an escape sequence.
+typedef struct dmadesc
+{
+    unsigned short wTransType;          /* transfer type as TM_xxx above        */
+    unsigned short wIdent;              /* identifier word                      */
+    unsigned int   dwSize;              /* bytes to transfer                    */
+    unsigned int   dwOffset;            /* offset into transfer area for trans  */
+    bool           bOutWard;            /* true when data is going TO 1401      */
+} DMADESC;
+
+#define INBUF_SZ         256            /* input buffer size */
+#define OUTBUF_SZ        256            /* output buffer size */
+#define STAGED_SZ 0x10000               // size of coherent buffer for staged transfers
+
+/// Structure to hold all of our device specific stuff. We are making this as similar as we
+/// can to the Windows driver to help in our understanding of what is going on.
+typedef struct _DEVICE_EXTENSION
+{
+    char inputBuffer[INBUF_SZ];         /* The two buffers */
+    char outputBuffer[OUTBUF_SZ];       /* accessed by the host functions */
+    volatile unsigned int dwNumInput;   /* num of chars in input buffer   */
+    volatile unsigned int dwInBuffGet;  /* where to get from input buffer */
+    volatile unsigned int dwInBuffPut;  /* where to put into input buffer */
+    volatile unsigned int dwNumOutput;  /* num of chars in output buffer  */
+    volatile unsigned int dwOutBuffGet; /* where to get from output buffer*/
+    volatile unsigned int dwOutBuffPut; /* where to put into output buffer*/
+
+    volatile bool bSendCharsPending;    /* Flag to indicate sendchar active */
+    volatile bool bReadCharsPending;    /* Flag to indicate a read is primed */
+    char* pCoherCharOut;                /* special aligned buffer for chars to 1401 */
+    struct urb* pUrbCharOut;            /* urb used for chars to 1401 */
+    char* pCoherCharIn;                 /* special aligned buffer for chars to host */
+    struct urb* pUrbCharIn;             /* urb used for chars to host */
+
+    spinlock_t charOutLock;             /* to protect the outputBuffer and outputting */
+    spinlock_t charInLock;              /* to protect the inputBuffer and char reads */
+    __u8 bInterval;                     /* Interrupt end point interval */
+
+    volatile unsigned int dwDMAFlag;    /* state of DMA */
+    TRANSAREA rTransDef[MAX_TRANSAREAS];/* transfer area info */
+    volatile DMADESC rDMAInfo;          // info on current DMA transfer
+    volatile bool bXFerWaiting;         // Flag set if DMA transfer stalled
+    volatile bool bInDrawDown;          // Flag that we want to halt transfers
+
+    // Parameters relating to a block read\write that is in progress. Some of these values
+    //  are equivalent to values in rDMAInfo. The values here are those in use, while those
+    //  in rDMAInfo are those recieved from the 1401 via an escape sequence. If another
+    //  escape sequence arrives before the previous xfer ends, rDMAInfo values are updated while these
+    //  are used to finish off the current transfer.
+    volatile short StagedId;            // The transfer area id for this transfer
+    volatile bool StagedRead;           // Flag TRUE for read from 1401, FALSE for write
+    volatile unsigned int StagedLength; // Total length of this transfer
+    volatile unsigned int StagedOffset; // Offset within memory area for transfer start
+    volatile unsigned int StagedDone;   // Bytes transferred so far
+    volatile bool bStagedUrbPending;    // Flag to indicate active
+    char* pCoherStagedIO;               // buffer used for block transfers
+    struct urb* pStagedUrb;             // The URB to use
+    spinlock_t stagedLock;              // protects ReadWriteMem() and circular buffer stuff
+
+    short s1401Type;                    // type of 1401 attached
+    short sCurrentState;                // current error state
+    bool bIsUSB2;                       // type of the interface we connect to
+    bool bForceReset;                   // Flag to make sure we get a real reset
+    __u32 statBuf[2];                   // buffer for 1401 state info
+
+    unsigned long ulSelfTestTime;       // used to timeout self test
+
+    int nPipes;                         // Should be 3 or 4 depending on 1401 usb chip
+    int bPipeError[4];                  // set non-zero if an error on one of the pipe
+    __u8 epAddr[4];                     // addresses of the 3/4 end points
+
+    struct usb_device *udev;            // the usb device for this device
+    struct usb_interface *interface;    // the interface for this device, NULL if removed
+    struct usb_anchor submitted;        // in case we need to retract our submissions
+    struct mutex io_mutex;              // synchronize I/O with disconnect, one user-mode caller at a time
+
+    int    errors;                      // the last request tanked
+    int    open_count;                  // count the number of openers
+    spinlock_t err_lock;                // lock for errors
+    struct kref kref;
+}DEVICE_EXTENSION, *PDEVICE_EXTENSION;
+#define to_DEVICE_EXTENSION(d) container_of(d, DEVICE_EXTENSION, kref)
+
+/// Definitions of routimes used between compilation object files
+// in usb1401.c
+extern int Allowi(DEVICE_EXTENSION* pdx, bool bInCallback);
+extern int SendChars(DEVICE_EXTENSION* pdx);
+extern void ced_draw_down(DEVICE_EXTENSION *pdx);
+extern int ReadWriteMem(DEVICE_EXTENSION *pdx, bool Read, unsigned short wIdent,
+                      unsigned int dwOffs, unsigned int dwLen);
+
+// in ced_ioc.c
+extern int ClearArea(DEVICE_EXTENSION *pdx, int nArea);
+extern int SendString(DEVICE_EXTENSION* pdx, const char __user* pData, unsigned int n);
+extern int SendChar(DEVICE_EXTENSION *pdx, char c);
+extern int Get1401State(DEVICE_EXTENSION* pdx, __u32* state, __u32* error);
+extern int ReadWrite_Cancel(DEVICE_EXTENSION *pdx);
+extern bool Is1401(DEVICE_EXTENSION* pdx);
+extern bool QuickCheck(DEVICE_EXTENSION* pdx, bool bTestBuff, bool bCanReset);
+extern int Reset1401(DEVICE_EXTENSION *pdx);
+extern int GetChar(DEVICE_EXTENSION *pdx);
+extern int GetString(DEVICE_EXTENSION *pdx, char __user* pUser, int n);
+extern int SetTransfer(DEVICE_EXTENSION *pdx, TRANSFERDESC __user *pTD);
+extern int UnsetTransfer(DEVICE_EXTENSION *pdx, int nArea);
+extern int SetEvent(DEVICE_EXTENSION *pdx, TRANSFEREVENT __user*pTE);
+extern int Stat1401(DEVICE_EXTENSION *pdx);
+extern int LineCount(DEVICE_EXTENSION *pdx);
+extern int GetOutBufSpace(DEVICE_EXTENSION *pdx);
+extern int GetTransfer(DEVICE_EXTENSION *pdx, TGET_TX_BLOCK __user *pGTB);
+extern int KillIO1401(DEVICE_EXTENSION *pdx);
+extern int BlkTransState(DEVICE_EXTENSION *pdx);
+extern int StateOf1401(DEVICE_EXTENSION *pdx);
+extern int StartSelfTest(DEVICE_EXTENSION *pdx);
+extern int CheckSelfTest(DEVICE_EXTENSION *pdx, TGET_SELFTEST __user *pGST);
+extern int TypeOf1401(DEVICE_EXTENSION *pdx);
+extern int TransferFlags(DEVICE_EXTENSION *pdx);
+extern int DbgPeek(DEVICE_EXTENSION *pdx, TDBGBLOCK __user* pDB);
+extern int DbgPoke(DEVICE_EXTENSION *pdx, TDBGBLOCK __user *pDB);
+extern int DbgRampData(DEVICE_EXTENSION *pdx, TDBGBLOCK __user *pDB);
+extern int DbgRampAddr(DEVICE_EXTENSION *pdx, TDBGBLOCK __user *pDB);
+extern int DbgGetData(DEVICE_EXTENSION *pdx, TDBGBLOCK __user *pDB);
+extern int DbgStopLoop(DEVICE_EXTENSION *pdx);
+extern int SetCircular(DEVICE_EXTENSION *pdx, TRANSFERDESC __user *pTD);
+extern int GetCircBlock(DEVICE_EXTENSION *pdx, TCIRCBLOCK __user* pCB);
+extern int FreeCircBlock(DEVICE_EXTENSION *pdx, TCIRCBLOCK __user* pCB);
+extern int WaitEvent(DEVICE_EXTENSION *pdx, int nArea, int msTimeOut);
+extern int TestEvent(DEVICE_EXTENSION *pdx, int nArea);
+#endif
diff --git a/drivers/staging/ced1401/use1401.h b/drivers/staging/ced1401/use1401.h
new file mode 100644
index 000000000000..86294e21db0c
--- /dev/null
+++ b/drivers/staging/ced1401/use1401.h
@@ -0,0 +1,287 @@
+/****************************************************************************
+** use1401.h
+** Copyright (C) Cambridge Electronic Design Ltd, 1992-2010
+** Authors: Paul Cox, Tim Bergel, Greg Smith
+** See CVS for revisions.
+**
+** Because the size of a long is different between 32-bit and 64-bit on some
+** systems, we avoid this in this interface.
+****************************************************************************/
+#ifndef __USE1401_H__
+#define __USE1401_H__
+#include "machine.h"
+
+// Some definitions to make things compatible. If you want to use Use1401 directly
+//  from a Windows program you should define U14_NOT_DLL, in which case you also
+//  MUST make sure that your application startup code calls U14InitLib().
+// DLL_USE1401 is defined when you are building the Use1401 dll, not otherwise.
+#ifdef _IS_WINDOWS_
+#ifndef U14_NOT_DLL
+#ifdef DLL_USE1401
+#define U14API(retType) retType DllExport __stdcall
+#else
+#define U14API(retType) retType DllImport __stdcall
+#endif
+#endif
+
+#define U14ERRBASE -500
+#define U14LONG long
+#endif
+
+#ifdef LINUX
+#define U14ERRBASE -1000
+#define U14LONG int
+#endif
+
+#ifdef _QT
+#ifndef U14_NOT_DLL
+#undef U14API
+#define U14API(retType) retType __declspec(dllimport) __stdcall
+#endif
+#undef U14LONG
+#define U14LONG int
+#endif
+
+#ifndef U14API
+#define U14API(retType) retType
+#endif
+
+#ifndef U14LONG
+#define U14LONG long
+#endif
+
+/// Error codes: We need them here as user space can see them.
+#define U14ERR_NOERROR        0             // no problems
+
+/// Device error codes, but these don't need to be extended - a succession is assumed
+#define U14ERR_STD            4              // standard 1401 connected
+#define U14ERR_U1401          5              // u1401 connected
+#define U14ERR_PLUS           6              // 1401 plus connected
+#define U14ERR_POWER          7              // Power1401 connected
+#define U14ERR_U14012         8              // u1401 mkII connected
+#define U14ERR_POWER2         9
+#define U14ERR_U14013        10
+#define U14ERR_POWER3        11
+
+/// NBNB Error numbers need shifting as some linux error codes start at 512
+#define U14ERR(n)             (n+U14ERRBASE)
+#define U14ERR_OFF            U14ERR(0)      /* 1401 there but switched off    */
+#define U14ERR_NC             U14ERR(-1)     /* 1401 not connected             */
+#define U14ERR_ILL            U14ERR(-2)     /* if present it is ill           */
+#define U14ERR_NOIF           U14ERR(-3)     /* I/F card missing               */
+#define U14ERR_TIME           U14ERR(-4)     /* 1401 failed to come ready      */
+#define U14ERR_BADSW          U14ERR(-5)     /* I/F card bad switches          */
+#define U14ERR_PTIME          U14ERR(-6)     /* 1401plus failed to come ready  */
+#define U14ERR_NOINT          U14ERR(-7)     /* couldn't grab the int vector   */
+#define U14ERR_INUSE          U14ERR(-8)     /* 1401 is already in use         */
+#define U14ERR_NODMA          U14ERR(-9)     /* couldn't get DMA channel       */
+#define U14ERR_BADHAND        U14ERR(-10)    /* handle provided was bad        */
+#define U14ERR_BAD1401NUM     U14ERR(-11)    /* 1401 number provided was bad   */
+
+#define U14ERR_NO_SUCH_FN     U14ERR(-20)    /* no such function               */
+#define U14ERR_NO_SUCH_SUBFN  U14ERR(-21)    /* no such sub function           */
+#define U14ERR_NOOUT          U14ERR(-22)    /* no room in output buffer       */
+#define U14ERR_NOIN           U14ERR(-23)    /* no input in buffer             */
+#define U14ERR_STRLEN         U14ERR(-24)    /* string longer than buffer      */
+#define U14ERR_ERR_STRLEN     U14ERR(-24)    /* string longer than buffer      */
+#define U14ERR_LOCKFAIL       U14ERR(-25)    /* failed to lock memory          */
+#define U14ERR_UNLOCKFAIL     U14ERR(-26)    /* failed to unlock memory        */
+#define U14ERR_ALREADYSET     U14ERR(-27)    /* area already set up            */
+#define U14ERR_NOTSET         U14ERR(-28)    /* area not set up                */
+#define U14ERR_BADAREA        U14ERR(-29)    /* illegal area number            */
+#define U14ERR_FAIL           U14ERR(-30)    /* we failed for some other reason*/
+
+#define U14ERR_NOFILE         U14ERR(-40)    /* command file not found         */
+#define U14ERR_READERR        U14ERR(-41)    /* error reading command file     */
+#define U14ERR_UNKNOWN        U14ERR(-42)    /* unknown command                */
+#define U14ERR_HOSTSPACE      U14ERR(-43)    /* not enough host space to load  */
+#define U14ERR_LOCKERR        U14ERR(-44)    /* could not lock resource/command*/
+#define U14ERR_CLOADERR       U14ERR(-45)    /* CLOAD command failed           */
+
+#define U14ERR_TOXXXERR       U14ERR(-60)    /* tohost/1401 failed             */
+#define U14ERR_NO386ENH       U14ERR(-80)    /* not 386 enhanced mode          */
+#define U14ERR_NO1401DRIV     U14ERR(-81)    /* no device driver               */
+#define U14ERR_DRIVTOOOLD     U14ERR(-82)    /* device driver too old          */
+
+#define U14ERR_TIMEOUT        U14ERR(-90)    /* timeout occurred               */
+
+#define U14ERR_BUFF_SMALL     U14ERR(-100)   /* buffer for getstring too small */
+#define U14ERR_CBALREADY      U14ERR(-101)   /* there is already a callback    */
+#define U14ERR_BADDEREG       U14ERR(-102)   /* bad parameter to deregcallback */
+#define U14ERR_NOMEMORY       U14ERR(-103)   /* no memory for allocation       */
+
+#define U14ERR_DRIVCOMMS      U14ERR(-110)   /* failed talking to driver       */
+#define U14ERR_OUTOFMEMORY    U14ERR(-111)   /* needed memory and couldnt get it*/
+
+/// 1401 type codes.
+#define U14TYPE1401           0           /* standard 1401                  */
+#define U14TYPEPLUS           1           /* 1401 plus                      */
+#define U14TYPEU1401          2           /* u1401                          */
+#define U14TYPEPOWER          3           /* power1401                      */
+#define U14TYPEU14012         4           /* u1401 mk II                    */
+#define U14TYPEPOWER2         5           /* power1401 mk II                */
+#define U14TYPEU14013         6           /* u1401-3                        */
+#define U14TYPEPOWER3         7           /* power1401-3                    */
+#define U14TYPEUNKNOWN        -1          /* dont know                      */
+
+/// Transfer flags to allow driver capabilities to be interrogated
+
+/// Constants for transfer flags
+#define U14TF_USEDMA          1           /* Transfer flag for use DMA      */
+#define U14TF_MULTIA          2           /* Transfer flag for multi areas  */
+#define U14TF_FIFO            4           /* for FIFO interface card        */
+#define U14TF_USB2            8           /* for USB2 interface and 1401    */
+#define U14TF_NOTIFY          16          /* for event notifications        */
+#define U14TF_SHORT           32          /* for PCI can short cycle        */
+#define U14TF_PCI2            64          /* for new PCI card 1401-70       */
+#define U14TF_CIRCTH          128         /* Circular-mode to host          */
+#define U14TF_DIAG            256         /* Diagnostics/debug functions    */
+#define U14TF_CIRC14          512         /* Circular-mode to 1401          */
+
+/// Definitions of element sizes for DMA transfers - to allow byte-swapping
+#define ESZBYTES              0           /* BYTE element size value        */
+#define ESZWORDS              1           /* WORD element size value        */
+#define ESZLONGS              2           /* long element size value        */
+#define ESZUNKNOWN            0           /* unknown element size value     */
+
+/// These define required access types for the debug/diagnostics function
+#define BYTE_SIZE             1           /* 8-bit access                   */
+#define WORD_SIZE             2           /* 16-bit access                  */
+#define LONG_SIZE             3           /* 32-bit access                  */
+
+/// Stuff used by U14_GetTransfer
+#define GET_TX_MAXENTRIES  257          /* (max length / page size + 1) */
+
+#ifdef _IS_WINDOWS_
+#pragma pack(1)
+
+typedef struct                          /* used for U14_GetTransfer results */
+{                                          /* Info on a single mapped block */
+   U14LONG physical;
+   U14LONG size;
+} TXENTRY;
+
+typedef struct TGetTxBlock              /* used for U14_GetTransfer results */
+{                                               /* matches structure in VXD */
+   U14LONG size;
+   U14LONG linear;
+   short   seg;
+   short   reserved;
+   short   avail;                      /* number of available entries */
+   short   used;                       /* number of used entries */
+   TXENTRY entries[GET_TX_MAXENTRIES];       /* Array of mapped block info */
+} TGET_TX_BLOCK;
+
+typedef TGET_TX_BLOCK *LPGET_TX_BLOCK;
+
+#pragma pack()
+#endif
+
+#ifdef LINUX
+typedef struct                          /* used for U14_GetTransfer results */
+{                                       /* Info on a single mapped block */
+   long long physical;
+   long     size;
+} TXENTRY;
+
+typedef struct TGetTxBlock              /* used for U14_GetTransfer results */
+{                                       /* matches structure in VXD */
+   long long linear;                    /* linear address */
+   long     size;                       /* total size of the mapped area, holds id when called */
+   short    seg;                        /* segment of the address for Win16 */
+   short    reserved;
+   short    avail;                      /* number of available entries */
+   short    used;                       /* number of used entries */
+   TXENTRY  entries[GET_TX_MAXENTRIES]; /* Array of mapped block info */
+} TGET_TX_BLOCK;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+U14API(int)   U14WhenToTimeOut(short hand);         // when to timeout in ms
+U14API(short) U14PassedTime(int iTime);             // non-zero if iTime passed
+
+U14API(short) U14LastErrCode(short hand);
+
+U14API(short) U14Open1401(short n1401);
+U14API(short) U14Close1401(short hand);
+U14API(short) U14Reset1401(short hand);
+U14API(short) U14ForceReset(short hand);
+U14API(short) U14TypeOf1401(short hand);
+U14API(short) U14NameOf1401(short hand, char* pBuf, WORD wMax);
+
+U14API(short) U14Stat1401(short hand);
+U14API(short) U14CharCount(short hand);
+U14API(short) U14LineCount(short hand);
+
+U14API(short) U14SendString(short hand, const char* pString);
+U14API(short) U14GetString(short hand, char* pBuffer, WORD wMaxLen);
+U14API(short) U14SendChar(short hand, char cChar);
+U14API(short) U14GetChar(short hand, char* pcChar);
+
+U14API(short) U14LdCmd(short hand, const char* command);
+U14API(DWORD) U14Ld(short hand, const char* vl, const char* str);
+
+U14API(short) U14SetTransArea(short hand, WORD wArea, void *pvBuff,
+                                            DWORD dwLength, short eSz);
+U14API(short) U14UnSetTransfer(short hand, WORD wArea);
+U14API(short) U14SetTransferEvent(short hand, WORD wArea, BOOL bEvent,
+                                  BOOL bToHost, DWORD dwStart, DWORD dwLength);
+U14API(int)   U14TestTransferEvent(short hand, WORD wArea);
+U14API(int)   U14WaitTransferEvent(short hand, WORD wArea, int msTimeOut);
+U14API(short) U14GetTransfer(short hand, TGET_TX_BLOCK *pTransBlock);
+
+U14API(short) U14ToHost(short hand, char* pAddrHost,DWORD dwSize,DWORD dw1401,
+                                                            short eSz);
+U14API(short) U14To1401(short hand, const char* pAddrHost,DWORD dwSize,DWORD dw1401,
+                                                            short eSz);
+
+U14API(short) U14SetCircular(short hand, WORD wArea, BOOL bToHost, void *pvBuff,
+                                         DWORD dwLength);
+
+U14API(int)   U14GetCircBlk(short hand, WORD wArea, DWORD *pdwOffs);
+U14API(int)   U14FreeCircBlk(short hand, WORD wArea, DWORD dwOffs, DWORD dwSize,
+                                         DWORD *pdwOffs);
+
+U14API(short) U14StrToLongs(const char* pszBuff, U14LONG *palNums, short sMaxLongs);
+U14API(short) U14LongsFrom1401(short hand, U14LONG *palBuff, short sMaxLongs);
+
+U14API(void)  U14SetTimeout(short hand, int lTimeout);
+U14API(int)   U14GetTimeout(short hand);
+U14API(short) U14OutBufSpace(short hand);
+U14API(int)   U14BaseAddr1401(short hand);
+U14API(int)   U14DriverVersion(short hand);
+U14API(int)   U14DriverType(short hand);
+U14API(short) U14DriverName(short hand, char* pBuf, WORD wMax);
+U14API(short) U14GetUserMemorySize(short hand, DWORD *pMemorySize);
+U14API(short) U14KillIO1401(short hand);
+
+U14API(short) U14BlkTransState(short hand);
+U14API(short) U14StateOf1401(short hand);
+
+U14API(short) U14Grab1401(short hand);
+U14API(short) U14Free1401(short hand);
+U14API(short) U14Peek1401(short hand, DWORD dwAddr, int nSize, int nRepeats);
+U14API(short) U14Poke1401(short hand, DWORD dwAddr, DWORD dwValue, int nSize, int nRepeats);
+U14API(short) U14Ramp1401(short hand, DWORD dwAddr, DWORD dwDef, DWORD dwEnable, int nSize, int nRepeats);
+U14API(short) U14RampAddr(short hand, DWORD dwDef, DWORD dwEnable, int nSize, int nRepeats);
+U14API(short) U14StopDebugLoop(short hand);
+U14API(short) U14GetDebugData(short hand, U14LONG *plValue);
+
+U14API(short) U14StartSelfTest(short hand);
+U14API(short) U14CheckSelfTest(short hand, U14LONG *pData);
+U14API(short) U14TransferFlags(short hand);
+U14API(void)  U14GetErrorString(short nErr, char* pStr, WORD wMax);
+U14API(int)   U14MonitorRev(short hand);
+U14API(void)  U14CloseAll(void);
+
+U14API(short) U14WorkingSet(DWORD dwMinKb, DWORD dwMaxKb);
+U14API(int)   U14InitLib(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* End of ifndef __USE1401_H__ */
diff --git a/drivers/staging/ced1401/use14_ioc.h b/drivers/staging/ced1401/use14_ioc.h
new file mode 100644
index 000000000000..15ca63888380
--- /dev/null
+++ b/drivers/staging/ced1401/use14_ioc.h
@@ -0,0 +1,301 @@
+/* use14_ioc.h
+** definitions of use1401 module stuff that is shared between use1401 and the driver.
+** Copyright (C) Cambridge Electronic Design Limited 2010
+** Author Greg P Smith
+************************************************************************************/
+#ifndef __USE14_IOC_H__
+#define __USE14_IOC_H__
+
+#define  MAX_TRANSAREAS   8   /* The number of transfer areas supported by driver */
+
+#define i386
+#include "winioctl.h"                   /* needed so we can access driver   */
+
+/*
+** Defines for IOCTL functions to ask driver to perform. These must be matched
+** in both use1401 and in the driver. The IOCTL code contains a command
+** identifier, plus other information about the device, the type of access
+** with which the file must have been opened, and the type of buffering.
+** The IOCTL function codes from 0x80 to 0xFF are for developer use.
+*/
+#define  FILE_DEVICE_CED1401    0x8001
+#define  FNNUMBASE              0x800
+
+#define  U14_OPEN1401            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE,               \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_CLOSE1401           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+1,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SENDSTRING          CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+2,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_RESET1401           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+3,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETCHAR             CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+4,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SENDCHAR            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+5,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STAT1401            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+6,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_LINECOUNT           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+7,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETSTRING           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+8,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_REGCALLBACK         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+9,             \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETMONITORBUF       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+10,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SETTRANSFER         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+11,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_UNSETTRANSFER       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+12,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SETTRANSEVENT       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+13,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETOUTBUFSPACE      CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+14,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETBASEADDRESS      CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+15,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETDRIVERREVISION   CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+16,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETTRANSFER         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+17,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_KILLIO1401          CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+18,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_BLKTRANSSTATE       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+19,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_BYTECOUNT           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+20,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_ZEROBLOCKCOUNT      CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+21,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STOPCIRCULAR        CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+22,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STATEOF1401         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+23,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_REGISTERS1401       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+24,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GRAB1401            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+25,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_FREE1401            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+26,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STEP1401            CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+27,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SET1401REGISTERS    CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+28,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STEPTILL1401        CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+29,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SETORIN             CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+30,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_STARTSELFTEST       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+31,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_CHECKSELFTEST       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+32,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_TYPEOF1401          CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+33,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_TRANSFERFLAGS       CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+34,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGPEEK             CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+35,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGPOKE             CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+36,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGRAMPDATA         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+37,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGRAMPADDR         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+38,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGGETDATA          CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+39,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_DBGSTOPLOOP         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+40,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_FULLRESET           CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+41,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_SETCIRCULAR         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+42,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_GETCIRCBLK          CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+43,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+#define  U14_FREECIRCBLK         CTL_CODE( FILE_DEVICE_CED1401,     \
+                                           FNNUMBASE+44,            \
+                                           METHOD_BUFFERED,         \
+                                           FILE_ANY_ACCESS)
+
+//--------------- Structures that are shared with the driver -------------
+#pragma pack(1)
+
+typedef struct                  /* used for get/set standard 1401 registers */
+{
+   short   sPC;
+   char    A;
+   char    X;
+   char    Y;
+   char    stat;
+   char    rubbish;
+} T1401REGISTERS;
+
+typedef union     /* to communicate with 1401 driver status & control funcs */
+{
+   char           chrs[22];
+   short          ints[11];
+   long           longs[5];
+   T1401REGISTERS registers;
+} TCSBLOCK;
+
+typedef TCSBLOCK*  LPTCSBLOCK;
+
+typedef struct paramBlk
+{
+    short       sState;
+    TCSBLOCK    csBlock;
+} PARAMBLK;
+
+typedef PARAMBLK*   PPARAMBLK;
+
+typedef struct TransferDesc          /* Structure and type for SetTransArea */
+{
+   WORD        wArea;            /* number of transfer area to set up       */
+   void FAR *  lpvBuff;          /* address of transfer area                */
+   DWORD       dwLength;         /* length of area to set up                */
+   short       eSize;            /* size to move (for swapping on MAC)      */
+} TRANSFERDESC;
+
+typedef TRANSFERDESC FAR *    LPTRANSFERDESC;
+
+/* This is the structure used to set up a transfer area */
+typedef struct VXTransferDesc    /* use1401.c and use1432x.x use only       */
+{
+   WORD        wArea;            /* number of transfer area to set up       */
+   WORD        wAddrSel;         /* 16 bit selector for area                */
+   DWORD       dwAddrOfs;        /* 32 bit offset for area start            */
+   DWORD       dwLength;         /* length of area to set up                */
+} VXTRANSFERDESC;
+
+#pragma pack()
+
+#endif
\ No newline at end of file
diff --git a/drivers/staging/ced1401/userspace/use1401.c b/drivers/staging/ced1401/userspace/use1401.c
new file mode 100644
index 000000000000..d4c63168ea27
--- /dev/null
+++ b/drivers/staging/ced1401/userspace/use1401.c
@@ -0,0 +1,3035 @@
+/****************************************************************************
+** use1401.c
+** Copyright (C) Cambridge Electronic Design Ltd, 1992-2010
+**
+** This program is free software; you can redistribute it and/or
+** modify it under the terms of the GNU General Public License
+** as published by the Free Software Foundation; either version 2
+** of the License, or (at your option) any later version.
+**
+** This program is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+** GNU General Public License for more details.
+**
+** You should have received a copy of the GNU General Public License
+** along with this program; if not, write to the Free Software
+** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+**
+** Contact CED: Cambridge Electronic Design Limited, Science Park, Milton Road
+**              Cambridge, CB6 0FE.
+**              www.ced.co.uk
+**              greg@ced.co.uk
+**
+**  Title:      USE1401.C
+**  Version:    4.00
+**  Author:     Paul Cox, Tim Bergel, Greg Smith
+**
+** The code was vigorously pruned in DEC 2010 to remove the macintosh options
+** and to get rid of the 16-bit support. It has also been aligned with the
+** Linux version. See CVS for revisions. This will work for Win 9x onwards.
+****************************************************************************
+**
+** Notes on Windows interface to driver
+** ************************************
+**
+** Under Windows 9x and NT, Use1401 uses DeviceIoControl to get access to
+** the 1401 driver. This has parameters for the device handle, the function
+** code, an input pointer and byte count, an output pointer and byte count
+** and a pointer to a DWORD to hold the output byte count. Note that input
+** and output are from the point-of-view of the driver, so the output stuff
+** is used to read values from the 1401, not send to the 1401. The use of
+** these parameters varies with the function in use and the operating
+** system; there are five separate DIOC calls SendString, GetString and
+** SetTransferArea all have their own specialised calls, the rest use the
+** Status1401 or Control1401 functions.
+**
+** There are two basic styles of DIOC call used, one for Win9x VxD drivers
+** and one for NT Kernel-mode and WDM drivers (see below for tables showing
+** the different parameters used. The array bUseNTDIOC[] selects between
+** these two calling styles.
+**
+** Function codes
+** In Win3.x, simple function codes from 0 to 40 were used, shifted left 8
+** bits with a sub-function code in the lower 8 bits. These were also used
+** in the Windows 95 driver, though we had to add 1 to the code value to
+** avoid problems (Open from CreateFile is zero), and the sub-function code
+** is now unused. We found that this gave some problems with Windows 98
+** as the function code values are reserved by microsoft, so we switched to
+** using the NT function codes instead. The NT codes are generated using the
+** CTL_CODE macro, essentially this gives 0x80012000 | (func << 2), where
+** func is the original 0 to 34 value. The driver will handle both types of
+** code and Use1432 only uses the NT codes if it knows the driver is new
+** enough. The array bUseNTCodes[] holds flags on the type of codes required.
+** GPS/TDB Dec 2010: we removed the bUseNTCodes array as this is always true
+** as we no longer support ancient versions.
+**
+** The CreateFile and CloseFile function calls are also handled
+** by DIOC, using the special function codes 0 and -1 respectively.
+**
+** Input pointer and buffer size
+** These are intended for data sent to the device driver. In nearly all cases
+** they are unused in calls to the Win95 driver, the NT driver uses them
+** for all information sent to the driver. The table below shows the pointer
+** and byte count used for the various calls:
+**
+**                      Win 95                  Win NT
+** SendString           NULL, 0                 pStr, nStr
+** GetString            NULL, 0                 NULL, 0
+** SetTransferArea      pBuf, nBuf (unused?)    pDesc, nDesc
+** GetTransfer          NULL, 0                 NULL, 0
+** Status1401           NULL, 0                 NULL, 0
+** Control1401          NULL, 0                 pBlk, nBlk
+**
+** pStr and nStr are pointers to a char buffer and the buffer length for
+** string I/O, note that these are temporary buffers owned by the DLL, not
+** application memory, pBuf and nBuf are the transfer area buffer (I think
+** these are unused), pDesc and nDesc are the TRANSFERDESC structure, pBlk
+** and nBlk are the TCSBLOCK structure.
+**
+**
+** Output pointer and buffer size
+** These are intended for data read from the device driver. These are used
+** for almost all information sent to the Win95 driver, the NT driver uses
+** them for information read from the driver, chiefly the error code. The
+** table below shows the pointer and byte count used for the various calls:
+**
+**                      Win 95                  Win NT
+** SendString           pStr, nStr              pPar, nPar
+** GetString            pStr, nStr+2            pStr, nStr+2
+** SetTransferArea      pDesc, nDesc            pPar, nPar
+** GetTransfer          pGet, nGet              pGet, nGet
+** Status1401           pBlk, nBlk              pPar, nPar
+** Control1401          pBlk, nBlk              pPar, nPar
+**
+** pStr and nStr are pointers to a char buffer and the buffer length for
+** string I/O, the +2 for GetString refers to two spare bytes at the start
+** used to hold the string length and returning an error code for NT. Note
+** again that these are (and must be) DLL-owned temporary buffers. pPar
+** and nPar are a PARAM structure used in NT (it holds an error code and a 
+** TCSBLOCK structure). pDesc and nDesc are the VXTRANSFERDESC structure,
+** pBlk and nBlk are the TCSBLOCK structure. pGet and nGet indicate the
+** TGET_TX_BLOCK structure used for GetTransfer.
+**
+**
+** The output byte count
+** Both drivers return the output buffer size here, regardless of the actual
+** bytes output. This is used to check that we did get through to the driver.
+**
+** Multiple 1401s
+** **************
+**
+** We have code that tries to support the use of multiple 1401s, but there
+** are problems: The lDriverVersion and lDriverType variables are global, not
+** per-1401 (a particular problem as the U14 functions that use them don't
+** have a hand parameter). In addition, the mechansim for finding a free
+** 1401 depends upon the 1401 device driver open operation failing if it's
+** already in use, which doesn't always happen, particularly with the VxDs.
+** The code in TryToOpen tries to fix this by relying on TYPEOF1401 to detect
+** the 1401-in-use state - the VxDs contain special code to help this. This is
+** working OK but multiple 1401 support works better with the Win2000 drivers.
+**
+** USB driver
+** **********
+**
+** The USB driver, which runs on both Win98 and NT2000, uses the NT-style
+** calling convention, both for the DIOC codes and the DIOC parameters. The
+** TryToOpen function has been altered to look for an NT driver first in
+** the appropriate circumstances, and to set the driver DIOC flags up in
+** the correct state.
+**
+** Adding a new 1401 type - now almost nothing to do
+** *************************************************
+**
+** The 1401 types are defined by a set of U14TYPExxxx codes in USE1401.H.
+** You should add a new one of these to keep things tidy for applications.
+**
+** DRIVERET_MAX (below) specifies the maximum allowed type code from the
+** 1401 driver; I have set this high to accomodate as yet undesigned 1401
+** types. Similarly, as long as the command file names follow the ARM,
+** ARN, ARO sequence, these are calculated by the ExtForType function, so
+** you don't need to do anything here either.
+**
+** Version number
+** **************
+** The new U14InitLib() function returns 0 if the OS is incapable of use,
+** otherwise is returns the version of the USE1401 library. This is done
+** in three parts: Major(31-24).Minor(23-16).Revision.(15-0) (brackets are
+** the bits used). The Major number starts at 2 for the first revision with
+** the U14InitLib() function. Changes to the Major version means that we
+** have broken backwards compatibility. Minor number changes mean that we
+** have added new functionality that does not break backwards compatibility.
+** we starts at 0. Revision changes mean we have fixed something. Each index
+** returns to 0 when a higer one changes.
+*/
+#define U14LIB_MAJOR 4
+#define U14LIB_MINOR 0
+#define U14LIB_REVISION 0
+#define U14LIB_VERSION ((U14LIB_MAJOR<<24) | (U14LIB_MINOR<<16) | U14LIB_REVISION)
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "USE1401.H"
+
+#ifdef _IS_WINDOWS_
+#include <io.h>
+#include <windows.h>
+#pragma warning(disable: 4100) /* Disable "Unused formal parameter" warning */
+#include <assert.h>
+#include "process.h"
+
+
+#define sprintf wsprintf
+#define PATHSEP '\\'
+#define PATHSEPSTR "\\"
+#define DEFCMDPATH "\\1401\\"   // default command path if all else fails
+#define MINDRIVERMAJREV 1       // minimum driver revision level we need
+#define __packed                // does nothing in Windows
+
+#include "use14_ioc.h"          // links to device driver stuff
+#endif
+
+#ifdef LINUX
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <errno.h>
+#include <sys/time.h>
+#include <sched.h>
+#include <libgen.h>
+#define PATHSEP '/'
+#define PATHSEPSTR "/"
+#define DEFCMDPATH "/var/1401/" // default command path if all else fails
+#define MINDRIVERMAJREV 2       // minimum driver revision level we need
+
+#include "ced_ioctl.h"          // links to device driver stuff
+#endif
+
+#define MAX1401         8       // The number of 1401s that can be supported
+
+/*
+** These are the 1401 type codes returned by the driver, they are a slightly
+** odd sequence & start for reasons of compatability with the DOS driver.
+** The maximum code value is the upper limit of 1401 device types.
+*/
+#define DRIVRET_STD     4       // Codes for 1401 types matching driver values
+#define DRIVRET_U1401   5       // This table does not need extending, as
+#define DRIVRET_PLUS    6       // we can calculate values now.
+#define DRIVRET_POWER   7       // but we need all of these values still
+#define DRIVRET_MAX     26      // Maximum tolerated code - future designs
+
+/*
+** These variables store data that will be used to generate the last
+** error string. For now, a string will hold the 1401 command file name.
+*/
+static char szLastName[20];     // additional text information
+
+/*
+** Information stored per handle. NBNB, driverType and DriverVersion used to be
+** only stored once for all handles... i.e. nonsensical. This change means that
+** three U14...() calls now include handles that were previously void. We have
+** set a constructor and a destructor call for the library (see the end) to
+** initialise important structures, or call use1401_load().
+*/
+static short asDriverType[MAX1401] = {0};
+static int lLastDriverVersion = U14ERR_NO1401DRIV;
+static int lLastDriverType = U14TYPEUNKNOWN;
+static int alDriverVersion[MAX1401];            // version/type of each driver
+static int alTimeOutPeriod[MAX1401];            // timeout time in milliseconds
+static short asLastRetCode[MAX1401];            // last code from a fn call
+static short asType1401[MAX1401] = {0};         // The type of the 1401
+static BOOL abGrabbed[MAX1401] = {0};           // Flag for grabbed, set true by grab1401
+static int iAttached = 0;                       // counts process attaches so can let go
+
+#ifdef _IS_WINDOWS_
+/****************************************************************************
+** Windows NT Specific Variables and internal types
+****************************************************************************/
+static HANDLE aHand1401[MAX1401] = {0};         // handles for 1401s
+static HANDLE aXferEvent[MAX1401] = {0};        // transfer events for the 1401s
+static LPVOID apAreas[MAX1401][MAX_TRANSAREAS]; // Locked areas
+static DWORD  auAreas[MAX1401][MAX_TRANSAREAS]; // Size of locked areas
+static BOOL   bWindows9x = FALSE;               // if we are Windows 95 or better
+#ifdef _WIN64
+#define USE_NT_DIOC(ind) TRUE
+#else
+static BOOL   abUseNTDIOC[MAX1401];             // Use NT-style DIOC parameters */
+#define USE_NT_DIOC(ind) abUseNTDIOC[ind]
+#endif
+
+#endif
+
+#ifdef LINUX
+static int aHand1401[MAX1401] = {0};    // handles for 1401s
+#define INVALID_HANDLE_VALUE 0          // to avoid code differences
+#endif
+
+
+/*
+** The CmdHead relates to backwards compatibility with ancient Microsoft (and Sperry!)
+** versions of BASIC, where this header was needed so we could load a command into
+** memory.
+*/
+#pragma pack(1)                 // pack our structure
+typedef struct CmdHead          // defines header block on command
+{                               // for PC commands
+   char   acBasic[5];           // BASIC information - needed to align things
+   WORD   wBasicSz;             // size as seen by BASIC
+   WORD   wCmdSize;             // size of the following info
+} __packed CMDHEAD;
+#pragma pack()                  // back to normal
+
+/*
+** The rest of the header looks like this...
+**  int    iRelPnt;             relocation pointer... actual start
+**  char   acName[8];           string holding the command name
+**  BYTE   bMonRev;             monitor revision level
+**  BYTE   bCmdRev;             command revision level
+*/
+
+typedef CMDHEAD *LPCMDHEAD;     // pointer to a command header
+
+#define  MAXSTRLEN   255        // maximum string length we use
+#define  TOHOST      FALSE
+#define  TO1401      TRUE
+
+static short CheckHandle(short h)
+{
+    if ((h < 0) || (h >= MAX1401))  // must be legal range...
+        return U14ERR_BADHAND;
+    if (aHand1401[h] <= 0)          // must be open
+        return U14ERR_BADHAND;
+    return U14ERR_NOERROR;
+}
+
+#ifdef _IS_WINDOWS_
+/****************************************************************************
+** U14Status1401    Used for functions which do not pass any data in but
+**                  get data back
+****************************************************************************/
+static short U14Status1401(short sHand, LONG lCode, TCSBLOCK* pBlk)
+{
+    DWORD dwBytes = 0;
+
+    if ((sHand < 0) || (sHand >= MAX1401))  /* Check parameters */
+        return U14ERR_BADHAND;
+#ifndef _WIN64
+    if (!USE_NT_DIOC(sHand)) 
+    {   /* Windows 9x DIOC methods? */
+        if (DeviceIoControl(aHand1401[sHand], lCode, NULL, 0, pBlk,sizeof(TCSBLOCK),&dwBytes,NULL))
+            return (short)((dwBytes>=sizeof(TCSBLOCK)) ? U14ERR_NOERROR : U14ERR_DRIVCOMMS);
+        else
+            return (short)GetLastError();
+    }
+    else
+#endif
+    {                                       /* Windows NT or USB driver */
+        PARAMBLK rWork;
+        rWork.sState = U14ERR_DRIVCOMMS;
+        if (DeviceIoControl(aHand1401[sHand], lCode, NULL, 0, &rWork,sizeof(PARAMBLK),&dwBytes,NULL) &&
+            (dwBytes >= sizeof(PARAMBLK)))
+        {
+            *pBlk = rWork.csBlock;
+            return rWork.sState;
+        }
+    }
+
+    return U14ERR_DRIVCOMMS;
+}
+
+/****************************************************************************
+** U14Control1401   Used for functions which pass data in and only expect
+**                  an error code back
+****************************************************************************/
+static short U14Control1401(short sHand, LONG lCode, TCSBLOCK* pBlk)
+{
+    DWORD dwBytes = 0;
+
+    if ((sHand < 0) || (sHand >= MAX1401))              /* Check parameters */
+        return U14ERR_BADHAND;
+
+#ifndef _WIN64
+    if (!USE_NT_DIOC(sHand))                    
+    {                            /* Windows 9x DIOC methods */
+        if (DeviceIoControl(aHand1401[sHand], lCode, NULL, 0, pBlk, sizeof(TCSBLOCK), &dwBytes, NULL))
+            return (short)(dwBytes >= sizeof(TCSBLOCK) ? U14ERR_NOERROR : U14ERR_DRIVCOMMS);
+        else
+            return (short)GetLastError();
+    }
+    else
+#endif
+    {                            /* Windows NT or later */
+        PARAMBLK rWork;
+        rWork.sState = U14ERR_DRIVCOMMS;
+        if (DeviceIoControl(aHand1401[sHand], lCode, pBlk, sizeof(TCSBLOCK), &rWork, sizeof(PARAMBLK), &dwBytes, NULL) &&
+            (dwBytes >= sizeof(PARAMBLK)))
+            return rWork.sState;
+    }
+
+    return U14ERR_DRIVCOMMS;
+}
+#endif
+
+/****************************************************************************
+** SafeTickCount
+** Gets time in approximately units of a millisecond.
+*****************************************************************************/
+static long SafeTickCount()
+{
+#ifdef _IS_WINDOWS_
+    return GetTickCount();
+#endif
+#ifdef LINUX
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    return (tv.tv_sec*1000 + tv.tv_usec/1000);
+#endif
+}
+
+/****************************************************************************
+** A utility routine to get the command file extension for a given type
+** of 1401. We assume the type code is vaguely legal.
+****************************************************************************/
+static int ExtForType(short sType, char* szExt)
+{
+    szExt[0] = 0;                       /* Default return is a blank string */
+    switch (sType)
+    {
+    case U14TYPE1401: strcpy(szExt, ".CMD");  break;    // Standard 1401
+    case U14TYPEPLUS: strcpy(szExt, ".GXC");  break;    // 1401 plus
+    default:               // All others are in a predictable sequence
+        strcpy(szExt, ".ARM");
+            szExt[3] = (char)('M' + sType - U14TYPEU1401);
+        if (szExt[3] > 'Z')             // Wrap round to ARA after ARZ
+                szExt[3] = (char)(szExt[3] - 26);
+    }
+    return 0;
+}
+
+/****************************************************************************
+**   U14WhenToTimeOut
+**       Returns the time to time out in time units suitable for the machine
+** we are running on  ie millsecs for pc/linux, or Mac/
+****************************************************************************/
+U14API(int) U14WhenToTimeOut(short hand)
+{
+    int iNow = SafeTickCount();
+    if ((hand >= 0) && (hand < MAX1401))
+        iNow += alTimeOutPeriod[hand];
+    return iNow;
+}
+
+/****************************************************************************
+** U14PassedTime
+** Returns non zero if the timed passed in has been passed 0 if not
+****************************************************************************/
+U14API(short) U14PassedTime(int lCheckTime)
+{
+    return (short)((SafeTickCount()-lCheckTime) > 0);
+}
+
+/****************************************************************************
+** TranslateString
+** Tidies up string that U14GetString returns. Converts all the commas in a
+** string to spaces. Removes terminating CR character. May do more in future.
+****************************************************************************/
+static void TranslateString(char* pStr)
+{
+    int i = 0;
+    while (pStr[i])
+    {
+        if (pStr[i] == ',')
+            pStr[i] = ' ';              /* convert comma to space */
+        ++i;
+    }
+
+    if ((i > 0) && (pStr[i-1] == '\n'))  /* kill terminating LF */
+        pStr[i-1] = (char)0;
+}
+
+/****************************************************************************
+** U14StrToLongs
+** Converts a string to an array of longs and returns the number of values
+****************************************************************************/
+U14API(short) U14StrToLongs(const char* pszBuff, U14LONG *palNums, short sMaxLongs)
+{
+    WORD wChInd = 0;                // index into source
+    short sLgInd = 0;               // index into result longs
+
+    while (pszBuff[wChInd] &&       // until we get to end of string...
+           (sLgInd < sMaxLongs))    // ...or filled the buffer
+    {
+        // Why not use a C Library converter?
+        switch (pszBuff[wChInd])
+        {
+        case '-':
+        case '0': case '1':   case '2': case '3':   case '4':
+        case '5': case '6':   case '7': case '8':   case '9':
+            {
+                BOOL bDone = FALSE; // true at end of number
+                int iSign = 1;      // sign of number
+                long lValue = 0;
+
+                while ((!bDone) && pszBuff[wChInd])
+                {
+                    switch (pszBuff[wChInd])
+                    {
+                    case '-':
+                        iSign = -1; // swap sign
+                        break;
+
+                    case '0': case '1':   case '2': case '3':   case '4':
+                    case '5': case '6':   case '7': case '8':   case '9':
+                        lValue *= 10;   // move to next digit base 10
+                        lValue += ((int)pszBuff[wChInd]-(int)'0');
+                        break;
+
+                    default:        // end of number
+                        bDone = TRUE;
+                        break;
+                    }
+                    wChInd++;       // move onto next character
+                }
+                palNums[sLgInd] = lValue * iSign;
+                sLgInd++;
+            }
+            break;
+
+        default:
+            wChInd++;               // look at next char
+            break;
+        }
+    }
+    return (sLgInd);
+}
+
+
+/****************************************************************************
+** U14LongsFrom1401
+** Gets the next waiting line from the 1401 and converts it longs
+** Returns the number of numbers read or an error.
+****************************************************************************/
+U14API(short) U14LongsFrom1401(short hand, U14LONG *palBuff, short sMaxLongs)
+{
+    char szWork[MAXSTRLEN];
+    short sResult = U14GetString(hand, szWork, MAXSTRLEN);/* get reply from 1401   */
+    if (sResult == U14ERR_NOERROR)                  /* if no error convert   */
+        sResult = U14StrToLongs(szWork, palBuff, sMaxLongs);
+    return sResult;
+}
+
+/****************************************************************************
+**   U14CheckErr
+**   Sends the ERR command to the 1401 and gets the result. Returns 0, a
+**   negative error code, or the first error value.
+****************************************************************************/
+U14API(short) U14CheckErr(short hand)
+{
+    short sResult = U14SendString(hand, ";ERR;");
+    if (sResult == U14ERR_NOERROR)
+    {
+        U14LONG er[3];
+        sResult = U14LongsFrom1401(hand, er, 3);
+        if (sResult > 0)
+        {
+            sResult = (short)er[0];        /* Either zero or an error value */
+#ifdef _DEBUG
+            if (er[0] != 0)
+            {
+                char szMsg[50];
+                sprintf(szMsg, "U14CheckErr returned %d,%d\n", er[0], er[1]);
+                OutputDebugString(szMsg);
+            }
+#endif
+        }
+        else
+        {
+            if (sResult == 0)
+                sResult = U14ERR_TIMEOUT;      /* No numbers equals timeout */
+        }
+    }
+
+    return sResult;
+}
+
+/****************************************************************************
+** U14LastErrCode
+** Returns the last code from the driver. This is for Windows where all calls
+** go through the Control and Status routines, so we can save any error.
+****************************************************************************/
+U14API(short) U14LastErrCode(short hand)
+{
+    if ((hand < 0) || (hand >= MAX1401))
+        return U14ERR_BADHAND;
+    return asLastRetCode[hand];
+}
+
+/****************************************************************************
+** U14SetTimeout
+** Set the timeout period for 1401 comms in milliseconds
+****************************************************************************/
+U14API(void) U14SetTimeout(short hand, int lTimeOut)
+{
+    if ((hand < 0) || (hand >= MAX1401))
+        return;
+    alTimeOutPeriod[hand] = lTimeOut;
+}
+
+/****************************************************************************
+** U14GetTimeout
+** Get the timeout period for 1401 comms in milliseconds
+****************************************************************************/
+U14API(int) U14GetTimeout(short hand)
+{
+    if ((hand < 0) || (hand >= MAX1401))
+        return U14ERR_BADHAND;
+    return alTimeOutPeriod[hand];
+}
+
+/****************************************************************************
+** U14OutBufSpace
+** Return the space in the output buffer, or an error.
+****************************************************************************/
+U14API(short) U14OutBufSpace(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_GETOUTBUFSPACE,&csBlock);
+    if (sErr == U14ERR_NOERROR)
+        sErr = csBlock.ints[0];
+    return sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_GetOutBufSpace(aHand1401[hand]) : sErr;
+#endif
+}
+
+
+/****************************************************************************
+** U14BaseAddr1401
+** Returns the 1401 base address or an error code. Meaningless nowadays
+****************************************************************************/
+U14API(int) U14BaseAddr1401(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    int iError = U14Status1401(hand, U14_GETBASEADDRESS,&csBlock);
+    if (iError == U14ERR_NOERROR)
+        iError = csBlock.longs[0];
+    return iError;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_GetBaseAddress(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14StateOf1401
+** Return error state, either NOERROR or a negative code.
+****************************************************************************/
+U14API(short) U14StateOf1401(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_STATEOF1401, &csBlock);
+    if (sErr == U14ERR_NOERROR)
+    {
+        sErr = csBlock.ints[0];      // returned 1401 state
+        if ((sErr >= DRIVRET_STD) && (sErr <= DRIVRET_MAX))
+            sErr = U14ERR_NOERROR;
+    }
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        sErr = (short)CED_StateOf1401(aHand1401[hand]);
+        if ((sErr >= DRIVRET_STD) && (sErr <= DRIVRET_MAX))
+            sErr = U14ERR_NOERROR;
+    }
+#endif
+    return sErr;
+}
+
+/****************************************************************************
+** U14DriverVersion
+** Returns the driver version. Hi word is major revision, low word is minor.
+** If you pass in a silly handle (like -1), we return the version of the last
+** driver we know of (to cope with PCI and no 1401 attached).
+****************************************************************************/
+U14API(int) U14DriverVersion(short hand)
+{
+    return CheckHandle(hand) != U14ERR_NOERROR ? lLastDriverVersion : alDriverVersion[hand];
+}
+
+/****************************************************************************
+** U14DriverType
+** Returns the driver type. The type, 0=ISA/NU-Bus, 1=PCI, 2=USB, 3=HSS
+** If you pass in a silly handle (like -1), we return the type of the last
+** driver we know of (to cope with PCI and no 1401 attached).
+****************************************************************************/
+U14API(int) U14DriverType(short hand)
+{
+    return CheckHandle(hand) != U14ERR_NOERROR ? lLastDriverType : asDriverType[hand];
+}
+
+/****************************************************************************
+** U14DriverName
+** Returns the driver type as 3 character (ISA, PCI, USB or HSS))
+****************************************************************************/
+U14API(short) U14DriverName(short hand, char* pBuf, WORD wMax)
+{
+    char* pName;
+    *pBuf = 0;                             // Start off with a blank string
+    switch (U14DriverType(hand))           // Results according to type
+    {
+    case 0:  pName = "ISA"; break;
+    case 1:  pName = "PCI"; break;
+    case 2:  pName = "USB"; break;
+    case 3:  pName = "HSS"; break;
+    default: pName = "???"; break;
+    }
+    strncpy(pBuf, pName, wMax);            // Copy the correct name to return
+
+    return U14ERR_NOERROR;
+}
+
+/****************************************************************************
+** U14BlkTransState
+** Returns 0 no transfer in progress, 1 transfer in progress or an error code
+****************************************************************************/
+U14API(short) U14BlkTransState(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_BLKTRANSSTATE, &csBlock);
+    if (sErr == U14ERR_NOERROR)
+        sErr = csBlock.ints[0];
+    return sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_BlkTransState(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14Grab1401
+** Take control of the 1401 for diagnostics purposes. USB does nothing.
+****************************************************************************/
+U14API(short) U14Grab1401(short hand)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+#ifdef _IS_WINDOWS_
+        if (abGrabbed[hand])            // 1401 should not have been grabbed
+            sErr = U14ERR_ALREADYSET;   // Error code defined for this
+        else
+        {
+            TCSBLOCK csBlock;
+            sErr = U14Control1401(hand, U14_GRAB1401, &csBlock);
+        }
+#endif
+#ifdef LINUX
+        // 1401 should not have been grabbed
+        sErr = abGrabbed[hand] ? U14ERR_ALREADYSET : CED_Grab1401(aHand1401[hand]);
+#endif
+        if (sErr == U14ERR_NOERROR)
+            abGrabbed[hand] = TRUE;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14Free1401
+****************************************************************************/
+U14API(short)  U14Free1401(short hand)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+#ifdef _IS_WINDOWS_
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+            TCSBLOCK csBlock;
+            sErr = U14Control1401(hand, U14_FREE1401, &csBlock);
+        }
+        else
+            sErr = U14ERR_NOTSET;
+#endif
+#ifdef LINUX
+        // 1401 should not have been grabbed
+        sErr = abGrabbed[hand] ? CED_Free1401(aHand1401[hand]) : U14ERR_NOTSET;
+#endif
+        if (sErr == U14ERR_NOERROR)
+            abGrabbed[hand] = FALSE;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14Peek1401
+** DESCRIPTION  Cause the 1401 to do one or more peek operations.
+** If lRepeats is zero, the loop will continue until U14StopDebugLoop
+** is called. After the peek is done, use U14GetDebugData to retrieve
+** the results of the peek.
+****************************************************************************/
+U14API(short) U14Peek1401(short hand, DWORD dwAddr, int nSize, int nRepeats)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+#ifdef _IS_WINDOWS_
+            TCSBLOCK csBlock;
+            csBlock.longs[0] = (long)dwAddr;
+            csBlock.longs[1] = nSize;
+            csBlock.longs[2] = nRepeats;
+            sErr = U14Control1401(hand, U14_DBGPEEK, &csBlock);
+#endif
+#ifdef LINUX
+            TDBGBLOCK dbb;
+            dbb.iAddr = (int)dwAddr;
+            dbb.iWidth = nSize;
+            dbb.iRepeats = nRepeats;
+            sErr = CED_DbgPeek(aHand1401[hand], &dbb);
+#endif
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14Poke1401
+** DESCRIPTION  Cause the 1401 to do one or more poke operations.
+** If lRepeats is zero, the loop will continue until U14StopDebugLoop
+** is called.
+****************************************************************************/
+U14API(short) U14Poke1401(short hand, DWORD dwAddr, DWORD dwValue,
+                                      int nSize, int nRepeats)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+#ifdef _IS_WINDOWS_
+            TCSBLOCK csBlock;
+            csBlock.longs[0] = (long)dwAddr;
+            csBlock.longs[1] = nSize;
+            csBlock.longs[2] = nRepeats;
+            csBlock.longs[3] = (long)dwValue;
+            sErr = U14Control1401(hand, U14_DBGPOKE, &csBlock);
+#endif
+#ifdef LINUX
+            TDBGBLOCK dbb;
+            dbb.iAddr = (int)dwAddr;
+            dbb.iWidth = nSize;
+            dbb.iRepeats= nRepeats;
+            dbb.iData = (int)dwValue;
+            sErr = CED_DbgPoke(aHand1401[hand], &dbb);
+#endif
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14Ramp1401
+** DESCRIPTION  Cause the 1401 to loop, writing a ramp to a location.
+** If lRepeats is zero, the loop will continue until U14StopDebugLoop.
+****************************************************************************/
+U14API(short) U14Ramp1401(short hand, DWORD dwAddr, DWORD dwDef, DWORD dwEnable,
+                                      int nSize, int nRepeats)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+#ifdef _IS_WINDOWS_
+            TCSBLOCK csBlock;
+            csBlock.longs[0] = (long)dwAddr;
+            csBlock.longs[1] = (long)dwDef;
+            csBlock.longs[2] = (long)dwEnable;
+            csBlock.longs[3] = nSize;
+            csBlock.longs[4] = nRepeats;
+            sErr = U14Control1401(hand, U14_DBGRAMPDATA, &csBlock);
+#endif
+#ifdef LINUX
+            TDBGBLOCK dbb;
+            dbb.iAddr = (int)dwAddr;
+            dbb.iDefault = (int)dwDef;
+            dbb.iMask = (int)dwEnable;
+            dbb.iWidth = nSize;
+            dbb.iRepeats = nRepeats;
+            sErr = CED_DbgRampAddr(aHand1401[hand], &dbb);
+#endif
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14RampAddr
+** DESCRIPTION  Cause the 1401 to loop, reading from a ramping location.
+** If lRepeats is zero, the loop will continue until U14StopDebugLoop
+****************************************************************************/
+U14API(short) U14RampAddr(short hand, DWORD dwDef, DWORD dwEnable,
+                                      int nSize, int nRepeats)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+#ifdef _IS_WINDOWS_
+            TCSBLOCK csBlock;
+            csBlock.longs[0] = (long)dwDef;
+            csBlock.longs[1] = (long)dwEnable;
+            csBlock.longs[2] = nSize;
+            csBlock.longs[3] = nRepeats;
+            sErr = U14Control1401(hand, U14_DBGRAMPADDR, &csBlock);
+#endif
+#ifdef LINUX
+            TDBGBLOCK dbb;
+            dbb.iDefault = (int)dwDef;
+            dbb.iMask = (int)dwEnable;
+            dbb.iWidth = nSize;
+            dbb.iRepeats = nRepeats;
+            sErr = CED_DbgRampAddr(aHand1401[hand], &dbb);
+#endif
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+**    U14StopDebugLoop
+**    DESCRIPTION Stops a peek\poke\ramp that, with repeats set to zero,
+**    will otherwise continue forever.
+****************************************************************************/
+U14API(short) U14StopDebugLoop(short hand)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+#ifdef _IS_WINDOWS_
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+            TCSBLOCK csBlock;
+            sErr = U14Control1401(hand, U14_DBGSTOPLOOP, &csBlock);
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+#endif
+#ifdef LINUX
+        sErr = abGrabbed[hand] ? CED_DbgStopLoop(aHand1401[hand]) : U14ERR_NOTSET;
+#endif
+    return sErr;
+}
+
+/****************************************************************************
+** U14GetDebugData
+** DESCRIPTION Returns the result from a previous peek operation.
+****************************************************************************/
+U14API(short) U14GetDebugData(short hand, U14LONG* plValue)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        if (abGrabbed[hand])    // 1401 should have been grabbed
+        {
+#ifdef _IS_WINDOWS_
+            TCSBLOCK csBlock;
+            sErr = U14Status1401(hand, U14_DBGGETDATA, &csBlock);
+            if (sErr == U14ERR_NOERROR)
+                *plValue = csBlock.longs[0];    // Return the data
+#endif
+#ifdef LINUX
+            TDBGBLOCK dbb;
+            sErr = CED_DbgGetData(aHand1401[hand], &dbb);
+            if (sErr == U14ERR_NOERROR)
+                *plValue = dbb.iData;                     /* Return the data */
+#endif
+        }
+        else
+            sErr = U14ERR_NOTSET;
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14StartSelfTest
+****************************************************************************/
+U14API(short) U14StartSelfTest(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    return U14Control1401(hand, U14_STARTSELFTEST, &csBlock);
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_StartSelfTest(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14CheckSelfTest
+****************************************************************************/
+U14API(short) U14CheckSelfTest(short hand, U14LONG *pData)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_CHECKSELFTEST, &csBlock);
+    if (sErr == U14ERR_NOERROR)
+    {
+        pData[0] = csBlock.longs[0];        /* Return the results to user */
+        pData[1] = csBlock.longs[1];
+        pData[2] = csBlock.longs[2];
+    }
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)                /* Check parameters */
+    {
+        TGET_SELFTEST gst;
+        sErr = CED_CheckSelfTest(aHand1401[hand], &gst);
+        if (sErr == U14ERR_NOERROR)
+        {
+            pData[0] = gst.code;        /* Return the results to user */
+            pData[1] = gst.x;
+            pData[2] = gst.y;
+        }
+    }
+#endif
+    return sErr;
+}
+
+/****************************************************************************
+** U14GetUserMemorySize
+****************************************************************************/
+U14API(short) U14GetUserMemorySize(short hand, DWORD *pMemorySize)
+{
+    // The original 1401 used a different command for getting the size
+    short sErr = U14SendString(hand, (asType1401[hand] == U14TYPE1401) ? "MEMTOP;" : "MEMTOP,?;");
+    *pMemorySize = 0;         /* if we get error then leave size set at 0  */
+    if (sErr == U14ERR_NOERROR)
+    {
+        U14LONG alLimits[4];
+        sErr = U14LongsFrom1401(hand, alLimits, 4);
+        if (sErr > 0)              /* +ve sErr is the number of values read */
+        {
+            sErr = U14ERR_NOERROR;                  /* All OK, flag success */
+            if (asType1401[hand] == U14TYPE1401)    /* result for standard  */
+                *pMemorySize = alLimits[0] - alLimits[1]; /* memtop-membot */
+            else
+                *pMemorySize = alLimits[0];   /* result for plus or u1401  */
+        }
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14TypeOf1401
+** Returns the type of the 1401, maybe unknown
+****************************************************************************/
+U14API(short) U14TypeOf1401(short hand)
+{
+    if ((hand < 0) || (hand >= MAX1401))                /* Check parameters */
+        return U14ERR_BADHAND;
+    else
+        return asType1401[hand];
+}
+
+/****************************************************************************
+** U14NameOf1401
+** Returns the type of the 1401 as a string, blank if unknown
+****************************************************************************/
+U14API(short) U14NameOf1401(short hand, char* pBuf, WORD wMax)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+    char* pName;
+    switch (asType1401[hand])               // Results according to type
+    {
+    case U14TYPE1401:  pName = "Std 1401"; break;
+    case U14TYPEPLUS:  pName = "1401plus"; break;
+    case U14TYPEU1401: pName = "micro1401"; break;
+    case U14TYPEPOWER: pName = "Power1401"; break;
+    case U14TYPEU14012:pName = "Micro1401 mk II"; break;
+    case U14TYPEPOWER2:pName = "Power1401 mk II"; break;
+    case U14TYPEU14013:pName = "Micro1401-3"; break;
+    case U14TYPEPOWER3:pName = "Power1401-3"; break;
+    default:           pName = "Unknown";
+    }
+        strncpy(pBuf, pName, wMax);
+    }
+    return sErr;
+}
+
+/****************************************************************************
+** U14TransferFlags
+**  Returns the driver block transfer flags.
+**  Bits can be set - see U14TF_ constants in use1401.h
+*****************************************************************************/
+U14API(short) U14TransferFlags(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_TRANSFERFLAGS, &csBlock);
+    return (sErr == U14ERR_NOERROR) ? (short)csBlock.ints[0] : sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_TransferFlags(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** GetDriverVersion
+** Actually reads driver version from the device driver.
+** Hi word is major revision, low word is minor revision.
+** Assumes that hand has been checked. Also codes driver type in bits 24 up.
+*****************************************************************************/
+static int GetDriverVersion(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    int iErr = U14Status1401(hand, U14_GETDRIVERREVISION, &csBlock);
+    if (iErr == U14ERR_NOERROR)
+        iErr = csBlock.longs[0];
+    return iErr;
+#endif
+#ifdef LINUX
+    return CED_GetDriverRevision(aHand1401[hand]);
+#endif
+}
+
+/****************************************************************************
+** U14MonitorRev
+** Returns the 1401 monitor revision number.
+** The number returned is the minor revision - the part after the
+** decimal point - plus the major revision times 1000.
+*****************************************************************************/
+U14API(int) U14MonitorRev(short hand)
+{
+    int iRev = 0;
+    int iErr = CheckHandle(hand);
+    if (iErr != U14ERR_NOERROR)                 // Check open and in use
+        return iErr;
+
+    if (asType1401[hand] >= U14TYPEPOWER2)      // The Power2 onwards can give us the monitor
+    {                                           //  revision directly for all versions
+        iErr = U14SendString(hand, "INFO,S,28;");
+        if (iErr == U14ERR_NOERROR)
+        {
+            U14LONG lVals[2];                   // Read a single number being the revision
+            iErr = U14LongsFrom1401(hand, lVals, 1);
+            if (iErr > 0)
+            {
+                iErr = U14ERR_NOERROR;
+                iRev = lVals[0];                // This is the minor part of the revision
+                iRev += asType1401[hand] * 10000;
+            }
+        }
+    }
+    else
+    {                                           /* Do it the hard way for older hardware */
+        iErr = U14SendString(hand, ";CLIST;");     /* ask for command levels */
+        if (iErr == U14ERR_NOERROR)
+        {     
+            while (iErr == U14ERR_NOERROR)
+            {
+                char wstr[50];
+                iErr = U14GetString(hand, wstr, 45);
+                if (iErr == U14ERR_NOERROR)
+                {
+                    char *pstr = strstr(wstr,"RESET");  /* Is this the RESET command? */
+                    if ((pstr == wstr) && (wstr[5] == ' '))
+                    {
+                        char *pstr2;
+                        size_t l;
+                        pstr += 6;       /* Move past RESET and followinmg char */
+                        l = strlen(pstr);       /* The length of text remaining */
+                        while (((pstr[l-1] == ' ') || (pstr[l-1] == 13)) && (l > 0))
+                        {
+                            pstr[l-1] = 0;         /* Tidy up string at the end */
+                            l--;                  /* by removing spaces and CRs */
+                        }
+                        pstr2 = strchr(pstr, '.');    /* Find the decimal point */
+                        if (pstr2 != NULL)                /* If we found the DP */
+                        {
+                            *pstr2 = 0;                /* End pstr string at DP */
+                            pstr2++;              /* Now past the decimal point */
+                            iRev = atoi(pstr2);   /* Get the number after point */
+                        }
+                        iRev += (atoi(pstr) * 1000);    /* Add first bit * 1000 */
+                    }
+                    if ((strlen(wstr) < 3) && (wstr[0] == ' '))
+                        break;              /* Spot the last line of results */
+                }
+            }
+        }
+    }
+    if (iErr == U14ERR_NOERROR)            /* Return revision if no error */
+        iErr = iRev;
+
+    return iErr;
+}
+
+/****************************************************************************
+** U14TryToOpen     Tries to open the 1401 number passed
+**  Note : This will succeed with NT driver even if no I/F card or
+**         1401 switched off, so we check state and close the driver
+**         if the state is unsatisfactory in U14Open1401.
+****************************************************************************/
+#ifdef _IS_WINDOWS_
+#define U14NAMEOLD "\\\\.\\CED_140%d"
+#define U14NAMENEW "\\\\.\\CED%d"
+static short U14TryToOpen(int n1401, long* plRetVal, short* psHandle)
+{
+    short sErr = U14ERR_NOERROR;
+    HANDLE hDevice = INVALID_HANDLE_VALUE;
+    DWORD dwErr = 0;
+    int nFirst, nLast, nDev = 0;        /* Used for the search for a 1401 */
+    BOOL bOldName = FALSE;               /* start by looking for a modern driver */
+
+    if (n1401 == 0)                             /* If we need to look for a 1401 */
+    {
+        nFirst = 1;                             /* Set the search range */
+        nLast = MAX1401;                        /* through all the possible 1401s */
+    }
+    else
+        nFirst = nLast = n1401;                 /* Otherwise just one 1401 */
+
+    while (hDevice == INVALID_HANDLE_VALUE)     /* Loop to try for a 1401 */
+    {
+        for (nDev = nFirst; nDev <= nLast; nDev++)
+        {
+            char szDevName[40];                 /* name of the device to open */
+            sprintf(szDevName, bOldName ? U14NAMEOLD : U14NAMENEW, nDev);
+            hDevice = CreateFile(szDevName, GENERIC_WRITE | GENERIC_READ,
+                                 0, 0,          /* Unshared mode does nothing as this is a device */
+                                 OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
+
+            if (hDevice != INVALID_HANDLE_VALUE)/* Check 1401 if opened */
+            {
+                TCSBLOCK csBlock;
+                assert(aHand1401[nDev-1] == INVALID_HANDLE_VALUE);  // assert if already open
+                aHand1401[nDev-1] = hDevice;    /* Save handle for now */
+
+#ifndef _WIN64
+                // Use DIOC method if not windows 9x or if using new device name
+                abUseNTDIOC[nDev-1] = (BOOL)(!bWindows9x || !bOldName);
+#endif
+                sErr = U14Status1401((short)(nDev-1), U14_TYPEOF1401, &csBlock);
+                if (sErr == U14ERR_NOERROR)
+                {
+                    *plRetVal = csBlock.ints[0];
+                    if (csBlock.ints[0] == U14ERR_INUSE)/* Prevent multi opens */
+                    {
+                        CloseHandle(hDevice);   /* treat as open failure */
+                        hDevice = INVALID_HANDLE_VALUE;
+                        aHand1401[nDev-1] = INVALID_HANDLE_VALUE;
+                        sErr = U14ERR_INUSE;
+                    }
+                    else
+                        break;                  /* Exit from for loop on success */
+                }
+                else
+                {
+                    CloseHandle(hDevice);       /* Give up if func fails */
+                    hDevice = INVALID_HANDLE_VALUE;
+                    aHand1401[nDev-1] = INVALID_HANDLE_VALUE;
+                }
+            }
+            else
+            {
+                DWORD dwe = GetLastError();     /* Get error code otherwise */
+                if ((dwe != ERROR_FILE_NOT_FOUND) || (dwErr == 0))
+                    dwErr = dwe;                /* Ignore repeats of 'not found' */
+            }
+        }
+
+        if ((hDevice == INVALID_HANDLE_VALUE) &&/* No device found, and... */
+            (bWindows9x) &&                     /* ...old names are allowed, and... */
+            (bOldName == FALSE))                /* ...not tried old names yet */
+            bOldName = TRUE;                    /* Set flag and go round again */
+        else
+            break;                              /* otherwise that's all folks */
+    }
+
+    if (hDevice != INVALID_HANDLE_VALUE)        /* If we got our device open */
+        *psHandle = (short)(nDev-1);            /* return 1401 number opened */
+    else
+    {
+        if (dwErr == ERROR_FILE_NOT_FOUND)      /* Sort out the error codes */
+            sErr = U14ERR_NO1401DRIV;           /* if file not found */
+        else if (dwErr == ERROR_NOT_SUPPORTED)
+            sErr = U14ERR_DRIVTOOOLD;           /* if DIOC not supported */
+        else if (dwErr == ERROR_ACCESS_DENIED)
+            sErr = U14ERR_INUSE;
+        else
+            sErr = U14ERR_DRIVCOMMS;            /* otherwise assume comms problem */
+    }
+    return sErr;
+}
+#endif
+#ifdef LINUX
+static short U14TryToOpen(int n1401, long* plRetVal, short* psHandle)
+{
+    short sErr = U14ERR_NOERROR;
+    int fh = 0;                             // will be 1401 handle
+    int iErr = 0;
+    int nFirst, nLast, nDev = 0;            // Used for the search for a 1401
+
+    if (n1401 == 0)                         // If we need to look for a 1401
+    {
+        nFirst = 1;                             /* Set the search range */
+        nLast = MAX1401;                        /* through all the possible 1401s */
+    }
+    else
+        nFirst = nLast = n1401;                 /* Otherwise just one 1401 */
+
+    for (nDev = nFirst; nDev <= nLast; nDev++)
+    {
+        char szDevName[40];                 // name of the device to open
+        sprintf(szDevName,"/dev/cedusb/%d", nDev-1);
+        fh = open(szDevName, O_RDWR);       // can only be opened once at a time
+        if (fh > 0)                         // Check 1401 if opened
+        {
+            int iType1401 = CED_TypeOf1401(fh); // get 1401 type
+            aHand1401[nDev-1] = fh;         // Save handle for now
+            if (iType1401 >= 0)
+            {
+                *plRetVal = iType1401;
+                 break;                     // Exit from for loop on success
+            }
+            else
+            {
+                close(fh);                  // Give up if func fails
+                fh = 0;
+                aHand1401[nDev-1] = 0;
+            }
+        }
+        else
+        {
+            if (((errno != ENODEV) && (errno != ENOENT)) || (iErr == 0))
+                iErr = errno;                // Ignore repeats of 'not found'
+        }
+    }
+
+
+    if (fh)                                 // If we got our device open
+        *psHandle = (short)(nDev-1);        // return 1401 number opened
+    else
+    {
+        if ((iErr == ENODEV) || (iErr == ENOENT)) // Sort out the error codes
+            sErr = U14ERR_NO1401DRIV;       // if file not found
+        else if (iErr == EBUSY)
+            sErr = U14ERR_INUSE;
+        else
+            sErr = U14ERR_DRIVCOMMS;        // otherwise assume comms problem
+    }
+
+    return sErr;
+}
+#endif
+/****************************************************************************
+** U14Open1401
+** Tries to get the 1401 for use by this application
+*****************************************************************************/
+U14API(short) U14Open1401(short n1401)
+{
+    long     lRetVal = -1;
+    short    sErr;
+    short    hand = 0;
+    
+    if ((n1401 < 0) || (n1401 > MAX1401))       // must check the 1401 number
+        return U14ERR_BAD1401NUM;
+
+    szLastName[0] = 0;          /* initialise the error info string */
+
+    sErr = U14TryToOpen(n1401, &lRetVal, &hand);
+    if (sErr == U14ERR_NOERROR)
+    {
+        long lDriverVersion = GetDriverVersion(hand);   /* get driver revision */
+        long lDriverRev = -1;
+		if (lDriverVersion >= 0)                    /* can use it if all OK */
+        {
+            lLastDriverType = (lDriverVersion >> 24) & 0x000000FF;
+            asDriverType[hand] = (short)lLastDriverType;    /* Drv type */
+            lLastDriverVersion = lDriverVersion & 0x00FFFFFF;
+            alDriverVersion[hand] = lLastDriverVersion;     /* Actual version */
+            lDriverRev = ((lDriverVersion>>16) & 0x00FF);    /* use hi word */
+        }
+        else
+        {
+            U14Close1401(hand);    /* If there is a problem we should close */
+            return (short)lDriverVersion;      /* and return the error code */
+        }
+    
+        if (lDriverRev < MINDRIVERMAJREV)       /* late enough version?     */
+        {
+            U14Close1401(hand);    /* If there is a problem we should close */
+            return U14ERR_DRIVTOOOLD;           /* too old                  */
+        }
+    
+        asLastRetCode[hand] = U14ERR_NOERROR; /* Initialise this 1401s info */
+        abGrabbed[hand] = FALSE;          /* we are not in single step mode */
+        U14SetTimeout(hand, 3000);      /* set 3 seconds as default timeout */
+
+        switch (lRetVal)
+        {
+        case DRIVRET_STD:  asType1401[hand] = U14TYPE1401; break;      /* Some we do by hand */
+        case DRIVRET_U1401:asType1401[hand] = U14TYPEU1401; break;
+        case DRIVRET_PLUS: asType1401[hand] = U14TYPEPLUS; break;
+        default:  // For the power upwards, we can calculate the codes
+                if ((lRetVal >= DRIVRET_POWER) && (lRetVal <= DRIVRET_MAX))
+                    asType1401[hand] = (short)(lRetVal - (DRIVRET_POWER - U14TYPEPOWER));
+                else
+                    asType1401[hand] = U14TYPEUNKNOWN;
+                break;
+            }
+        U14KillIO1401(hand);                     /* resets the 1401 buffers */
+
+        if (asType1401[hand] != U14TYPEUNKNOWN)   /* If all seems OK so far */
+        {
+            sErr = U14CheckErr(hand);        /* we can check 1401 comms now */
+            if (sErr != 0)                       /* If this failed to go OK */
+                U14Reset1401(hand); /* Reset the 1401 to try to sort it out */
+        }
+
+        sErr = U14StateOf1401(hand);/* Get the state of the 1401 for return */
+        if (sErr == U14ERR_NOERROR)
+            sErr = hand;                 /* return the handle if no problem */
+        else
+            U14Close1401(hand);    /* If there is a problem we should close */
+    }
+
+    return sErr;
+}
+
+
+/****************************************************************************
+** U14Close1401
+** Closes the 1401 so someone else can use it.
+****************************************************************************/
+U14API(short) U14Close1401(short hand)
+{
+    int j;
+    int iAreaMask = 0;                          // Mask for active areas
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)                 // Check open and in use
+        return sErr;
+
+    for (j = 0; j<MAX_TRANSAREAS; ++j)
+    {
+        TGET_TX_BLOCK gtb;
+        int iReturn = U14GetTransfer(hand, &gtb);   // get area information
+        if (iReturn == U14ERR_NOERROR)          // ignore if any problem
+            if (gtb.used)
+                iAreaMask |= (1 << j);          // set a bit for each used area
+    }
+
+    if (iAreaMask)                              // if any areas are in use
+    {
+        U14Reset1401(hand);                     // in case an active transfer running
+        for (j = 0; j < MAX_TRANSAREAS; ++j)    // Locate locked areas
+            if (iAreaMask & (1 << j))           // And kill off any transfers
+                U14UnSetTransfer(hand, (WORD)j);
+    }
+
+#ifdef _IS_WINDOWS_
+    if (aXferEvent[hand])                       // if this 1401 has an open event handle
+    {
+        CloseHandle(aXferEvent[hand]);          // close down the handle
+        aXferEvent[hand] = NULL;                // and mark it as gone
+    }
+
+    if (CloseHandle(aHand1401[hand]))
+#endif
+#ifdef LINUX
+    if (close(aHand1401[hand]) == 0)            // make sure that close works
+#endif
+    {
+        aHand1401[hand] = INVALID_HANDLE_VALUE;
+        asType1401[hand] = U14TYPEUNKNOWN;
+        return U14ERR_NOERROR;
+    }
+    else
+        return U14ERR_BADHAND;     /* BUGBUG GetLastError() ? */
+}
+
+/**************************************************************************
+**
+** Look for open 1401s and attempt to close them down. 32-bit windows only.
+**************************************************************************/
+U14API(void) U14CloseAll(void)
+{
+    int i;
+    for (i = 0; i < MAX1401; i++)       // Tidy up and make safe
+        if (aHand1401[i] != INVALID_HANDLE_VALUE)
+            U14Close1401((short)i);     // Last ditch close 1401
+}
+
+/****************************************************************************
+** U14Reset1401
+** Resets the 1401
+****************************************************************************/
+U14API(short) U14Reset1401(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    return U14Control1401(hand, U14_RESET1401, &csBlock);
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_Reset1401(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14ForceReset
+**    Sets the 1401 full reset flag, so that next call to Reset1401 will
+**     always cause a genuine reset.
+*****************************************************************************/
+U14API(short) U14ForceReset(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    return U14Control1401(hand, U14_FULLRESET, &csBlock);
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_FullReset(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14KillIO1401
+**    Removes any pending IO from the buffers.
+*****************************************************************************/
+U14API(short) U14KillIO1401(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    return U14Control1401(hand, U14_KILLIO1401, &csBlock);
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_KillIO1401(aHand1401[hand]) : sErr;
+#endif
+}
+
+
+/****************************************************************************
+** U14SendString
+** Send characters to the 1401
+*****************************************************************************/
+U14API(short) U14SendString(short hand, const char* pString)
+{
+    int nChars;                     // length we are sending
+    long lTimeOutTicks;             // when to time out
+    BOOL bSpaceToSend;              // space to send yet
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+
+    nChars = (int)strlen(pString);  // get string length we want to send
+    if (nChars > MAXSTRLEN)
+        return U14ERR_STRLEN;       // String too long
+
+#ifdef _IS_WINDOWS_
+    // To get here we must wait for the buffer to have some space
+    lTimeOutTicks = U14WhenToTimeOut(hand);
+    do
+    {
+        bSpaceToSend = (BOOL)((long)U14OutBufSpace(hand) >= nChars);
+    }
+    while (!bSpaceToSend && !U14PassedTime(lTimeOutTicks));
+
+    if (!bSpaceToSend)             /* Last-ditch attempt to avoid timeout */
+    {           /* This can happen with anti-virus or network activity! */
+        int i;
+        for (i = 0; (i < 4) && (!bSpaceToSend); ++i)
+        {
+            Sleep(25);       /* Give other threads a chance for a while */
+            bSpaceToSend = (BOOL)((long)U14OutBufSpace(hand) >= nChars);
+        }
+    }
+
+    if (asLastRetCode[hand] == U14ERR_NOERROR)      /* no errors? */
+    {
+        if (bSpaceToSend)
+        {
+            PARAMBLK    rData;
+            DWORD       dwBytes;
+            char        tstr[MAXSTRLEN+5];          /* Buffer for chars */
+
+            if ((hand < 0) || (hand >= MAX1401))
+                sErr = U14ERR_BADHAND;
+            else
+            {
+                strcpy(tstr, pString);              /* Into local buf */
+#ifndef _WIN64
+                if (!USE_NT_DIOC(hand))             /* Using WIN 95 driver access? */
+                {
+                    int iOK = DeviceIoControl(aHand1401[hand], (DWORD)U14_SENDSTRING,
+                                    NULL, 0, tstr, nChars,
+                                    &dwBytes, NULL);
+                    if (iOK)
+                        sErr = (dwBytes >= (DWORD)nChars) ? U14ERR_NOERROR : U14ERR_DRIVCOMMS;
+                    else
+                        sErr = (short)GetLastError();
+                }
+                else
+#endif
+                {
+                    int iOK = DeviceIoControl(aHand1401[hand],(DWORD)U14_SENDSTRING,
+                                    tstr, nChars,
+                                    &rData,sizeof(PARAMBLK),&dwBytes,NULL);
+                    if (iOK && (dwBytes >= sizeof(PARAMBLK)))
+                        sErr = rData.sState;
+                    else
+                        sErr = U14ERR_DRIVCOMMS;
+                }
+
+                if (sErr != U14ERR_NOERROR) // If we have had a comms error
+                    U14ForceReset(hand);    //  make sure we get real reset
+            }
+
+            return sErr;
+
+        }
+        else
+        {
+            U14ForceReset(hand);                //  make sure we get real reset
+            return U14ERR_TIMEOUT;
+        }
+    }
+    else
+        return asLastRetCode[hand];
+#endif
+#ifdef LINUX
+    // Just try to send it and see what happens!
+    sErr = CED_SendString(aHand1401[hand], pString, nChars);
+    if (sErr != U14ERR_NOOUT)       // if any result except "no room in output"...
+    {
+        if (sErr != U14ERR_NOERROR) // if a problem...
+             U14ForceReset(hand);   // ...make sure we get real reset next time
+        return sErr;                // ... we are done as nothing we can do
+    }
+
+    // To get here we must wait for the buffer to have some space
+    lTimeOutTicks = U14WhenToTimeOut(hand);
+    do
+    {
+        bSpaceToSend = (BOOL)((long)U14OutBufSpace(hand) >= nChars);
+        if (!bSpaceToSend)
+            sched_yield();          // let others have fun while we wait
+    }
+    while (!bSpaceToSend && !U14PassedTime(lTimeOutTicks));
+
+    if (asLastRetCode[hand] == U14ERR_NOERROR)                /* no errors? */
+    {
+        if (bSpaceToSend)
+        {
+            sErr = CED_SendString(aHand1401[hand], pString, nChars);
+            if (sErr != U14ERR_NOERROR) // If we have had a comms error
+                U14ForceReset(hand);    //  make sure we get real reset
+            return sErr;
+        }
+        else
+        {
+            U14ForceReset(hand);                //  make sure we get real reset
+            return U14ERR_TIMEOUT;
+        }
+    }
+    else
+        return asLastRetCode[hand];
+#endif
+}
+
+/****************************************************************************
+** U14SendChar
+** Send character to the 1401
+*****************************************************************************/
+U14API(short) U14SendChar(short hand, char cChar)
+{
+#ifdef _IS_WINDOWS_
+    char sz[2]=" ";                         // convert to a string and send
+    sz[0] = cChar;
+    sz[1] = 0;
+    return(U14SendString(hand, sz));        // String routines are better
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_SendChar(aHand1401[hand], cChar) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14GetString
+** Get a string from the 1401. Returns a null terminated string.
+** The string is all the characters up to the next CR in the buffer
+** or the end of the buffer if that comes first. This only returns text
+** if there is a CR in the buffer. The terminating CR character is removed.
+** wMaxLen  Is the size of the buffer and must be at least 2 or an error.
+** Returns  U14ERR_NOERR if OK with the result in the string or a negative
+**          error code. Any error from the device causes us to set up for
+**          a full reset.
+****************************************************************************/
+U14API(short) U14GetString(short hand, char* pBuffer, WORD wMaxLen)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)             // If an error...
+        return sErr;                        // ...bail out!
+
+#ifdef _IS_WINDOWS_
+    if (wMaxLen>1)                          // we need space for terminating 0
+    {
+        BOOL bLineToGet;                    // true when a line to get
+        long lTimeOutTicks = U14WhenToTimeOut(hand);
+        do
+            bLineToGet = (BOOL)(U14LineCount(hand) != 0);
+        while (!bLineToGet && !U14PassedTime(lTimeOutTicks));
+
+        if (!bLineToGet)             /* Last-ditch attempt to avoid timeout */
+        {           /* This can happen with anti-virus or network activity! */
+            int i;
+            for (i = 0; (i < 4) && (!bLineToGet); ++i)
+            {
+                Sleep(25);       /* Give other threads a chance for a while */
+                bLineToGet = (BOOL)(U14LineCount(hand) != 0);
+            }
+        }
+
+        if (bLineToGet)
+        {
+            if (asLastRetCode[hand] == U14ERR_NOERROR)     /* all ok so far */
+            {
+                DWORD       dwBytes = 0;
+                *((WORD *)pBuffer) = wMaxLen;       /* set up length */
+#ifndef _WIN64
+                if (!USE_NT_DIOC(hand))             /* Win 95 DIOC here ? */
+                {
+                    char tstr[MAXSTRLEN+5];         /* Buffer for Win95 chars */
+                    int iOK;
+
+                    if (wMaxLen > MAXSTRLEN)        /* Truncate length */
+                        wMaxLen = MAXSTRLEN;    
+
+                    *((WORD *)tstr) = wMaxLen;      /* set len */
+
+                    iOK = DeviceIoControl(aHand1401[hand],(DWORD)U14_GETSTRING,
+                                    NULL, 0, tstr, wMaxLen+sizeof(short),
+                                    &dwBytes, NULL);
+                    if (iOK)                        /* Device IO control OK ? */
+                    {
+                        if (dwBytes >= 0)           /* If driver OK */
+                        {
+                            strcpy(pBuffer, tstr);
+                            sErr = U14ERR_NOERROR;
+                        }
+                        else
+                            sErr = U14ERR_DRIVCOMMS;
+                    }
+                    else
+                    {
+                        sErr = (short)GetLastError();
+                        if (sErr > 0)               /* Errors are -ve */
+                            sErr = (short)-sErr;
+                    }
+                }
+                else
+#endif
+                {       /* Here for NT, the DLL must own the buffer */
+                    HANDLE hMem = GlobalAlloc(GMEM_MOVEABLE,wMaxLen+sizeof(short));
+                    if (hMem)
+                    {
+                        char* pMem = (char*)GlobalLock(hMem);
+                        if (pMem)
+                        {
+                            int iOK = DeviceIoControl(aHand1401[hand],(DWORD)U14_GETSTRING,
+                                            NULL, 0, pMem, wMaxLen+sizeof(short),
+                                            &dwBytes, NULL);
+                            if (iOK)                /* Device IO control OK ? */
+                            {
+                                if (dwBytes >= wMaxLen)
+                                {
+                                    strcpy(pBuffer, pMem+sizeof(short));
+                                    sErr = *((SHORT*)pMem);
+                                }
+                                else
+                                    sErr = U14ERR_DRIVCOMMS;
+                            }
+                            else
+                                sErr = U14ERR_DRIVCOMMS;
+
+                            GlobalUnlock(hMem);
+                        }
+                        else
+                            sErr = U14ERR_OUTOFMEMORY;
+
+                        GlobalFree(hMem);
+                    }
+                    else
+                        sErr = U14ERR_OUTOFMEMORY;
+                }
+
+                if (sErr == U14ERR_NOERROR)     // If all OK...
+                    TranslateString(pBuffer);   // ...convert any commas to spaces
+                else                            // If we have had a comms error...
+                    U14ForceReset(hand);        // ...make sure we get real reset
+
+            }
+            else
+                sErr = asLastRetCode[hand];
+        }
+        else
+        {
+            sErr = U14ERR_TIMEOUT;
+            U14ForceReset(hand);            //  make sure we get real reset
+        }
+    }
+    else
+        sErr = U14ERR_BUFF_SMALL;
+    return sErr;
+#endif
+#ifdef LINUX
+    if (wMaxLen>1)                          // we need space for terminating 0
+    {
+        BOOL bLineToGet;                    // true when a line to get
+        long lTimeOutTicks = U14WhenToTimeOut(hand);
+        do
+        {
+            bLineToGet = (BOOL)(U14LineCount(hand) != 0);
+            if (!bLineToGet)
+                sched_yield();
+
+        }
+        while (!bLineToGet && !U14PassedTime(lTimeOutTicks));
+
+        if (bLineToGet)
+        {
+            sErr = CED_GetString(aHand1401[hand], pBuffer, wMaxLen-1);   // space for terminator
+            if (sErr >=0)                    // if we were OK...
+            {
+                if (sErr >= wMaxLen)         // this should NOT happen unless
+                    sErr = U14ERR_DRIVCOMMS; // ...driver Comms are very bad
+                else
+                {
+                    pBuffer[sErr] = 0;      // OK, so terminate the string...
+                    TranslateString(pBuffer);  // ...and convert commas to spaces.
+                }
+            }
+
+            if (sErr < U14ERR_NOERROR)       // If we have had a comms error
+                U14ForceReset(hand);            //  make sure we get real reset
+        }
+        else
+        {
+            sErr = U14ERR_TIMEOUT;
+            U14ForceReset(hand);            //  make sure we get real reset
+        }
+    }
+    else
+        sErr = U14ERR_BUFF_SMALL;
+
+    return sErr >= U14ERR_NOERROR ? U14ERR_NOERROR : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14GetChar
+** Get a character from the 1401. CR returned as CR.
+*****************************************************************************/
+U14API(short) U14GetChar(short hand, char* pcChar)
+{
+#ifdef _IS_WINDOWS_
+    char sz[2];                             // read a very short string
+    short sErr = U14GetString(hand, sz, 2); // read one char and nul terminate it
+    *pcChar = sz[0];    // copy to result, NB char translate done by GetString
+    if (sErr == U14ERR_NOERROR)
+    {                                       // undo translate of CR to zero
+        if (*pcChar == '\0')                // by converting back
+            *pcChar = '\n';                 // What a nasty thing to have to do
+    }
+    return sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)             // Check parameters
+        return sErr;
+    sErr = CED_GetChar(aHand1401[hand]);    // get one char, if available
+    if (sErr >= 0)
+    {
+        *pcChar = (char)sErr;              // return if it we have one
+        return U14ERR_NOERROR;              // say all OK
+    }
+    else
+        return sErr;
+#endif
+}
+
+/****************************************************************************
+** U14Stat1401
+** Returns 0 for no lines or error or non zero for something waiting
+****************************************************************************/
+U14API(short) U14Stat1401(short hand)
+{
+    return ((short)(U14LineCount(hand) > 0));
+}
+
+/****************************************************************************
+** U14CharCount
+** Returns the number of characters in the input buffer
+*****************************************************************************/
+U14API(short) U14CharCount(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_STAT1401, &csBlock);
+    if (sErr == U14ERR_NOERROR)
+        sErr = csBlock.ints[0];
+    return sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_Stat1401(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14LineCount
+** Returns the number of CR characters in the input buffer
+*****************************************************************************/
+U14API(short) U14LineCount(short hand)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14Status1401(hand, U14_LINECOUNT, &csBlock);
+    if (sErr == U14ERR_NOERROR)
+        sErr = csBlock.ints[0];
+    return sErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_LineCount(aHand1401[hand]) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14GetErrorString
+** Converts error code supplied to a decent descriptive string.
+** NOTE: This function may use some extra information stored
+**       internally in the DLL. This information is stored on a
+**       per-process basis, but it might be altered if you call
+**       other functions after getting an error and before using
+**       this function.
+****************************************************************************/
+U14API(void)  U14GetErrorString(short nErr, char* pStr, WORD wMax)
+{
+    char    wstr[150];
+
+    switch (nErr)              /* Basically, we do this with a switch block */
+    {
+    case U14ERR_OFF:
+        sprintf(wstr, "The 1401 is apparently switched off (code %d)", nErr);
+        break;
+
+    case U14ERR_NC:
+        sprintf(wstr, "The 1401 is not connected to the interface card (code %d)", nErr);
+        break;
+
+    case U14ERR_ILL:
+        sprintf(wstr, "The 1401 is not working correctly (code %d)", nErr);
+        break;
+
+    case U14ERR_NOIF:
+        sprintf(wstr, "The 1401 interface card was not detected (code %d)", nErr);
+        break;
+
+    case U14ERR_TIME:
+        sprintf(wstr, "The 1401 fails to become ready for use (code %d)", nErr);
+        break;
+
+    case U14ERR_BADSW:
+        sprintf(wstr, "The 1401 interface card jumpers are incorrect (code %d)", nErr);
+        break;
+
+    case U14ERR_NOINT:
+        sprintf(wstr, "The 1401 interrupt is not available for use (code %d)", nErr);
+        break;
+
+    case U14ERR_INUSE:
+        sprintf(wstr, "The 1401 is already in use by another program (code %d)", nErr);
+        break;
+
+    case U14ERR_NODMA:
+        sprintf(wstr, "The 1401 DMA channel is not available for use (code %d)", nErr);
+        break;
+
+    case U14ERR_BADHAND:
+        sprintf(wstr, "The application supplied an incorrect 1401 handle (code %d)", nErr);
+        break;
+
+    case U14ERR_BAD1401NUM:
+        sprintf(wstr, "The application used an incorrect 1401 number (code %d)", nErr);
+        break;
+
+    case U14ERR_NO_SUCH_FN:
+        sprintf(wstr, "The code passed to the 1401 driver is invalid (code %d)", nErr);
+        break;
+
+    case U14ERR_NO_SUCH_SUBFN:
+        sprintf(wstr, "The sub-code passed to the 1401 driver is invalid (code %d)", nErr);
+        break;
+
+    case U14ERR_NOOUT:
+        sprintf(wstr, "No room in buffer for characters for the 1401 (code %d)", nErr);
+        break;
+
+    case U14ERR_NOIN:
+        sprintf(wstr, "No characters from the 1401 are available (code %d)", nErr);
+        break;
+
+    case U14ERR_STRLEN:
+        sprintf(wstr, "A string sent to or read from the 1401 was too long (code %d)", nErr);
+        break;
+
+    case U14ERR_LOCKFAIL:
+        sprintf(wstr, "Failed to lock host memory for data transfer (code %d)", nErr);
+        break;
+
+    case U14ERR_UNLOCKFAIL:
+        sprintf(wstr, "Failed to unlock host memory after data transfer (code %d)", nErr);
+        break;
+
+    case U14ERR_ALREADYSET:
+        sprintf(wstr, "The transfer area used is already set up (code %d)", nErr);
+        break;
+
+    case U14ERR_NOTSET:
+        sprintf(wstr, "The transfer area used has not been set up (code %d)", nErr);
+        break;
+
+    case U14ERR_BADAREA:
+        sprintf(wstr, "The transfer area number is incorrect (code %d)", nErr);
+        break;
+
+    case U14ERR_NOFILE:
+        sprintf(wstr, "The command file %s could not be opened (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_READERR:
+        sprintf(wstr, "The command file %s could not be read (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_UNKNOWN:
+        sprintf(wstr, "The %s command resource could not be found (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_HOSTSPACE:
+        sprintf(wstr, "Unable to allocate memory for loading command %s (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_LOCKERR:
+        sprintf(wstr, "Unable to lock memory for loading command %s (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_CLOADERR:
+        sprintf(wstr, "Error in loading command %s, bad command format (code %d)", szLastName, nErr);
+        break;
+
+    case U14ERR_TOXXXERR:
+        sprintf(wstr, "Error detected after data transfer to or from the 1401 (code %d)", nErr);
+        break;
+
+    case U14ERR_NO386ENH:
+        sprintf(wstr, "Windows 3.1 is not running in 386 enhanced mode (code %d)", nErr);
+        break;
+
+    case U14ERR_NO1401DRIV:
+        sprintf(wstr, "The 1401 device driver cannot be found (code %d)\nUSB:   check plugged in and powered\nOther: not installed?", nErr);
+        break;
+
+    case U14ERR_DRIVTOOOLD:
+        sprintf(wstr, "The 1401 device driver is too old for use (code %d)", nErr);
+        break;
+
+    case U14ERR_TIMEOUT:
+        sprintf(wstr, "Character transmissions to the 1401 timed-out (code %d)", nErr);
+        break;
+
+    case U14ERR_BUFF_SMALL:
+        sprintf(wstr, "Buffer for text from the 1401 was too small (code %d)", nErr);
+        break;
+
+    case U14ERR_CBALREADY:
+        sprintf(wstr, "1401 monitor callback already set up (code %d)", nErr);
+        break;
+
+    case U14ERR_BADDEREG:
+        sprintf(wstr, "1401 monitor callback deregister invalid (code %d)", nErr);
+        break;
+
+    case U14ERR_DRIVCOMMS:
+        sprintf(wstr, "1401 device driver communications failed (code %d)", nErr);
+        break;
+
+    case U14ERR_OUTOFMEMORY:
+        sprintf(wstr, "Failed to allocate or lock memory for text from the 1401 (code %d)", nErr);
+        break;
+
+    default:
+        sprintf(wstr, "1401 error code %d returned; this code is unknown", nErr);
+        break;
+
+    }
+    if ((WORD)strlen(wstr) >= wMax-1)  /* Check for string being too long */
+        wstr[wMax-1] = 0;                          /* and truncate it if so */
+    strcpy(pStr, wstr);                       /* Return the error string */
+}
+
+/***************************************************************************
+** U14GetTransfer
+** Get a TGET_TX_BLOCK describing a transfer area (held in the block)
+***************************************************************************/
+U14API(short) U14GetTransfer(short hand, TGET_TX_BLOCK *pTransBlock)
+{
+    short sErr = CheckHandle(hand);
+#ifdef _IS_WINDOWS_
+    if (sErr == U14ERR_NOERROR)
+    { 
+        DWORD dwBytes = 0;
+        BOOL bOK = DeviceIoControl(aHand1401[hand], (DWORD)U14_GETTRANSFER, NULL, 0, pTransBlock,
+                              sizeof(TGET_TX_BLOCK), &dwBytes, NULL);
+    
+        if (bOK && (dwBytes >= sizeof(TGET_TX_BLOCK)))
+            sErr = U14ERR_NOERROR;
+        else
+            sErr = U14ERR_DRIVCOMMS;
+    }
+    return sErr;
+#endif
+#ifdef LINUX
+    return (sErr == U14ERR_NOERROR) ? CED_GetTransfer(aHand1401[hand], pTransBlock) : sErr;
+#endif
+}
+/////////////////////////////////////////////////////////////////////////////
+// U14WorkingSet
+// For Win32 only, adjusts process working set so that minimum is at least
+//  dwMinKb and maximum is at least dwMaxKb.
+// Return value is zero if all went OK, or a code from 1 to 3 indicating the
+//  cause of the failure:
+//
+//     1 unable to access process (insufficient rights?)
+//     2 unable to read process working set
+//     3 unable to set process working set - bad parameters?
+U14API(short) U14WorkingSet(DWORD dwMinKb, DWORD dwMaxKb)
+{
+#ifdef _IS_WINDOWS_
+    short sRetVal = 0;                      // 0 means all is OK
+    HANDLE hProcess;
+    DWORD dwVer = GetVersion();
+	if (dwVer & 0x80000000)                 // is this not NT?
+        return 0;                           // then give up right now
+
+    // Now attempt to get information on working set size
+    hProcess = OpenProcess(STANDARD_RIGHTS_REQUIRED |
+                                  PROCESS_QUERY_INFORMATION |
+                                  PROCESS_SET_QUOTA,
+                                  FALSE, _getpid());
+    if (hProcess)
+    {
+        SIZE_T dwMinSize,dwMaxSize;
+        if (GetProcessWorkingSetSize(hProcess, &dwMinSize, &dwMaxSize))
+        {
+            DWORD dwMin = dwMinKb << 10;    // convert from kb to bytes
+            DWORD dwMax = dwMaxKb << 10;
+
+            // if we get here, we have managed to read the current size
+            if (dwMin > dwMinSize)          // need to change sizes?
+                dwMinSize = dwMin;
+
+            if (dwMax > dwMaxSize)
+                dwMaxSize = dwMax;
+
+            if (!SetProcessWorkingSetSize(hProcess, dwMinSize, dwMaxSize))
+                sRetVal = 3;                // failed to change size
+        }
+        else
+            sRetVal = 2;                    // failed to read original size
+
+        CloseHandle(hProcess);
+    }
+    else
+        sRetVal = 1;            // failed to get handle
+
+    return sRetVal;
+#endif
+#ifdef LINUX
+    if (dwMinKb | dwMaxKb)
+    {
+        // to stop compiler moaning
+    }
+    return U14ERR_NOERROR;
+#endif
+}
+
+/****************************************************************************
+** U14UnSetTransfer  Cancels a transfer area
+** wArea    The index of a block previously used in by SetTransfer
+*****************************************************************************/
+U14API(short) U14UnSetTransfer(short hand, WORD wArea)
+{
+    short sErr = CheckHandle(hand);
+#ifdef _IS_WINDOWS_
+    if (sErr == U14ERR_NOERROR)
+    {
+       TCSBLOCK csBlock;
+       csBlock.ints[0] = (short)wArea;       /* Area number into control block */
+       sErr = U14Control1401(hand, U14_UNSETTRANSFER, &csBlock);  /* Free area */
+   
+       VirtualUnlock(apAreas[hand][wArea], auAreas[hand][wArea]);/* Unlock */
+       apAreas[hand][wArea] = NULL;                         /* Clear locations */
+       auAreas[hand][wArea] = 0;
+    }
+    return sErr;
+#endif
+#ifdef LINUX
+    return (sErr == U14ERR_NOERROR) ? CED_UnsetTransfer(aHand1401[hand], wArea) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14SetTransArea      Sets an area up to be used for transfers
+** WORD  wArea     The area number to set up
+** void *pvBuff    The address of the buffer for the data.
+** DWORD dwLength  The length of the buffer for the data
+** short eSz       The element size (used for byte swapping on the Mac)
+****************************************************************************/
+U14API(short) U14SetTransArea(short hand, WORD wArea, void *pvBuff,
+                                          DWORD dwLength, short eSz)
+{
+    TRANSFERDESC td;
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+    if (wArea >= MAX_TRANSAREAS)                    // Is this a valid area number
+        return U14ERR_BADAREA;
+
+#ifdef _IS_WINDOWS_
+    assert(apAreas[hand][wArea] == NULL);
+    assert(auAreas[hand][wArea] == 0);
+
+    apAreas[hand][wArea] = pvBuff;                  /* Save data for later */
+    auAreas[hand][wArea] = dwLength;
+
+    if (!VirtualLock(pvBuff, dwLength))             /* Lock using WIN32 calls */
+    {
+        apAreas[hand][wArea] = NULL;                /* Clear locations */
+        auAreas[hand][wArea] = 0;
+        return U14ERR_LOCKERR;                      /* VirtualLock failed */
+    }
+#ifndef _WIN64
+    if (!USE_NT_DIOC(hand))                         /* Use Win 9x DIOC? */
+    {
+        DWORD dwBytes;
+        VXTRANSFERDESC vxDesc;                      /* Structure to pass to VXD */
+        vxDesc.wArea = wArea;                       /* Copy across simple params */
+        vxDesc.dwLength = dwLength;
+
+        // Check we are not asking an old driver for more than area 0
+        if ((wArea != 0) && (U14DriverVersion(hand) < 0x00010002L))
+            sErr = U14ERR_DRIVTOOOLD;
+        else
+        {
+            vxDesc.dwAddrOfs = (DWORD)pvBuff;       /* 32 bit offset */
+            vxDesc.wAddrSel  = 0;
+
+            if (DeviceIoControl(aHand1401[hand], (DWORD)U14_SETTRANSFER,
+                                pvBuff,dwLength,    /* Will translate pointer */
+                                &vxDesc,sizeof(VXTRANSFERDESC),
+                                &dwBytes,NULL))
+            {
+                if (dwBytes >= sizeof(VXTRANSFERDESC)) /* Driver OK ? */
+                    sErr = U14ERR_NOERROR;
+                else
+                    sErr = U14ERR_DRIVCOMMS;        /* Else never got there */
+            }
+            else
+                sErr = (short)GetLastError();
+        }
+    }
+    else
+#endif
+    {
+        PARAMBLK rWork;
+        DWORD dwBytes;
+        td.wArea = wArea;     /* Pure NT - put data into struct */
+        td.lpvBuff = pvBuff;
+        td.dwLength = dwLength;
+        td.eSize = 0;                // Dummy element size
+
+        if (DeviceIoControl(aHand1401[hand],(DWORD)U14_SETTRANSFER,
+                            &td,sizeof(TRANSFERDESC),
+                            &rWork,sizeof(PARAMBLK),&dwBytes,NULL))
+        {
+            if (dwBytes >= sizeof(PARAMBLK))    // maybe error from driver?
+                sErr = rWork.sState;            // will report any error
+            else
+                sErr = U14ERR_DRIVCOMMS;        // Else never got there
+        }
+        else
+            sErr = U14ERR_DRIVCOMMS;
+    }
+
+    if (sErr != U14ERR_NOERROR)
+    {
+        if (sErr != U14ERR_LOCKERR)             // unless lock failed...
+            VirtualUnlock(pvBuff, dwLength);    // ...release the lock
+        apAreas[hand][wArea] = NULL;            // Clear locations
+        auAreas[hand][wArea] = 0;
+    }
+
+    return sErr;
+#endif
+#ifdef LINUX
+    // The strange cast is so that it works in 64 and 32-bit linux as long is 64-bits
+    // in the 64 bit version.
+    td.lpvBuff = (long long)((unsigned long)pvBuff);
+    td.wAreaNum = wArea;
+    td.dwLength = dwLength;
+    td.eSize = eSz;                // Dummy element size
+    return CED_SetTransfer(aHand1401[hand], &td);
+#endif
+}
+
+/****************************************************************************
+** U14SetTransferEvent  Sets an event for notification of application
+** wArea       The tranfer area index, from 0 to MAXAREAS-1
+**    bEvent      True to create an event, false to remove it
+**    bToHost     Set 0 for notification on to1401 tranfers, 1 for
+**                notification of transfers to the host PC
+**    dwStart     The offset of the sub-area of interest
+**    dwLength    The size of the sub-area of interest
+**
+** The device driver will set the event supplied to the signalled state
+** whenever a DMA transfer to/from the specified area is completed. The
+** transfer has to be in the direction specified by bToHost, and overlap
+** that part of the whole transfer area specified by dwStart and dwLength.
+** It is important that this function is called with bEvent false to release
+** the event once 1401 activity is finished.
+**
+** Returns 1 if an event handle exists, 0 if all OK and no event handle or
+** a negative code for an error.
+****************************************************************************/
+U14API(short) U14SetTransferEvent(short hand, WORD wArea, BOOL bEvent,
+                                  BOOL bToHost, DWORD dwStart, DWORD dwLength)
+{
+#ifdef _IS_WINDOWS_
+    TCSBLOCK csBlock;
+    short sErr = U14TransferFlags(hand);        // see if we can handle events
+    if (sErr >= U14ERR_NOERROR)                 // check handle is OK
+    {
+        bEvent = bEvent && ((sErr & U14TF_NOTIFY) != 0); // remove request if we cannot do events
+        if (wArea >= MAX_TRANSAREAS)            // Check a valid area...
+            return U14ERR_BADAREA;              // ...and bail of not
+
+        // We can hold an event for each area, so see if we need to change the
+        // state of the event.
+        if ((bEvent != 0) != (aXferEvent[hand] != 0))    // change of event state?
+        {
+            if (bEvent)                         // want one and none present
+                aXferEvent[hand] = CreateEvent(NULL, FALSE, FALSE, NULL);
+            else
+            {
+                CloseHandle(aXferEvent[hand]);  // clear the existing event
+                aXferEvent[hand] = NULL;        // and clear handle
+            }
+        }
+
+        // We have to store the parameters differently for 64-bit operations
+        //  because a handle is 64 bits long. The drivers know of this and
+        //  handle the information appropriately.
+#ifdef _WIN64
+        csBlock.longs[0] = wArea;               // Pass paramaters into the driver...
+        if (bToHost != 0)                       // The direction flag is held in the
+            csBlock.longs[0] |= 0x10000;        //  upper word of the transfer area value
+        *((HANDLE*)&csBlock.longs[1]) = aXferEvent[hand];  // The event handle is 64-bits
+        csBlock.longs[3] = dwStart;             // Thankfully these two remain
+        csBlock.longs[4] = dwLength;            //  as unsigned 32-bit values
+#else
+        csBlock.longs[0] = wArea;               // pass paramaters into the driver...
+        csBlock.longs[1] = (long)aXferEvent[hand];    // ...especially the event handle
+        csBlock.longs[2] = bToHost;
+        csBlock.longs[3] = dwStart;
+        csBlock.longs[4] = dwLength;
+#endif
+        sErr = U14Control1401(hand, U14_SETTRANSEVENT, &csBlock);
+        if (sErr == U14ERR_NOERROR)
+            sErr = (short)(aXferEvent[hand] != NULL);    // report if we have a flag
+    }
+
+    return sErr;
+#endif
+#ifdef LINUX
+    TRANSFEREVENT te;
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+
+    if (wArea >= MAX_TRANSAREAS)            // Is this a valid area number
+        return U14ERR_BADAREA;
+
+    te.wAreaNum = wArea;                    // copy parameters to the control block
+    te.wFlags = bToHost ? 1 : 0;            // bit 0 sets the direction
+    te.dwStart = dwStart;                   // start offset of the event area
+    te.dwLength = dwLength;                 // size of the event area
+    te.iSetEvent = bEvent;                  // in Windows, this creates/destroys the event
+    return CED_SetEvent(aHand1401[hand], &te);
+#endif
+}
+
+/****************************************************************************
+** U14TestTransferEvent
+** Would a U14WaitTransferEvent() call return immediately? return 1 if so,
+** 0 if not or a negative code if a problem.
+****************************************************************************/
+U14API(int) U14TestTransferEvent(short hand, WORD wArea)
+{
+#ifdef _IS_WINDOWS_
+    int iErr = CheckHandle(hand);
+    if (iErr == U14ERR_NOERROR)
+    {
+        if (aXferEvent[hand])           // if a handle is set...
+            iErr = WaitForSingleObject(aXferEvent[hand], 0) == WAIT_OBJECT_0;
+    }
+    return iErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_TestEvent(aHand1401[hand], wArea) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14WaitTransferEvent
+** Wait for a transfer event with a timeout.
+** msTimeOut is 0 for an infinite wait, else it is the maximum time to wait
+**           in milliseconds in range 0-0x00ffffff.
+** Returns   If no event handle then return immediately. Else return 1 if
+**           timed out or 0=event, and a negative code if a problem.
+****************************************************************************/
+U14API(int) U14WaitTransferEvent(short hand, WORD wArea, int msTimeOut)
+{
+#ifdef _IS_WINDOWS_
+    int iErr = CheckHandle(hand);
+    if (iErr == U14ERR_NOERROR)
+    {
+        if (aXferEvent[hand])
+        {
+            if (msTimeOut == 0)
+                msTimeOut = INFINITE;
+            iErr = WaitForSingleObject(aXferEvent[hand], msTimeOut) != WAIT_OBJECT_0;
+        }
+        else
+            iErr = TRUE;                // say we timed out if no event
+    }
+    return iErr;
+#endif
+#ifdef LINUX
+    short sErr = CheckHandle(hand);
+    return (sErr == U14ERR_NOERROR) ? CED_WaitEvent(aHand1401[hand], wArea, msTimeOut) : sErr;
+#endif
+}
+
+/****************************************************************************
+** U14SetCircular    Sets an area up for circular DMA transfers
+** WORD  wArea          The area number to set up
+** BOOL  bToHost        Sets the direction of data transfer
+** void *pvBuff        The address of the buffer for the data
+** DWORD dwLength       The length of the buffer for the data
+****************************************************************************/
+U14API(short) U14SetCircular(short hand, WORD wArea, BOOL bToHost,
+									void *pvBuff, DWORD dwLength)
+{
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+
+    if (wArea >= MAX_TRANSAREAS)         /* Is this a valid area number */
+        return U14ERR_BADAREA;
+
+	if (!bToHost)             /* For now, support tohost transfers only */
+        return U14ERR_BADAREA;            /* best error code I can find */
+#ifdef _IS_WINDOWS_
+    assert(apAreas[hand][wArea] == NULL);
+    assert(auAreas[hand][wArea] == 0);
+
+    apAreas[hand][wArea] = pvBuff;              /* Save data for later */
+    auAreas[hand][wArea] = dwLength;
+
+    if (!VirtualLock(pvBuff, dwLength))      /* Lock using WIN32 calls */
+        sErr = U14ERR_LOCKERR;                    /* VirtualLock failed */
+    else
+    {
+        PARAMBLK rWork;
+        DWORD dwBytes;
+        TRANSFERDESC txDesc;
+        txDesc.wArea = wArea;             /* Pure NT - put data into struct */
+        txDesc.lpvBuff = pvBuff;
+        txDesc.dwLength = dwLength;
+        txDesc.eSize = (short)bToHost;       /* Use this for direction flag */
+   
+        if (DeviceIoControl(aHand1401[hand],(DWORD)U14_SETCIRCULAR,
+                           &txDesc, sizeof(TRANSFERDESC),
+                           &rWork, sizeof(PARAMBLK),&dwBytes,NULL))
+        {
+           if (dwBytes >= sizeof(PARAMBLK))          /* error from driver? */
+               sErr = rWork.sState;         /* No, just return driver data */
+           else
+               sErr = U14ERR_DRIVCOMMS;            /* Else never got there */
+        }
+        else
+            sErr = U14ERR_DRIVCOMMS;
+    }
+
+    if (sErr != U14ERR_NOERROR)
+    {
+        if (sErr != U14ERR_LOCKERR)
+            VirtualUnlock(pvBuff, dwLength);         /* Release NT lock */
+        apAreas[hand][wArea] = NULL;                 /* Clear locations */
+        auAreas[hand][wArea] = 0;
+    }
+
+    return sErr;
+#endif
+#ifdef LINUX
+    else
+    {
+        TRANSFERDESC td;
+        td.lpvBuff = (long long)((unsigned long)pvBuff);
+        td.wAreaNum = wArea;
+        td.dwLength = dwLength;
+        td.eSize = (short)bToHost;       /* Use this for direction flag */
+        return CED_SetCircular(aHand1401[hand], &td);
+    }
+#endif
+}
+
+/****************************************************************************
+** Function  GetCircBlk returns the size (& start offset) of the next
+**           available block of circular data.
+****************************************************************************/
+U14API(int) U14GetCircBlk(short hand, WORD wArea, DWORD *pdwOffs)
+{
+    int lErr = CheckHandle(hand);
+    if (lErr != U14ERR_NOERROR)
+        return lErr;
+
+    if (wArea >= MAX_TRANSAREAS)            // Is this a valid area number?
+        return U14ERR_BADAREA;
+    else
+    {
+#ifdef _IS_WINDOWS_
+        PARAMBLK rWork;
+        TCSBLOCK csBlock;
+        DWORD dwBytes;
+        csBlock.longs[0] = wArea;               // Area number into control block
+        rWork.sState = U14ERR_DRIVCOMMS;
+        if (DeviceIoControl(aHand1401[hand], (DWORD)U14_GETCIRCBLK, &csBlock, sizeof(TCSBLOCK), &rWork, sizeof(PARAMBLK), &dwBytes, NULL) &&
+           (dwBytes >= sizeof(PARAMBLK)))
+            lErr = rWork.sState;
+        else
+            lErr = U14ERR_DRIVCOMMS;
+   
+        if (lErr == U14ERR_NOERROR)             // Did everything go OK?
+        {                                       // Yes, we can pass the results back
+            lErr = rWork.csBlock.longs[1];      // Return the block information
+            *pdwOffs = rWork.csBlock.longs[0];  // Offset is first in array
+        }
+#endif
+#ifdef LINUX
+        TCIRCBLOCK cb;
+        cb.nArea = wArea;                       // Area number into control block
+        cb.dwOffset = 0;
+        cb.dwSize = 0;
+        lErr = CED_GetCircBlock(aHand1401[hand], &cb);
+        if (lErr == U14ERR_NOERROR)             // Did everything go OK?
+        {                                       // Yes, we can pass the results back
+            lErr = cb.dwSize;                   // return the size
+            *pdwOffs = cb.dwOffset;             // and the offset
+        }
+#endif
+    }
+    return lErr;
+}
+
+/****************************************************************************
+** Function  FreeCircBlk marks the specified area of memory as free for
+**           resuse for circular transfers and returns the size (& start
+**           offset) of the next available block of circular data.
+****************************************************************************/
+U14API(int) U14FreeCircBlk(short hand, WORD wArea, DWORD dwOffs, DWORD dwSize,
+                                        DWORD *pdwOffs)
+{
+    int lErr = CheckHandle(hand);
+    if (lErr != U14ERR_NOERROR)
+        return lErr;
+
+    if (wArea < MAX_TRANSAREAS)                 // Is this a valid area number
+    {
+#ifdef _IS_WINDOWS_
+        PARAMBLK rWork;
+        TCSBLOCK csBlock;
+        DWORD dwBytes;
+        csBlock.longs[0] = wArea;               // Area number into control block
+        csBlock.longs[1] = dwOffs;
+        csBlock.longs[2] = dwSize;
+        rWork.sState = U14ERR_DRIVCOMMS;
+        if (DeviceIoControl(aHand1401[hand], (DWORD)U14_FREECIRCBLK, &csBlock, sizeof(TCSBLOCK),
+                           &rWork, sizeof(PARAMBLK), &dwBytes, NULL) &&
+           (dwBytes >= sizeof(PARAMBLK)))
+           lErr = rWork.sState;
+        else
+           lErr = U14ERR_DRIVCOMMS;
+       if (lErr == U14ERR_NOERROR)             // Did everything work OK?
+       {                                       // Yes, we can pass the results back
+           lErr = rWork.csBlock.longs[1];      // Return the block information
+           *pdwOffs = rWork.csBlock.longs[0];  // Offset is first in array
+       }
+#endif
+#ifdef LINUX
+        TCIRCBLOCK cb;
+        cb.nArea = wArea;                       // Area number into control block
+        cb.dwOffset = dwOffs;
+        cb.dwSize = dwSize;
+    
+        lErr = CED_FreeCircBlock(aHand1401[hand], &cb);
+        if (lErr == U14ERR_NOERROR)             // Did everything work OK?
+        {                                       // Yes, we can pass the results back
+            lErr = cb.dwSize;                   // Return the block information
+            *pdwOffs = cb.dwOffset;             // Offset is first in array
+        }
+#endif
+    }
+    else
+        lErr = U14ERR_BADAREA;
+
+    return lErr;
+}
+
+/****************************************************************************
+** Transfer
+** Transfer moves data to 1401 or to host
+** Assumes memory is allocated and locked,
+** which it should be to get a pointer
+*****************************************************************************/
+static short Transfer(short hand, BOOL bTo1401, char* pData,
+                       DWORD dwSize, DWORD dw1401, short eSz)
+{
+    char strcopy[MAXSTRLEN+1];          // to hold copy of work string
+    short sResult = U14SetTransArea(hand, 0, (void *)pData, dwSize, eSz);
+    if (sResult == U14ERR_NOERROR)      // no error
+    {
+        sprintf(strcopy,                // data offset is always 0
+                "TO%s,$%X,$%X,0;", bTo1401 ? "1401" : "HOST", dw1401, dwSize);
+
+        U14SendString(hand, strcopy);   // send transfer string
+
+        sResult = U14CheckErr(hand);    // Use ERR command to check for done
+        if (sResult > 0)
+            sResult = U14ERR_TOXXXERR;  // If a 1401 error, use this code
+
+        U14UnSetTransfer(hand, 0);
+    }
+    return sResult;
+}
+
+/****************************************************************************
+** Function  ToHost transfers data into the host from the 1401
+****************************************************************************/
+U14API(short) U14ToHost(short hand, char* pAddrHost, DWORD dwSize,
+                                            DWORD dw1401, short eSz)
+{
+    short sErr = CheckHandle(hand);
+    if ((sErr == U14ERR_NOERROR) && dwSize) // TOHOST is a constant
+        sErr = Transfer(hand, TOHOST, pAddrHost, dwSize, dw1401, eSz);
+    return sErr;
+}
+
+/****************************************************************************
+** Function  To1401 transfers data into the 1401 from the host
+****************************************************************************/
+U14API(short) U14To1401(short hand, const char* pAddrHost,DWORD dwSize,
+                                    DWORD dw1401, short eSz)
+{
+    short sErr = CheckHandle(hand);
+    if ((sErr == U14ERR_NOERROR) && dwSize) // TO1401 is a constant
+        sErr = Transfer(hand, TO1401, (char*)pAddrHost, dwSize, dw1401, eSz);
+    return sErr;
+}
+
+/****************************************************************************
+** Function  LdCmd    Loads a command from a full path or just a file
+*****************************************************************************/
+#ifdef _IS_WINDOWS_
+#define file_exist(name) (_access(name, 0) != -1)
+#define file_open(name) _lopen(name, OF_READ)
+#define file_close(h)   _lclose(h)
+#define file_seek(h, pos) _llseek(h, pos, FILE_BEGIN) 
+#define file_read(h, buffer, size) (_lread(h, buffer, size) == size)
+#endif
+#ifdef LINUX
+#define file_exist(name) (access(name, F_OK) != -1)
+#define file_open(name) open(name, O_RDONLY)
+#define file_close(h)   close(h)
+#define file_seek(h, pos) lseek(h, pos, SEEK_SET) 
+#define file_read(h, buffer, size) (read(h, buffer, size) == (ssize_t)size)
+static DWORD GetModuleFileName(void* dummy, char* buffer, int max)
+{
+    // The following works for Linux systems with a /proc file system.
+    char szProcPath[32];
+    sprintf(szProcPath, "/proc/%d/exe", getpid());  // attempt to read link
+    if (readlink(szProcPath, buffer, max) != -1)
+    {
+        dirname (buffer);
+        strcat  (buffer, "/");
+        return strlen(buffer);
+    }
+    return 0;
+}
+#endif
+
+U14API(short) U14LdCmd(short hand, const char* command)
+{
+    char strcopy[MAXSTRLEN+1];      // to hold copy of work string
+    BOOL bGotIt = FALSE;            // have we found the command file?
+    int iFHandle;                   // file handle of command
+#define FNSZ 260
+    char filnam[FNSZ];              // space to build name in
+    char szCmd[25];                 // just the command name with extension
+
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+
+    if (strchr(command, '.') != NULL)       // see if we have full name
+    {
+        if (file_exist(command))            // If the file exists
+        {
+            strcpy(filnam, command);        // use name as is
+            bGotIt = TRUE;                  // Flag no more searching
+        }
+        else                                // not found, get file name for search
+        {
+            char* pStr = strrchr(command, PATHSEP);  // Point to last separator
+            if (pStr != NULL)               // Check we got it
+            {
+                pStr++;                     // move past the backslash
+                strcpy(szCmd, pStr);        // copy file name as is
+            }
+            else
+                strcpy(szCmd, command);     // use as is
+        }
+    }
+    else    // File extension not supplied, so build the command file name
+    {
+        char szExt[8];
+        strcpy(szCmd, command);             // Build command file name
+        ExtForType(asType1401[hand], szExt);// File extension string
+        strcat(szCmd, szExt);               // add it to the end
+    }
+
+    // Next place to look is in the 1401 folder in the same place as the
+    // application was run from.
+    if (!bGotIt)                            // Still not got it?
+    {
+        DWORD dwLen = GetModuleFileName(NULL, filnam, FNSZ); // Get app path
+        if (dwLen > 0)                      // and use it as path if found
+        {
+            char* pStr = strrchr(filnam, PATHSEP);    // Point to last separator
+            if (pStr != NULL)
+            {
+                *(++pStr) = 0;                  // Terminate string there
+                if (strlen(filnam) < FNSZ-6)    // make sure we have space
+                {
+                    strcat(filnam, "1401" PATHSEPSTR);  // add in 1401 subdir
+                    strcat(filnam,szCmd);
+                    bGotIt = (BOOL)file_exist(filnam);  // See if file exists
+                }
+            }
+        }
+    }
+
+    // Next place to look is in whatever path is set by the 1401DIR environment
+    // variable, if it exists.
+    if (!bGotIt)                            // Need to do more searches?/
+    {
+        char* pStr = getenv("1401DIR");     // Try to find environment var
+        if (pStr != NULL)                   // and use it as path if found
+        {
+            strcpy(filnam, pStr);                   // Use path in environment
+            if (filnam[strlen(filnam)-1] != PATHSEP)// We need separator
+                strcat(filnam, PATHSEPSTR);
+            strcat(filnam, szCmd);
+            bGotIt = (BOOL)file_exist(filnam); // Got this one?
+        }
+    }
+
+    // Last place to look is the default location.
+    if (!bGotIt)                        // Need to do more searches?
+    {
+        strcpy(filnam, DEFCMDPATH);     // Use default path
+        strcat(filnam, szCmd);
+        bGotIt = file_exist(filnam);    // Got this one?
+    }
+
+    iFHandle = file_open(filnam);
+    if (iFHandle == -1)
+        sErr = U14ERR_NOFILE;
+    else
+    {                                   // first read in the header block
+        CMDHEAD rCmdHead;               // to hold the command header
+        if (file_read(iFHandle, &rCmdHead, sizeof(CMDHEAD)))
+        {
+            size_t nComSize = rCmdHead.wCmdSize;
+            char* pMem = malloc(nComSize);
+            if (pMem != NULL)
+            {
+                file_seek(iFHandle, sizeof(CMDHEAD));
+                if (file_read(iFHandle, pMem, (UINT)nComSize))
+                {
+                    sErr = U14SetTransArea(hand, 0, (void *)pMem, (DWORD)nComSize, ESZBYTES);
+                    if (sErr == U14ERR_NOERROR)
+                    {
+                        sprintf(strcopy, "CLOAD,0,$%X;", (int)nComSize);
+                        sErr = U14SendString(hand, strcopy);
+                        if (sErr == U14ERR_NOERROR)
+                        {
+                            sErr = U14CheckErr(hand);     // Use ERR to check for done
+                            if (sErr > 0)
+                                sErr = U14ERR_CLOADERR;   // If an error, this code
+                        }
+                        U14UnSetTransfer(hand, 0);  // release transfer area
+                    }
+                }
+                else
+                    sErr = U14ERR_READERR;
+                free(pMem);
+            }
+            else
+                sErr = U14ERR_HOSTSPACE;    // memory allocate failed
+        }
+        else
+            sErr = U14ERR_READERR;
+
+        file_close(iFHandle);               // close the file
+    }
+
+    return sErr;
+}
+
+
+/****************************************************************************
+** Ld
+** Loads a command into the 1401
+** Returns NOERROR code or a long with error in lo word and index of
+** command that failed in high word
+****************************************************************************/
+U14API(DWORD) U14Ld(short hand, const char* vl, const char* str)
+{
+    DWORD dwIndex = 0;              // index to current command
+    long lErr = U14ERR_NOERROR;     // what the error was that went wrong
+    char strcopy[MAXSTRLEN+1];      // stores unmodified str parameter
+    char szFExt[8];                 // The command file extension
+    short sErr = CheckHandle(hand);
+    if (sErr != U14ERR_NOERROR)
+        return sErr;
+
+    ExtForType(asType1401[hand], szFExt);   // File extension string
+    strcpy(strcopy, str);               // to avoid changing original
+
+    // now break out one command at a time and see if loaded
+    if (*str)                           // if anything there
+    {
+        BOOL bDone = FALSE;             // true when finished all commands
+        int iLoop1 = 0;                 // Point at start of string for command name
+        int iLoop2 = 0;                 // and at start of str parameter
+        do                              // repeat until end of str
+        {
+            char filnam[MAXSTRLEN+1];   // filename to use
+            char szFName[MAXSTRLEN+1];  // filename work string
+
+            if (!strcopy[iLoop1])       // at the end of the string?
+                bDone = TRUE;           // set the finish flag
+
+            if (bDone || (strcopy[iLoop1] == ','))  // end of cmd?
+            {
+                U14LONG er[5];                  // Used to read back error results
+                ++dwIndex;                      // Keep count of command number, first is 1
+                szFName[iLoop2]=(char)0;        // null terminate name of command
+
+                strncpy(szLastName, szFName, sizeof(szLastName));    // Save for error info
+                szLastName[sizeof(szLastName)-1] = 0;
+                strncat(szLastName, szFExt, sizeof(szLastName));     // with extension included
+                szLastName[sizeof(szLastName)-1] = 0;
+
+                U14SendString(hand, szFName);   // ask if loaded
+                U14SendString(hand, ";ERR;");   // add err return
+
+                lErr = U14LongsFrom1401(hand, er, 5);
+                if (lErr > 0)
+                {
+                    lErr = U14ERR_NOERROR;
+                    if (er[0] == 255)           // if command not loaded at all
+                    {
+                        if (vl && *vl)          // if we have a path name
+                        {
+                            strcpy(filnam, vl);
+                            if (strchr("\\/:", filnam[strlen(filnam)-1]) == NULL)
+                                strcat(filnam, PATHSEPSTR); // add separator if none found
+                            strcat(filnam, szFName);    // add the file name
+                            strcat(filnam, szFExt);     // and extension
+                        }
+                        else
+                            strcpy(filnam, szFName);    // simple name
+
+                        lErr = U14LdCmd(hand, filnam);  // load cmd
+                        if (lErr != U14ERR_NOERROR)     // spot any errors
+                            bDone = TRUE;               // give up if an error
+                    }
+                }
+                else
+                    bDone = TRUE;       // give up if an error
+
+                iLoop2 = 0;             // Reset pointer to command name string
+                ++iLoop1;               // and move on through str parameter
+            }
+            else
+                szFName[iLoop2++] = strcopy[iLoop1++];  // no command end, so copy 1 char
+        }
+        while (!bDone);
+    }
+
+    if (lErr == U14ERR_NOERROR)
+    {
+        szLastName[0] = 0;      // No error, so clean out command name here
+        return lErr;
+    }
+    else
+        return ((dwIndex<<16) | ((DWORD)lErr & 0x0000FFFF));
+}
+
+// Initialise the library (if not initialised) and return the library version
+U14API(int) U14InitLib(void)
+{
+    int iRetVal = U14LIB_VERSION;
+    if (iAttached == 0)         // only do this the first time please
+    {
+        int i;
+#ifdef _IS_WINDOWS_
+        int j;
+        DWORD   dwVersion = GetVersion();
+        bWindows9x = FALSE;                  // Assume not Win9x
+
+        if (dwVersion & 0x80000000)                 // if not windows NT
+        {
+            if ((LOBYTE(LOWORD(dwVersion)) < 4) &&  // if Win32s or...
+                 (HIBYTE(LOWORD(dwVersion)) < 95))  // ...below Windows 95
+            iRetVal = 0;                            // We do not support this
+        else
+            bWindows9x = TRUE;                      // Flag we have Win9x
+        }
+#endif
+        
+        for (i = 0; i < MAX1401; i++)               // initialise the device area
+        {
+            aHand1401[i] = INVALID_HANDLE_VALUE;    // Clear handle values
+            asType1401[i] = U14TYPEUNKNOWN;         // and 1401 type codes
+            alTimeOutPeriod[i] = 3000;              // 3 second timeouts
+#ifdef _IS_WINDOWS_
+#ifndef _WIN64
+            abUseNTDIOC[i] = (BOOL)!bWindows9x;
+#endif
+            aXferEvent[i] = NULL;                   // there are no Xfer events
+            for (j = 0; j < MAX_TRANSAREAS; j++)    // Clear out locked area info
+            {
+                apAreas[i][j] = NULL;
+                auAreas[i][j] = 0;
+            }
+#endif
+        }
+    }
+    return iRetVal;
+}
+
+///--------------------------------------------------------------------------------
+/// Functions called when the library is loaded and unloaded to give us a chance to
+/// setup the library.
+
+
+#ifdef _IS_WINDOWS_
+#ifndef U14_NOT_DLL
+/****************************************************************************
+** FUNCTION: DllMain(HANDLE, DWORD, LPVOID)
+** LibMain is called by Windows when the DLL is initialized, Thread Attached,
+** and other times. Refer to SDK documentation, as to the different ways this
+** may be called.
+****************************************************************************/
+INT APIENTRY DllMain(HANDLE hInst, DWORD ul_reason_being_called, LPVOID lpReserved)
+{
+    int iRetVal = 1;
+
+    switch (ul_reason_being_called)
+    {
+    case DLL_PROCESS_ATTACH:
+        iRetVal = U14InitLib() > 0;         // does nothing if iAttached != 0
+        ++iAttached;                        // count times attached
+        break;
+
+    case DLL_PROCESS_DETACH:
+        if (--iAttached == 0)               // last man out?
+            U14CloseAll();                  // release all open handles
+        break;
+    }
+    return iRetVal;
+
+    UNREFERENCED_PARAMETER(lpReserved);
+}
+#endif
+#endif
+#ifdef LINUX
+void __attribute__((constructor)) use1401_load(void)
+{
+    U14InitLib();
+    ++iAttached;
+}
+
+void __attribute__((destructor)) use1401_unload(void)
+{
+        if (--iAttached == 0)               // last man out?
+            U14CloseAll();                  // release all open handles
+}
+#endif
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 6cee7855b019..2093403af253 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -549,6 +549,23 @@ menuconfig COMEDI_PCI_DRIVERS
 
 if COMEDI_PCI_DRIVERS
 
+config COMEDI_8255_PCI
+	tristate "Generic PCI based 8255 digital i/o board support"
+	select COMEDI_8255
+	---help---
+	  Enable support for PCI based 8255 digital i/o boards. This driver
+	  provides a PCI wrapper around the generic 8255 driver.
+
+	  Supported boards:
+	    ADlink - PCI-7224, PCI-7248, and PCI-7296
+	    Measurement Computing - PCI-DIO24, PCI-DIO24H, PCI-DIO48H and
+	                            PCI-DIO96H
+	    National Instruments - PCI-DIO-96, PCI-DIO-96B, PXI-6508, PCI-6503,
+	                           PCI-6503B, PCI-6503X, and PXI-6503
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called 8255_pci.
+
 config COMEDI_ADDI_APCI_035
 	tristate "ADDI-DATA APCI_035 support"
 	depends on VIRT_TO_BUS
@@ -676,30 +693,16 @@ config COMEDI_ADL_PCI6208
 	  To compile this driver as a module, choose M here: the module will be
 	  called adl_pci6208.
 
-config COMEDI_ADL_PCI7230
-	tristate "ADLink PCI-7230 digital io board support"
-	---help---
-	  Enable support for ADlink PCI-7230 digital io board support
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called adl_pci7230.
-
-config COMEDI_ADL_PCI7296
-	tristate "ADLink PCI-7296 96 ch. digital io board support"
-	select COMEDI_8255
+config COMEDI_ADL_PCI7X3X
+	tristate "ADLink PCI-723X/743X isolated digital i/o board support"
 	---help---
-	  Enable support for ADlink PCI-7296 96 ch. digital io board support
+	  Enable support for ADlink PCI-723X/743X isolated digital i/o boards.
+	  Supported boards include the 32-channel PCI-7230 (16 in/16 out),
+	  PCI-7233 (32 in), and PCI-7234 (32 out) as well as the 64-channel
+	  PCI-7432 (32 in/32 out), PCI-7433 (64 in), and PCI-7434 (64 out).
 
 	  To compile this driver as a module, choose M here: the module will be
-	  called adl_pci7296.
-
-config COMEDI_ADL_PCI7432
-	tristate "ADLink PCI-7432 64 ch. isolated digital io board support"
-	---help---
-	  Enable support for ADlink PCI-7432 64 ch. isolated digital io board
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called adl_pci7432.
+	  called adl_pci7x3x.
 
 config COMEDI_ADL_PCI8164
 	tristate "ADLink PCI-8164 4 Axes Motion Control board support"
@@ -935,16 +938,6 @@ config COMEDI_CB_PCIDDA
 	  To compile this driver as a module, choose M here: the module will be
 	  called cb_pcidda.
 
-config COMEDI_CB_PCIDIO
-	tristate "MeasurementComputing PCI-DIO series support"
-	select COMEDI_8255
-	---help---
-	  Enable support for ComputerBoards/MeasurementComputing PCI-DIO series
-	  PCI-DIO24, PCI-DIO24H and PCI-DIO48H
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called cb_pcidio.
-
 config COMEDI_CB_PCIMDAS
 	tristate "MeasurementComputing PCIM-DAS1602/16 support"
 	select COMEDI_8255
@@ -1039,15 +1032,12 @@ config COMEDI_NI_LABPC
 	  called ni_labpc.
 
 config COMEDI_NI_PCIDIO
-	tristate "NI PCI-DIO32HS, PCI-DIO96, PCI-6533, PCI-6503 support"
+	tristate "NI PCI-DIO32HS, PCI-6533, PCI-6534 support"
 	select COMEDI_MITE
 	select COMEDI_8255
 	---help---
 	  Enable support for National Instruments PCI-DIO-32HS, PXI-6533,
-	  PCI-DIO-96, PCI-DIO-96B, PXI-6508, PCI-6503, PCI-6503B, PCI-6503X,
-	  PXI-6503, PCI-6533 and PCI-6534
-	  The DIO-96 appears as four 8255 subdevices. See the 8255
-	  driver notes for details.
+	  PCI-6533 and PCI-6534
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called ni_pcidio.
@@ -1262,8 +1252,8 @@ config COMEDI_8255
 	  that has an 8255 chip. For multifunction boards, the main driver will
 	  configure the 8255 subdevice automatically.
 
-	  Note that most PCI 8255 boards do NOT work with this driver, and
-	  need a separate driver as a wrapper.
+	  Note that most PCI based 8255 boards use the 8255_pci driver as a
+	  wrapper around this driver.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called 8255.
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index 8ea55aef10a7..133f013e0f6d 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -208,92 +208,92 @@
 
 /* subdevice types */
 
-	enum comedi_subdevice_type {
-		COMEDI_SUBD_UNUSED,	/* unused by driver */
-		COMEDI_SUBD_AI,	/* analog input */
-		COMEDI_SUBD_AO,	/* analog output */
-		COMEDI_SUBD_DI,	/* digital input */
-		COMEDI_SUBD_DO,	/* digital output */
-		COMEDI_SUBD_DIO,	/* digital input/output */
-		COMEDI_SUBD_COUNTER,	/* counter */
-		COMEDI_SUBD_TIMER,	/* timer */
-		COMEDI_SUBD_MEMORY,	/* memory, EEPROM, DPRAM */
-		COMEDI_SUBD_CALIB,	/* calibration DACs */
-		COMEDI_SUBD_PROC,	/* processor, DSP */
-		COMEDI_SUBD_SERIAL,	/* serial IO */
-		COMEDI_SUBD_PWM	/* PWM */
-	};
+enum comedi_subdevice_type {
+	COMEDI_SUBD_UNUSED,	/* unused by driver */
+	COMEDI_SUBD_AI,		/* analog input */
+	COMEDI_SUBD_AO,		/* analog output */
+	COMEDI_SUBD_DI,		/* digital input */
+	COMEDI_SUBD_DO,		/* digital output */
+	COMEDI_SUBD_DIO,	/* digital input/output */
+	COMEDI_SUBD_COUNTER,	/* counter */
+	COMEDI_SUBD_TIMER,	/* timer */
+	COMEDI_SUBD_MEMORY,	/* memory, EEPROM, DPRAM */
+	COMEDI_SUBD_CALIB,	/* calibration DACs */
+	COMEDI_SUBD_PROC,	/* processor, DSP */
+	COMEDI_SUBD_SERIAL,	/* serial IO */
+	COMEDI_SUBD_PWM		/* PWM */
+};
 
 /* configuration instructions */
 
-	enum configuration_ids {
-		INSN_CONFIG_DIO_INPUT = 0,
-		INSN_CONFIG_DIO_OUTPUT = 1,
-		INSN_CONFIG_DIO_OPENDRAIN = 2,
-		INSN_CONFIG_ANALOG_TRIG = 16,
+enum configuration_ids {
+	INSN_CONFIG_DIO_INPUT = 0,
+	INSN_CONFIG_DIO_OUTPUT = 1,
+	INSN_CONFIG_DIO_OPENDRAIN = 2,
+	INSN_CONFIG_ANALOG_TRIG = 16,
 /*	INSN_CONFIG_WAVEFORM = 17, */
 /*	INSN_CONFIG_TRIG = 18, */
 /*	INSN_CONFIG_COUNTER = 19, */
-		INSN_CONFIG_ALT_SOURCE = 20,
-		INSN_CONFIG_DIGITAL_TRIG = 21,
-		INSN_CONFIG_BLOCK_SIZE = 22,
-		INSN_CONFIG_TIMER_1 = 23,
-		INSN_CONFIG_FILTER = 24,
-		INSN_CONFIG_CHANGE_NOTIFY = 25,
-
-		 /*ALPHA*/ INSN_CONFIG_SERIAL_CLOCK = 26,
-		INSN_CONFIG_BIDIRECTIONAL_DATA = 27,
-		INSN_CONFIG_DIO_QUERY = 28,
-		INSN_CONFIG_PWM_OUTPUT = 29,
-		INSN_CONFIG_GET_PWM_OUTPUT = 30,
-		INSN_CONFIG_ARM = 31,
-		INSN_CONFIG_DISARM = 32,
-		INSN_CONFIG_GET_COUNTER_STATUS = 33,
-		INSN_CONFIG_RESET = 34,
-		/* Use CTR as single pulsegenerator */
-		INSN_CONFIG_GPCT_SINGLE_PULSE_GENERATOR = 1001,
-		/* Use CTR as pulsetraingenerator */
-		INSN_CONFIG_GPCT_PULSE_TRAIN_GENERATOR = 1002,
-		/* Use the counter as encoder */
-		INSN_CONFIG_GPCT_QUADRATURE_ENCODER = 1003,
-		INSN_CONFIG_SET_GATE_SRC = 2001,	/* Set gate source */
-		INSN_CONFIG_GET_GATE_SRC = 2002,	/* Get gate source */
-		/* Set master clock source */
-		INSN_CONFIG_SET_CLOCK_SRC = 2003,
-		INSN_CONFIG_GET_CLOCK_SRC = 2004, /* Get master clock source */
-		INSN_CONFIG_SET_OTHER_SRC = 2005, /* Set other source */
-		/* INSN_CONFIG_GET_OTHER_SRC = 2006,*//* Get other source */
-		/* Get size in bytes of subdevice's on-board fifos used during
-		 * streaming input/output */
-		INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE = 2006,
-		INSN_CONFIG_SET_COUNTER_MODE = 4097,
-		/* INSN_CONFIG_8254_SET_MODE is deprecated */
-		INSN_CONFIG_8254_SET_MODE = INSN_CONFIG_SET_COUNTER_MODE,
-		INSN_CONFIG_8254_READ_STATUS = 4098,
-		INSN_CONFIG_SET_ROUTING = 4099,
-		INSN_CONFIG_GET_ROUTING = 4109,
-/* PWM */
-		INSN_CONFIG_PWM_SET_PERIOD = 5000,	/* sets frequency */
-		INSN_CONFIG_PWM_GET_PERIOD = 5001,	/* gets frequency */
-		INSN_CONFIG_GET_PWM_STATUS = 5002,	/* is it running? */
-		/* sets H bridge: duty cycle and sign bit for a relay at the
-		 * same time */
-		INSN_CONFIG_PWM_SET_H_BRIDGE = 5003,
-		/* gets H bridge data: duty cycle and the sign bit */
-		INSN_CONFIG_PWM_GET_H_BRIDGE = 5004
-	};
-
-	enum comedi_io_direction {
-		COMEDI_INPUT = 0,
-		COMEDI_OUTPUT = 1,
-		COMEDI_OPENDRAIN = 2
-	};
-
-	enum comedi_support_level {
-		COMEDI_UNKNOWN_SUPPORT = 0,
-		COMEDI_SUPPORTED,
-		COMEDI_UNSUPPORTED
-	};
+	INSN_CONFIG_ALT_SOURCE = 20,
+	INSN_CONFIG_DIGITAL_TRIG = 21,
+	INSN_CONFIG_BLOCK_SIZE = 22,
+	INSN_CONFIG_TIMER_1 = 23,
+	INSN_CONFIG_FILTER = 24,
+	INSN_CONFIG_CHANGE_NOTIFY = 25,
+
+	INSN_CONFIG_SERIAL_CLOCK = 26,	/*ALPHA*/
+	INSN_CONFIG_BIDIRECTIONAL_DATA = 27,
+	INSN_CONFIG_DIO_QUERY = 28,
+	INSN_CONFIG_PWM_OUTPUT = 29,
+	INSN_CONFIG_GET_PWM_OUTPUT = 30,
+	INSN_CONFIG_ARM = 31,
+	INSN_CONFIG_DISARM = 32,
+	INSN_CONFIG_GET_COUNTER_STATUS = 33,
+	INSN_CONFIG_RESET = 34,
+	/* Use CTR as single pulsegenerator */
+	INSN_CONFIG_GPCT_SINGLE_PULSE_GENERATOR = 1001,
+	/* Use CTR as pulsetraingenerator */
+	INSN_CONFIG_GPCT_PULSE_TRAIN_GENERATOR = 1002,
+	/* Use the counter as encoder */
+	INSN_CONFIG_GPCT_QUADRATURE_ENCODER = 1003,
+	INSN_CONFIG_SET_GATE_SRC = 2001,	/* Set gate source */
+	INSN_CONFIG_GET_GATE_SRC = 2002,	/* Get gate source */
+	/* Set master clock source */
+	INSN_CONFIG_SET_CLOCK_SRC = 2003,
+	INSN_CONFIG_GET_CLOCK_SRC = 2004, /* Get master clock source */
+	INSN_CONFIG_SET_OTHER_SRC = 2005, /* Set other source */
+	/* INSN_CONFIG_GET_OTHER_SRC = 2006,*//* Get other source */
+	/* Get size in bytes of subdevice's on-board fifos used during
+	 * streaming input/output */
+	INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE = 2006,
+	INSN_CONFIG_SET_COUNTER_MODE = 4097,
+	/* INSN_CONFIG_8254_SET_MODE is deprecated */
+	INSN_CONFIG_8254_SET_MODE = INSN_CONFIG_SET_COUNTER_MODE,
+	INSN_CONFIG_8254_READ_STATUS = 4098,
+	INSN_CONFIG_SET_ROUTING = 4099,
+	INSN_CONFIG_GET_ROUTING = 4109,
+	/* PWM */
+	INSN_CONFIG_PWM_SET_PERIOD = 5000,	/* sets frequency */
+	INSN_CONFIG_PWM_GET_PERIOD = 5001,	/* gets frequency */
+	INSN_CONFIG_GET_PWM_STATUS = 5002,	/* is it running? */
+	/* sets H bridge: duty cycle and sign bit for a relay at the
+	 * same time */
+	INSN_CONFIG_PWM_SET_H_BRIDGE = 5003,
+	/* gets H bridge data: duty cycle and the sign bit */
+	INSN_CONFIG_PWM_GET_H_BRIDGE = 5004
+};
+
+enum comedi_io_direction {
+	COMEDI_INPUT = 0,
+	COMEDI_OUTPUT = 1,
+	COMEDI_OPENDRAIN = 2
+};
+
+enum comedi_support_level {
+	COMEDI_UNKNOWN_SUPPORT = 0,
+	COMEDI_SUPPORTED,
+	COMEDI_UNSUPPORTED
+};
 
 /* ioctls */
 
@@ -317,133 +317,133 @@
 
 /* structures */
 
-	struct comedi_trig {
-		unsigned int subdev;	/* subdevice */
-		unsigned int mode;	/* mode */
-		unsigned int flags;
-		unsigned int n_chan;	/* number of channels */
-		unsigned int *chanlist;	/* channel/range list */
-		short *data;	/* data list, size depends on subd flags */
-		unsigned int n;	/* number of scans */
-		unsigned int trigsrc;
-		unsigned int trigvar;
-		unsigned int trigvar1;
-		unsigned int data_len;
-		unsigned int unused[3];
-	};
-
-	struct comedi_insn {
-		unsigned int insn;
-		unsigned int n;
-		unsigned int __user *data;
-		unsigned int subdev;
-		unsigned int chanspec;
-		unsigned int unused[3];
-	};
-
-	struct comedi_insnlist {
-		unsigned int n_insns;
-		struct comedi_insn __user *insns;
-	};
-
-	struct comedi_cmd {
-		unsigned int subdev;
-		unsigned int flags;
-
-		unsigned int start_src;
-		unsigned int start_arg;
-
-		unsigned int scan_begin_src;
-		unsigned int scan_begin_arg;
-
-		unsigned int convert_src;
-		unsigned int convert_arg;
-
-		unsigned int scan_end_src;
-		unsigned int scan_end_arg;
-
-		unsigned int stop_src;
-		unsigned int stop_arg;
-
-		unsigned int __user *chanlist;	/* channel/range list */
-		unsigned int chanlist_len;
-
-		short __user *data; /* data list, size depends on subd flags */
-		unsigned int data_len;
-	};
-
-	struct comedi_chaninfo {
-		unsigned int subdev;
-		unsigned int __user *maxdata_list;
-		unsigned int __user *flaglist;
-		unsigned int __user *rangelist;
-		unsigned int unused[4];
-	};
-
-	struct comedi_rangeinfo {
-		unsigned int range_type;
-		void __user *range_ptr;
-	};
-
-	struct comedi_krange {
-		int min;	/* fixed point, multiply by 1e-6 */
-		int max;	/* fixed point, multiply by 1e-6 */
-		unsigned int flags;
-	};
-
-	struct comedi_subdinfo {
-		unsigned int type;
-		unsigned int n_chan;
-		unsigned int subd_flags;
-		unsigned int timer_type;
-		unsigned int len_chanlist;
-		unsigned int maxdata;
-		unsigned int flags;	/* channel flags */
-		unsigned int range_type;	/* lookup in kernel */
-		unsigned int settling_time_0;
-		/* see support_level enum for values */
-		unsigned insn_bits_support;
-		unsigned int unused[8];
-	};
-
-	struct comedi_devinfo {
-		unsigned int version_code;
-		unsigned int n_subdevs;
-		char driver_name[COMEDI_NAMELEN];
-		char board_name[COMEDI_NAMELEN];
-		int read_subdevice;
-		int write_subdevice;
-		int unused[30];
-	};
-
-	struct comedi_devconfig {
-		char board_name[COMEDI_NAMELEN];
-		int options[COMEDI_NDEVCONFOPTS];
-	};
-
-	struct comedi_bufconfig {
-		unsigned int subdevice;
-		unsigned int flags;
-
-		unsigned int maximum_size;
-		unsigned int size;
-
-		unsigned int unused[4];
-	};
-
-	struct comedi_bufinfo {
-		unsigned int subdevice;
-		unsigned int bytes_read;
-
-		unsigned int buf_write_ptr;
-		unsigned int buf_read_ptr;
-		unsigned int buf_write_count;
-		unsigned int buf_read_count;
-
-		unsigned int bytes_written;
-
-		unsigned int unused[4];
-	};
+struct comedi_trig {
+	unsigned int subdev;	/* subdevice */
+	unsigned int mode;	/* mode */
+	unsigned int flags;
+	unsigned int n_chan;	/* number of channels */
+	unsigned int *chanlist;	/* channel/range list */
+	short *data;		/* data list, size depends on subd flags */
+	unsigned int n;		/* number of scans */
+	unsigned int trigsrc;
+	unsigned int trigvar;
+	unsigned int trigvar1;
+	unsigned int data_len;
+	unsigned int unused[3];
+};
+
+struct comedi_insn {
+	unsigned int insn;
+	unsigned int n;
+	unsigned int __user *data;
+	unsigned int subdev;
+	unsigned int chanspec;
+	unsigned int unused[3];
+};
+
+struct comedi_insnlist {
+	unsigned int n_insns;
+	struct comedi_insn __user *insns;
+};
+
+struct comedi_cmd {
+	unsigned int subdev;
+	unsigned int flags;
+
+	unsigned int start_src;
+	unsigned int start_arg;
+
+	unsigned int scan_begin_src;
+	unsigned int scan_begin_arg;
+
+	unsigned int convert_src;
+	unsigned int convert_arg;
+
+	unsigned int scan_end_src;
+	unsigned int scan_end_arg;
+
+	unsigned int stop_src;
+	unsigned int stop_arg;
+
+	unsigned int *chanlist;	/* channel/range list */
+	unsigned int chanlist_len;
+
+	short __user *data; /* data list, size depends on subd flags */
+	unsigned int data_len;
+};
+
+struct comedi_chaninfo {
+	unsigned int subdev;
+	unsigned int __user *maxdata_list;
+	unsigned int __user *flaglist;
+	unsigned int __user *rangelist;
+	unsigned int unused[4];
+};
+
+struct comedi_rangeinfo {
+	unsigned int range_type;
+	void __user *range_ptr;
+};
+
+struct comedi_krange {
+	int min;	/* fixed point, multiply by 1e-6 */
+	int max;	/* fixed point, multiply by 1e-6 */
+	unsigned int flags;
+};
+
+struct comedi_subdinfo {
+	unsigned int type;
+	unsigned int n_chan;
+	unsigned int subd_flags;
+	unsigned int timer_type;
+	unsigned int len_chanlist;
+	unsigned int maxdata;
+	unsigned int flags;		/* channel flags */
+	unsigned int range_type;	/* lookup in kernel */
+	unsigned int settling_time_0;
+	/* see support_level enum for values */
+	unsigned insn_bits_support;
+	unsigned int unused[8];
+};
+
+struct comedi_devinfo {
+	unsigned int version_code;
+	unsigned int n_subdevs;
+	char driver_name[COMEDI_NAMELEN];
+	char board_name[COMEDI_NAMELEN];
+	int read_subdevice;
+	int write_subdevice;
+	int unused[30];
+};
+
+struct comedi_devconfig {
+	char board_name[COMEDI_NAMELEN];
+	int options[COMEDI_NDEVCONFOPTS];
+};
+
+struct comedi_bufconfig {
+	unsigned int subdevice;
+	unsigned int flags;
+
+	unsigned int maximum_size;
+	unsigned int size;
+
+	unsigned int unused[4];
+};
+
+struct comedi_bufinfo {
+	unsigned int subdevice;
+	unsigned int bytes_read;
+
+	unsigned int buf_write_ptr;
+	unsigned int buf_read_ptr;
+	unsigned int buf_write_count;
+	unsigned int buf_read_count;
+
+	unsigned int bytes_written;
+
+	unsigned int unused[4];
+};
 
 /* range stuff */
 
@@ -495,306 +495,306 @@
 
 */
 
-	enum i8254_mode {
-		I8254_MODE0 = (0 << 1),	/* Interrupt on terminal count */
-		I8254_MODE1 = (1 << 1),	/* Hardware retriggerable one-shot */
-		I8254_MODE2 = (2 << 1),	/* Rate generator */
-		I8254_MODE3 = (3 << 1),	/* Square wave mode */
-		I8254_MODE4 = (4 << 1),	/* Software triggered strobe */
-		I8254_MODE5 = (5 << 1),	/* Hardware triggered strobe
-					 * (retriggerable) */
-		I8254_BCD = 1,	/* use binary-coded decimal instead of binary
+enum i8254_mode {
+	I8254_MODE0 = (0 << 1),	/* Interrupt on terminal count */
+	I8254_MODE1 = (1 << 1),	/* Hardware retriggerable one-shot */
+	I8254_MODE2 = (2 << 1),	/* Rate generator */
+	I8254_MODE3 = (3 << 1),	/* Square wave mode */
+	I8254_MODE4 = (4 << 1),	/* Software triggered strobe */
+	I8254_MODE5 = (5 << 1),	/* Hardware triggered strobe
+				 * (retriggerable) */
+	I8254_BCD = 1,		/* use binary-coded decimal instead of binary
 				 * (pretty useless) */
-		I8254_BINARY = 0
-	};
-
-	static inline unsigned NI_USUAL_PFI_SELECT(unsigned pfi_channel)
-	{
-		if (pfi_channel < 10)
-			return 0x1 + pfi_channel;
-		else
-			return 0xb + pfi_channel;
-	}
-
-	static inline unsigned NI_USUAL_RTSI_SELECT(unsigned rtsi_channel)
-	{
-		if (rtsi_channel < 7)
-			return 0xb + rtsi_channel;
-		else
-			return 0x1b;
-	}
+	I8254_BINARY = 0
+};
+
+static inline unsigned NI_USUAL_PFI_SELECT(unsigned pfi_channel)
+{
+	if (pfi_channel < 10)
+		return 0x1 + pfi_channel;
+	else
+		return 0xb + pfi_channel;
+}
+
+static inline unsigned NI_USUAL_RTSI_SELECT(unsigned rtsi_channel)
+{
+	if (rtsi_channel < 7)
+		return 0xb + rtsi_channel;
+	else
+		return 0x1b;
+}
 
 /* mode bits for NI general-purpose counters, set with
  * INSN_CONFIG_SET_COUNTER_MODE */
 #define NI_GPCT_COUNTING_MODE_SHIFT 16
 #define NI_GPCT_INDEX_PHASE_BITSHIFT 20
 #define NI_GPCT_COUNTING_DIRECTION_SHIFT 24
-	enum ni_gpct_mode_bits {
-		NI_GPCT_GATE_ON_BOTH_EDGES_BIT = 0x4,
-		NI_GPCT_EDGE_GATE_MODE_MASK = 0x18,
-		NI_GPCT_EDGE_GATE_STARTS_STOPS_BITS = 0x0,
-		NI_GPCT_EDGE_GATE_STOPS_STARTS_BITS = 0x8,
-		NI_GPCT_EDGE_GATE_STARTS_BITS = 0x10,
-		NI_GPCT_EDGE_GATE_NO_STARTS_NO_STOPS_BITS = 0x18,
-		NI_GPCT_STOP_MODE_MASK = 0x60,
-		NI_GPCT_STOP_ON_GATE_BITS = 0x00,
-		NI_GPCT_STOP_ON_GATE_OR_TC_BITS = 0x20,
-		NI_GPCT_STOP_ON_GATE_OR_SECOND_TC_BITS = 0x40,
-		NI_GPCT_LOAD_B_SELECT_BIT = 0x80,
-		NI_GPCT_OUTPUT_MODE_MASK = 0x300,
-		NI_GPCT_OUTPUT_TC_PULSE_BITS = 0x100,
-		NI_GPCT_OUTPUT_TC_TOGGLE_BITS = 0x200,
-		NI_GPCT_OUTPUT_TC_OR_GATE_TOGGLE_BITS = 0x300,
-		NI_GPCT_HARDWARE_DISARM_MASK = 0xc00,
-		NI_GPCT_NO_HARDWARE_DISARM_BITS = 0x000,
-		NI_GPCT_DISARM_AT_TC_BITS = 0x400,
-		NI_GPCT_DISARM_AT_GATE_BITS = 0x800,
-		NI_GPCT_DISARM_AT_TC_OR_GATE_BITS = 0xc00,
-		NI_GPCT_LOADING_ON_TC_BIT = 0x1000,
-		NI_GPCT_LOADING_ON_GATE_BIT = 0x4000,
-		NI_GPCT_COUNTING_MODE_MASK = 0x7 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_NORMAL_BITS =
-		    0x0 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_QUADRATURE_X1_BITS =
-		    0x1 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_QUADRATURE_X2_BITS =
-		    0x2 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_QUADRATURE_X4_BITS =
-		    0x3 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_TWO_PULSE_BITS =
-		    0x4 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_COUNTING_MODE_SYNC_SOURCE_BITS =
-		    0x6 << NI_GPCT_COUNTING_MODE_SHIFT,
-		NI_GPCT_INDEX_PHASE_MASK = 0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
-		NI_GPCT_INDEX_PHASE_LOW_A_LOW_B_BITS =
-		    0x0 << NI_GPCT_INDEX_PHASE_BITSHIFT,
-		NI_GPCT_INDEX_PHASE_LOW_A_HIGH_B_BITS =
-		    0x1 << NI_GPCT_INDEX_PHASE_BITSHIFT,
-		NI_GPCT_INDEX_PHASE_HIGH_A_LOW_B_BITS =
-		    0x2 << NI_GPCT_INDEX_PHASE_BITSHIFT,
-		NI_GPCT_INDEX_PHASE_HIGH_A_HIGH_B_BITS =
-		    0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
-		NI_GPCT_INDEX_ENABLE_BIT = 0x400000,
-		NI_GPCT_COUNTING_DIRECTION_MASK =
-		    0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
-		NI_GPCT_COUNTING_DIRECTION_DOWN_BITS =
-		    0x00 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
-		NI_GPCT_COUNTING_DIRECTION_UP_BITS =
-		    0x1 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
-		NI_GPCT_COUNTING_DIRECTION_HW_UP_DOWN_BITS =
-		    0x2 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
-		NI_GPCT_COUNTING_DIRECTION_HW_GATE_BITS =
-		    0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
-		NI_GPCT_RELOAD_SOURCE_MASK = 0xc000000,
-		NI_GPCT_RELOAD_SOURCE_FIXED_BITS = 0x0,
-		NI_GPCT_RELOAD_SOURCE_SWITCHING_BITS = 0x4000000,
-		NI_GPCT_RELOAD_SOURCE_GATE_SELECT_BITS = 0x8000000,
-		NI_GPCT_OR_GATE_BIT = 0x10000000,
-		NI_GPCT_INVERT_OUTPUT_BIT = 0x20000000
-	};
+enum ni_gpct_mode_bits {
+	NI_GPCT_GATE_ON_BOTH_EDGES_BIT = 0x4,
+	NI_GPCT_EDGE_GATE_MODE_MASK = 0x18,
+	NI_GPCT_EDGE_GATE_STARTS_STOPS_BITS = 0x0,
+	NI_GPCT_EDGE_GATE_STOPS_STARTS_BITS = 0x8,
+	NI_GPCT_EDGE_GATE_STARTS_BITS = 0x10,
+	NI_GPCT_EDGE_GATE_NO_STARTS_NO_STOPS_BITS = 0x18,
+	NI_GPCT_STOP_MODE_MASK = 0x60,
+	NI_GPCT_STOP_ON_GATE_BITS = 0x00,
+	NI_GPCT_STOP_ON_GATE_OR_TC_BITS = 0x20,
+	NI_GPCT_STOP_ON_GATE_OR_SECOND_TC_BITS = 0x40,
+	NI_GPCT_LOAD_B_SELECT_BIT = 0x80,
+	NI_GPCT_OUTPUT_MODE_MASK = 0x300,
+	NI_GPCT_OUTPUT_TC_PULSE_BITS = 0x100,
+	NI_GPCT_OUTPUT_TC_TOGGLE_BITS = 0x200,
+	NI_GPCT_OUTPUT_TC_OR_GATE_TOGGLE_BITS = 0x300,
+	NI_GPCT_HARDWARE_DISARM_MASK = 0xc00,
+	NI_GPCT_NO_HARDWARE_DISARM_BITS = 0x000,
+	NI_GPCT_DISARM_AT_TC_BITS = 0x400,
+	NI_GPCT_DISARM_AT_GATE_BITS = 0x800,
+	NI_GPCT_DISARM_AT_TC_OR_GATE_BITS = 0xc00,
+	NI_GPCT_LOADING_ON_TC_BIT = 0x1000,
+	NI_GPCT_LOADING_ON_GATE_BIT = 0x4000,
+	NI_GPCT_COUNTING_MODE_MASK = 0x7 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_NORMAL_BITS =
+		0x0 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X1_BITS =
+		0x1 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X2_BITS =
+		0x2 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X4_BITS =
+		0x3 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_TWO_PULSE_BITS =
+		0x4 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_SYNC_SOURCE_BITS =
+		0x6 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_INDEX_PHASE_MASK = 0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_LOW_B_BITS =
+		0x0 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_HIGH_B_BITS =
+		0x1 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_LOW_B_BITS =
+		0x2 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_HIGH_B_BITS =
+		0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_ENABLE_BIT = 0x400000,
+	NI_GPCT_COUNTING_DIRECTION_MASK =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_DOWN_BITS =
+		0x00 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_UP_BITS =
+		0x1 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_UP_DOWN_BITS =
+		0x2 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_GATE_BITS =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_RELOAD_SOURCE_MASK = 0xc000000,
+	NI_GPCT_RELOAD_SOURCE_FIXED_BITS = 0x0,
+	NI_GPCT_RELOAD_SOURCE_SWITCHING_BITS = 0x4000000,
+	NI_GPCT_RELOAD_SOURCE_GATE_SELECT_BITS = 0x8000000,
+	NI_GPCT_OR_GATE_BIT = 0x10000000,
+	NI_GPCT_INVERT_OUTPUT_BIT = 0x20000000
+};
 
 /* Bits for setting a clock source with
  * INSN_CONFIG_SET_CLOCK_SRC when using NI general-purpose counters. */
-	enum ni_gpct_clock_source_bits {
-		NI_GPCT_CLOCK_SRC_SELECT_MASK = 0x3f,
-		NI_GPCT_TIMEBASE_1_CLOCK_SRC_BITS = 0x0,
-		NI_GPCT_TIMEBASE_2_CLOCK_SRC_BITS = 0x1,
-		NI_GPCT_TIMEBASE_3_CLOCK_SRC_BITS = 0x2,
-		NI_GPCT_LOGIC_LOW_CLOCK_SRC_BITS = 0x3,
-		NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS = 0x4,
-		NI_GPCT_NEXT_TC_CLOCK_SRC_BITS = 0x5,
-		/* NI 660x-specific */
-		NI_GPCT_SOURCE_PIN_i_CLOCK_SRC_BITS = 0x6,
-		NI_GPCT_PXI10_CLOCK_SRC_BITS = 0x7,
-		NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS = 0x8,
-		NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS = 0x9,
-		NI_GPCT_PRESCALE_MODE_CLOCK_SRC_MASK = 0x30000000,
-		NI_GPCT_NO_PRESCALE_CLOCK_SRC_BITS = 0x0,
-		/* divide source by 2 */
-		NI_GPCT_PRESCALE_X2_CLOCK_SRC_BITS = 0x10000000,
-		/* divide source by 8 */
-		NI_GPCT_PRESCALE_X8_CLOCK_SRC_BITS = 0x20000000,
-		NI_GPCT_INVERT_CLOCK_SRC_BIT = 0x80000000
-	};
-	static inline unsigned NI_GPCT_SOURCE_PIN_CLOCK_SRC_BITS(unsigned n)
-	{
-		/* NI 660x-specific */
-		return 0x10 + n;
-	}
-	static inline unsigned NI_GPCT_RTSI_CLOCK_SRC_BITS(unsigned n)
-	{
-		return 0x18 + n;
-	}
-	static inline unsigned NI_GPCT_PFI_CLOCK_SRC_BITS(unsigned n)
-	{
-		/* no pfi on NI 660x */
-		return 0x20 + n;
-	}
+enum ni_gpct_clock_source_bits {
+	NI_GPCT_CLOCK_SRC_SELECT_MASK = 0x3f,
+	NI_GPCT_TIMEBASE_1_CLOCK_SRC_BITS = 0x0,
+	NI_GPCT_TIMEBASE_2_CLOCK_SRC_BITS = 0x1,
+	NI_GPCT_TIMEBASE_3_CLOCK_SRC_BITS = 0x2,
+	NI_GPCT_LOGIC_LOW_CLOCK_SRC_BITS = 0x3,
+	NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS = 0x4,
+	NI_GPCT_NEXT_TC_CLOCK_SRC_BITS = 0x5,
+	/* NI 660x-specific */
+	NI_GPCT_SOURCE_PIN_i_CLOCK_SRC_BITS = 0x6,
+	NI_GPCT_PXI10_CLOCK_SRC_BITS = 0x7,
+	NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS = 0x8,
+	NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS = 0x9,
+	NI_GPCT_PRESCALE_MODE_CLOCK_SRC_MASK = 0x30000000,
+	NI_GPCT_NO_PRESCALE_CLOCK_SRC_BITS = 0x0,
+	/* divide source by 2 */
+	NI_GPCT_PRESCALE_X2_CLOCK_SRC_BITS = 0x10000000,
+	/* divide source by 8 */
+	NI_GPCT_PRESCALE_X8_CLOCK_SRC_BITS = 0x20000000,
+	NI_GPCT_INVERT_CLOCK_SRC_BIT = 0x80000000
+};
+static inline unsigned NI_GPCT_SOURCE_PIN_CLOCK_SRC_BITS(unsigned n)
+{
+	/* NI 660x-specific */
+	return 0x10 + n;
+}
+static inline unsigned NI_GPCT_RTSI_CLOCK_SRC_BITS(unsigned n)
+{
+	return 0x18 + n;
+}
+static inline unsigned NI_GPCT_PFI_CLOCK_SRC_BITS(unsigned n)
+{
+	/* no pfi on NI 660x */
+	return 0x20 + n;
+}
 
 /* Possibilities for setting a gate source with
 INSN_CONFIG_SET_GATE_SRC when using NI general-purpose counters.
 May be bitwise-or'd with CR_EDGE or CR_INVERT. */
-	enum ni_gpct_gate_select {
-		/* m-series gates */
-		NI_GPCT_TIMESTAMP_MUX_GATE_SELECT = 0x0,
-		NI_GPCT_AI_START2_GATE_SELECT = 0x12,
-		NI_GPCT_PXI_STAR_TRIGGER_GATE_SELECT = 0x13,
-		NI_GPCT_NEXT_OUT_GATE_SELECT = 0x14,
-		NI_GPCT_AI_START1_GATE_SELECT = 0x1c,
-		NI_GPCT_NEXT_SOURCE_GATE_SELECT = 0x1d,
-		NI_GPCT_ANALOG_TRIGGER_OUT_GATE_SELECT = 0x1e,
-		NI_GPCT_LOGIC_LOW_GATE_SELECT = 0x1f,
-		/* more gates for 660x */
-		NI_GPCT_SOURCE_PIN_i_GATE_SELECT = 0x100,
-		NI_GPCT_GATE_PIN_i_GATE_SELECT = 0x101,
-		/* more gates for 660x "second gate" */
-		NI_GPCT_UP_DOWN_PIN_i_GATE_SELECT = 0x201,
-		NI_GPCT_SELECTED_GATE_GATE_SELECT = 0x21e,
-		/* m-series "second gate" sources are unknown,
-		 * we should add them here with an offset of 0x300 when
-		 * known. */
-		NI_GPCT_DISABLED_GATE_SELECT = 0x8000,
-	};
-	static inline unsigned NI_GPCT_GATE_PIN_GATE_SELECT(unsigned n)
-	{
-		return 0x102 + n;
-	}
-	static inline unsigned NI_GPCT_RTSI_GATE_SELECT(unsigned n)
-	{
-		return NI_USUAL_RTSI_SELECT(n);
-	}
-	static inline unsigned NI_GPCT_PFI_GATE_SELECT(unsigned n)
-	{
-		return NI_USUAL_PFI_SELECT(n);
-	}
-	static inline unsigned NI_GPCT_UP_DOWN_PIN_GATE_SELECT(unsigned n)
-	{
-		return 0x202 + n;
-	}
+enum ni_gpct_gate_select {
+	/* m-series gates */
+	NI_GPCT_TIMESTAMP_MUX_GATE_SELECT = 0x0,
+	NI_GPCT_AI_START2_GATE_SELECT = 0x12,
+	NI_GPCT_PXI_STAR_TRIGGER_GATE_SELECT = 0x13,
+	NI_GPCT_NEXT_OUT_GATE_SELECT = 0x14,
+	NI_GPCT_AI_START1_GATE_SELECT = 0x1c,
+	NI_GPCT_NEXT_SOURCE_GATE_SELECT = 0x1d,
+	NI_GPCT_ANALOG_TRIGGER_OUT_GATE_SELECT = 0x1e,
+	NI_GPCT_LOGIC_LOW_GATE_SELECT = 0x1f,
+	/* more gates for 660x */
+	NI_GPCT_SOURCE_PIN_i_GATE_SELECT = 0x100,
+	NI_GPCT_GATE_PIN_i_GATE_SELECT = 0x101,
+	/* more gates for 660x "second gate" */
+	NI_GPCT_UP_DOWN_PIN_i_GATE_SELECT = 0x201,
+	NI_GPCT_SELECTED_GATE_GATE_SELECT = 0x21e,
+	/* m-series "second gate" sources are unknown,
+	 * we should add them here with an offset of 0x300 when
+	 * known. */
+	NI_GPCT_DISABLED_GATE_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_GATE_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x102 + n;
+}
+static inline unsigned NI_GPCT_RTSI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_RTSI_SELECT(n);
+}
+static inline unsigned NI_GPCT_PFI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
+static inline unsigned NI_GPCT_UP_DOWN_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x202 + n;
+}
 
 /* Possibilities for setting a source with
 INSN_CONFIG_SET_OTHER_SRC when using NI general-purpose counters. */
-	enum ni_gpct_other_index {
-		NI_GPCT_SOURCE_ENCODER_A,
-		NI_GPCT_SOURCE_ENCODER_B,
-		NI_GPCT_SOURCE_ENCODER_Z
-	};
-	enum ni_gpct_other_select {
-		/* m-series gates */
-		/* Still unknown, probably only need NI_GPCT_PFI_OTHER_SELECT */
-		NI_GPCT_DISABLED_OTHER_SELECT = 0x8000,
-	};
-	static inline unsigned NI_GPCT_PFI_OTHER_SELECT(unsigned n)
-	{
-		return NI_USUAL_PFI_SELECT(n);
-	}
+enum ni_gpct_other_index {
+	NI_GPCT_SOURCE_ENCODER_A,
+	NI_GPCT_SOURCE_ENCODER_B,
+	NI_GPCT_SOURCE_ENCODER_Z
+};
+enum ni_gpct_other_select {
+	/* m-series gates */
+	/* Still unknown, probably only need NI_GPCT_PFI_OTHER_SELECT */
+	NI_GPCT_DISABLED_OTHER_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_PFI_OTHER_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
 
 /* start sources for ni general-purpose counters for use with
 INSN_CONFIG_ARM */
-	enum ni_gpct_arm_source {
-		NI_GPCT_ARM_IMMEDIATE = 0x0,
-		NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1, /* Start both the counter
-						     * and the adjacent paired
-						     * counter simultaneously */
-		/* NI doesn't document bits for selecting hardware arm triggers.
-		 * If the NI_GPCT_ARM_UNKNOWN bit is set, we will pass the least
-		 * significant bits (3 bits for 660x or 5 bits for m-series)
-		 * through to the hardware.  This will at least allow someone to
-		 * figure out what the bits do later. */
-		NI_GPCT_ARM_UNKNOWN = 0x1000,
-	};
+enum ni_gpct_arm_source {
+	NI_GPCT_ARM_IMMEDIATE = 0x0,
+	NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1, /* Start both the counter
+					     * and the adjacent paired
+					     * counter simultaneously */
+	/* NI doesn't document bits for selecting hardware arm triggers.
+	 * If the NI_GPCT_ARM_UNKNOWN bit is set, we will pass the least
+	 * significant bits (3 bits for 660x or 5 bits for m-series)
+	 * through to the hardware.  This will at least allow someone to
+	 * figure out what the bits do later. */
+	NI_GPCT_ARM_UNKNOWN = 0x1000,
+};
 
 /* digital filtering options for ni 660x for use with INSN_CONFIG_FILTER. */
-	enum ni_gpct_filter_select {
-		NI_GPCT_FILTER_OFF = 0x0,
-		NI_GPCT_FILTER_TIMEBASE_3_SYNC = 0x1,
-		NI_GPCT_FILTER_100x_TIMEBASE_1 = 0x2,
-		NI_GPCT_FILTER_20x_TIMEBASE_1 = 0x3,
-		NI_GPCT_FILTER_10x_TIMEBASE_1 = 0x4,
-		NI_GPCT_FILTER_2x_TIMEBASE_1 = 0x5,
-		NI_GPCT_FILTER_2x_TIMEBASE_3 = 0x6
-	};
+enum ni_gpct_filter_select {
+	NI_GPCT_FILTER_OFF = 0x0,
+	NI_GPCT_FILTER_TIMEBASE_3_SYNC = 0x1,
+	NI_GPCT_FILTER_100x_TIMEBASE_1 = 0x2,
+	NI_GPCT_FILTER_20x_TIMEBASE_1 = 0x3,
+	NI_GPCT_FILTER_10x_TIMEBASE_1 = 0x4,
+	NI_GPCT_FILTER_2x_TIMEBASE_1 = 0x5,
+	NI_GPCT_FILTER_2x_TIMEBASE_3 = 0x6
+};
 
 /* PFI digital filtering options for ni m-series for use with
  * INSN_CONFIG_FILTER. */
-	enum ni_pfi_filter_select {
-		NI_PFI_FILTER_OFF = 0x0,
-		NI_PFI_FILTER_125ns = 0x1,
-		NI_PFI_FILTER_6425ns = 0x2,
-		NI_PFI_FILTER_2550us = 0x3
-	};
+enum ni_pfi_filter_select {
+	NI_PFI_FILTER_OFF = 0x0,
+	NI_PFI_FILTER_125ns = 0x1,
+	NI_PFI_FILTER_6425ns = 0x2,
+	NI_PFI_FILTER_2550us = 0x3
+};
 
 /* master clock sources for ni mio boards and INSN_CONFIG_SET_CLOCK_SRC */
-	enum ni_mio_clock_source {
-		NI_MIO_INTERNAL_CLOCK = 0,
-		NI_MIO_RTSI_CLOCK = 1,	/* doesn't work for m-series, use
-					   NI_MIO_PLL_RTSI_CLOCK() */
-		/* the NI_MIO_PLL_* sources are m-series only */
-		NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK = 2,
-		NI_MIO_PLL_PXI10_CLOCK = 3,
-		NI_MIO_PLL_RTSI0_CLOCK = 4
-	};
-	static inline unsigned NI_MIO_PLL_RTSI_CLOCK(unsigned rtsi_channel)
-	{
-		return NI_MIO_PLL_RTSI0_CLOCK + rtsi_channel;
-	}
+enum ni_mio_clock_source {
+	NI_MIO_INTERNAL_CLOCK = 0,
+	NI_MIO_RTSI_CLOCK = 1,	/* doesn't work for m-series, use
+				   NI_MIO_PLL_RTSI_CLOCK() */
+	/* the NI_MIO_PLL_* sources are m-series only */
+	NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK = 2,
+	NI_MIO_PLL_PXI10_CLOCK = 3,
+	NI_MIO_PLL_RTSI0_CLOCK = 4
+};
+static inline unsigned NI_MIO_PLL_RTSI_CLOCK(unsigned rtsi_channel)
+{
+	return NI_MIO_PLL_RTSI0_CLOCK + rtsi_channel;
+}
 
 /* Signals which can be routed to an NI RTSI pin with INSN_CONFIG_SET_ROUTING.
  The numbers assigned are not arbitrary, they correspond to the bits required
  to program the board. */
-	enum ni_rtsi_routing {
-		NI_RTSI_OUTPUT_ADR_START1 = 0,
-		NI_RTSI_OUTPUT_ADR_START2 = 1,
-		NI_RTSI_OUTPUT_SCLKG = 2,
-		NI_RTSI_OUTPUT_DACUPDN = 3,
-		NI_RTSI_OUTPUT_DA_START1 = 4,
-		NI_RTSI_OUTPUT_G_SRC0 = 5,
-		NI_RTSI_OUTPUT_G_GATE0 = 6,
-		NI_RTSI_OUTPUT_RGOUT0 = 7,
-		NI_RTSI_OUTPUT_RTSI_BRD_0 = 8,
-		NI_RTSI_OUTPUT_RTSI_OSC = 12	/* pre-m-series always have RTSI
-						 * clock on line 7 */
-	};
-	static inline unsigned NI_RTSI_OUTPUT_RTSI_BRD(unsigned n)
-	{
-		return NI_RTSI_OUTPUT_RTSI_BRD_0 + n;
-	}
+enum ni_rtsi_routing {
+	NI_RTSI_OUTPUT_ADR_START1 = 0,
+	NI_RTSI_OUTPUT_ADR_START2 = 1,
+	NI_RTSI_OUTPUT_SCLKG = 2,
+	NI_RTSI_OUTPUT_DACUPDN = 3,
+	NI_RTSI_OUTPUT_DA_START1 = 4,
+	NI_RTSI_OUTPUT_G_SRC0 = 5,
+	NI_RTSI_OUTPUT_G_GATE0 = 6,
+	NI_RTSI_OUTPUT_RGOUT0 = 7,
+	NI_RTSI_OUTPUT_RTSI_BRD_0 = 8,
+	NI_RTSI_OUTPUT_RTSI_OSC = 12	/* pre-m-series always have RTSI
+					 * clock on line 7 */
+};
+static inline unsigned NI_RTSI_OUTPUT_RTSI_BRD(unsigned n)
+{
+	return NI_RTSI_OUTPUT_RTSI_BRD_0 + n;
+}
 
 /* Signals which can be routed to an NI PFI pin on an m-series board with
  * INSN_CONFIG_SET_ROUTING.  These numbers are also returned by
  * INSN_CONFIG_GET_ROUTING on pre-m-series boards, even though their routing
  * cannot be changed.  The numbers assigned are not arbitrary, they correspond
  * to the bits required to program the board. */
-	enum ni_pfi_routing {
-		NI_PFI_OUTPUT_PFI_DEFAULT = 0,
-		NI_PFI_OUTPUT_AI_START1 = 1,
-		NI_PFI_OUTPUT_AI_START2 = 2,
-		NI_PFI_OUTPUT_AI_CONVERT = 3,
-		NI_PFI_OUTPUT_G_SRC1 = 4,
-		NI_PFI_OUTPUT_G_GATE1 = 5,
-		NI_PFI_OUTPUT_AO_UPDATE_N = 6,
-		NI_PFI_OUTPUT_AO_START1 = 7,
-		NI_PFI_OUTPUT_AI_START_PULSE = 8,
-		NI_PFI_OUTPUT_G_SRC0 = 9,
-		NI_PFI_OUTPUT_G_GATE0 = 10,
-		NI_PFI_OUTPUT_EXT_STROBE = 11,
-		NI_PFI_OUTPUT_AI_EXT_MUX_CLK = 12,
-		NI_PFI_OUTPUT_GOUT0 = 13,
-		NI_PFI_OUTPUT_GOUT1 = 14,
-		NI_PFI_OUTPUT_FREQ_OUT = 15,
-		NI_PFI_OUTPUT_PFI_DO = 16,
-		NI_PFI_OUTPUT_I_ATRIG = 17,
-		NI_PFI_OUTPUT_RTSI0 = 18,
-		NI_PFI_OUTPUT_PXI_STAR_TRIGGER_IN = 26,
-		NI_PFI_OUTPUT_SCXI_TRIG1 = 27,
-		NI_PFI_OUTPUT_DIO_CHANGE_DETECT_RTSI = 28,
-		NI_PFI_OUTPUT_CDI_SAMPLE = 29,
-		NI_PFI_OUTPUT_CDO_UPDATE = 30
-	};
-	static inline unsigned NI_PFI_OUTPUT_RTSI(unsigned rtsi_channel)
-	{
-		return NI_PFI_OUTPUT_RTSI0 + rtsi_channel;
-	}
+enum ni_pfi_routing {
+	NI_PFI_OUTPUT_PFI_DEFAULT = 0,
+	NI_PFI_OUTPUT_AI_START1 = 1,
+	NI_PFI_OUTPUT_AI_START2 = 2,
+	NI_PFI_OUTPUT_AI_CONVERT = 3,
+	NI_PFI_OUTPUT_G_SRC1 = 4,
+	NI_PFI_OUTPUT_G_GATE1 = 5,
+	NI_PFI_OUTPUT_AO_UPDATE_N = 6,
+	NI_PFI_OUTPUT_AO_START1 = 7,
+	NI_PFI_OUTPUT_AI_START_PULSE = 8,
+	NI_PFI_OUTPUT_G_SRC0 = 9,
+	NI_PFI_OUTPUT_G_GATE0 = 10,
+	NI_PFI_OUTPUT_EXT_STROBE = 11,
+	NI_PFI_OUTPUT_AI_EXT_MUX_CLK = 12,
+	NI_PFI_OUTPUT_GOUT0 = 13,
+	NI_PFI_OUTPUT_GOUT1 = 14,
+	NI_PFI_OUTPUT_FREQ_OUT = 15,
+	NI_PFI_OUTPUT_PFI_DO = 16,
+	NI_PFI_OUTPUT_I_ATRIG = 17,
+	NI_PFI_OUTPUT_RTSI0 = 18,
+	NI_PFI_OUTPUT_PXI_STAR_TRIGGER_IN = 26,
+	NI_PFI_OUTPUT_SCXI_TRIG1 = 27,
+	NI_PFI_OUTPUT_DIO_CHANGE_DETECT_RTSI = 28,
+	NI_PFI_OUTPUT_CDI_SAMPLE = 29,
+	NI_PFI_OUTPUT_CDO_UPDATE = 30
+};
+static inline unsigned NI_PFI_OUTPUT_RTSI(unsigned rtsi_channel)
+{
+	return NI_PFI_OUTPUT_RTSI0 + rtsi_channel;
+}
 
 /* Signals which can be routed to output on a NI PFI pin on a 660x board
  with INSN_CONFIG_SET_ROUTING.  The numbers assigned are
@@ -802,113 +802,112 @@ INSN_CONFIG_ARM */
  to program the board.  Lines 0 to 7 can only be set to
  NI_660X_PFI_OUTPUT_DIO.  Lines 32 to 39 can only be set to
  NI_660X_PFI_OUTPUT_COUNTER. */
-	enum ni_660x_pfi_routing {
-		NI_660X_PFI_OUTPUT_COUNTER = 1,	/* counter */
-		NI_660X_PFI_OUTPUT_DIO = 2,	/* static digital output */
-	};
+enum ni_660x_pfi_routing {
+	NI_660X_PFI_OUTPUT_COUNTER = 1,	/* counter */
+	NI_660X_PFI_OUTPUT_DIO = 2,	/* static digital output */
+};
 
 /* NI External Trigger lines.  These values are not arbitrary, but are related
  * to the bits required to program the board (offset by 1 for historical
  * reasons). */
-	static inline unsigned NI_EXT_PFI(unsigned pfi_channel)
-	{
-		return NI_USUAL_PFI_SELECT(pfi_channel) - 1;
-	}
-	static inline unsigned NI_EXT_RTSI(unsigned rtsi_channel)
-	{
-		return NI_USUAL_RTSI_SELECT(rtsi_channel) - 1;
-	}
+static inline unsigned NI_EXT_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel) - 1;
+}
+static inline unsigned NI_EXT_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel) - 1;
+}
 
 /* status bits for INSN_CONFIG_GET_COUNTER_STATUS */
-	enum comedi_counter_status_flags {
-		COMEDI_COUNTER_ARMED = 0x1,
-		COMEDI_COUNTER_COUNTING = 0x2,
-		COMEDI_COUNTER_TERMINAL_COUNT = 0x4,
-	};
+enum comedi_counter_status_flags {
+	COMEDI_COUNTER_ARMED = 0x1,
+	COMEDI_COUNTER_COUNTING = 0x2,
+	COMEDI_COUNTER_TERMINAL_COUNT = 0x4,
+};
 
 /* Clock sources for CDIO subdevice on NI m-series boards.  Used as the
  * scan_begin_arg for a comedi_command. These sources may also be bitwise-or'd
  * with CR_INVERT to change polarity. */
-	enum ni_m_series_cdio_scan_begin_src {
-		NI_CDIO_SCAN_BEGIN_SRC_GROUND = 0,
-		NI_CDIO_SCAN_BEGIN_SRC_AI_START = 18,
-		NI_CDIO_SCAN_BEGIN_SRC_AI_CONVERT = 19,
-		NI_CDIO_SCAN_BEGIN_SRC_PXI_STAR_TRIGGER = 20,
-		NI_CDIO_SCAN_BEGIN_SRC_G0_OUT = 28,
-		NI_CDIO_SCAN_BEGIN_SRC_G1_OUT = 29,
-		NI_CDIO_SCAN_BEGIN_SRC_ANALOG_TRIGGER = 30,
-		NI_CDIO_SCAN_BEGIN_SRC_AO_UPDATE = 31,
-		NI_CDIO_SCAN_BEGIN_SRC_FREQ_OUT = 32,
-		NI_CDIO_SCAN_BEGIN_SRC_DIO_CHANGE_DETECT_IRQ = 33
-	};
-	static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
-	{
-		return NI_USUAL_PFI_SELECT(pfi_channel);
-	}
-	static inline unsigned
-		NI_CDIO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
-	{
-		return NI_USUAL_RTSI_SELECT(rtsi_channel);
-	}
+enum ni_m_series_cdio_scan_begin_src {
+	NI_CDIO_SCAN_BEGIN_SRC_GROUND = 0,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_START = 18,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_CONVERT = 19,
+	NI_CDIO_SCAN_BEGIN_SRC_PXI_STAR_TRIGGER = 20,
+	NI_CDIO_SCAN_BEGIN_SRC_G0_OUT = 28,
+	NI_CDIO_SCAN_BEGIN_SRC_G1_OUT = 29,
+	NI_CDIO_SCAN_BEGIN_SRC_ANALOG_TRIGGER = 30,
+	NI_CDIO_SCAN_BEGIN_SRC_AO_UPDATE = 31,
+	NI_CDIO_SCAN_BEGIN_SRC_FREQ_OUT = 32,
+	NI_CDIO_SCAN_BEGIN_SRC_DIO_CHANGE_DETECT_IRQ = 33
+};
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
 
 /* scan_begin_src for scan_begin_arg==TRIG_EXT with analog output command on NI
  * boards.  These scan begin sources can also be bitwise-or'd with CR_INVERT to
  * change polarity. */
-	static inline unsigned NI_AO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
-	{
-		return NI_USUAL_PFI_SELECT(pfi_channel);
-	}
-	static inline unsigned NI_AO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
-	{
-		return NI_USUAL_RTSI_SELECT(rtsi_channel);
-	}
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
 
 /* Bits for setting a clock source with
  * INSN_CONFIG_SET_CLOCK_SRC when using NI frequency output subdevice. */
-	enum ni_freq_out_clock_source_bits {
-		NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC,	/* 10 MHz */
-		NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC	/* 100 KHz */
-	};
+enum ni_freq_out_clock_source_bits {
+	NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC,	/* 10 MHz */
+	NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC	/* 100 KHz */
+};
 
 /* Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
  * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
-	enum amplc_dio_clock_source {
-		AMPLC_DIO_CLK_CLKN,	/* per channel external clock
-					   input/output pin (pin is only an
-					   input when clock source set to this
-					   value, otherwise it is an output) */
-		AMPLC_DIO_CLK_10MHZ,	/* 10 MHz internal clock */
-		AMPLC_DIO_CLK_1MHZ,	/* 1 MHz internal clock */
-		AMPLC_DIO_CLK_100KHZ,	/* 100 kHz internal clock */
-		AMPLC_DIO_CLK_10KHZ,	/* 10 kHz internal clock */
-		AMPLC_DIO_CLK_1KHZ,	/* 1 kHz internal clock */
-		AMPLC_DIO_CLK_OUTNM1,	/* output of preceding counter channel
-					   (for channel 0, preceding counter
-					   channel is channel 2 on preceding
-					   counter subdevice, for first counter
-					   subdevice, preceding counter
-					   subdevice is the last counter
-					   subdevice) */
-		AMPLC_DIO_CLK_EXT	/* per chip external input pin */
-	};
+enum amplc_dio_clock_source {
+	AMPLC_DIO_CLK_CLKN,	/* per channel external clock
+				   input/output pin (pin is only an
+				   input when clock source set to this
+				   value, otherwise it is an output) */
+	AMPLC_DIO_CLK_10MHZ,	/* 10 MHz internal clock */
+	AMPLC_DIO_CLK_1MHZ,	/* 1 MHz internal clock */
+	AMPLC_DIO_CLK_100KHZ,	/* 100 kHz internal clock */
+	AMPLC_DIO_CLK_10KHZ,	/* 10 kHz internal clock */
+	AMPLC_DIO_CLK_1KHZ,	/* 1 kHz internal clock */
+	AMPLC_DIO_CLK_OUTNM1,	/* output of preceding counter channel
+				   (for channel 0, preceding counter
+				   channel is channel 2 on preceding
+				   counter subdevice, for first counter
+				   subdevice, preceding counter
+				   subdevice is the last counter
+				   subdevice) */
+	AMPLC_DIO_CLK_EXT	/* per chip external input pin */
+};
 
 /* Values for setting a gate source with INSN_CONFIG_SET_GATE_SRC for
  * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
-	enum amplc_dio_gate_source {
-		AMPLC_DIO_GAT_VCC,	/* internal high logic level */
-		AMPLC_DIO_GAT_GND,	/* internal low logic level */
-		AMPLC_DIO_GAT_GATN,	/* per channel external gate input */
-		AMPLC_DIO_GAT_NOUTNM2,	/* negated output of counter channel
-					   minus 2 (for channels 0 or 1,
-					   channel minus 2 is channel 1 or 2 on
-					   the preceding counter subdevice, for
-					   the first counter subdevice the
-					   preceding counter subdevice is the
-					   last counter subdevice) */
-		AMPLC_DIO_GAT_RESERVED4,
-		AMPLC_DIO_GAT_RESERVED5,
-		AMPLC_DIO_GAT_RESERVED6,
-		AMPLC_DIO_GAT_RESERVED7
-	};
+enum amplc_dio_gate_source {
+	AMPLC_DIO_GAT_VCC,	/* internal high logic level */
+	AMPLC_DIO_GAT_GND,	/* internal low logic level */
+	AMPLC_DIO_GAT_GATN,	/* per channel external gate input */
+	AMPLC_DIO_GAT_NOUTNM2,	/* negated output of counter channel
+				   minus 2 (for channels 0 or 1,
+				   channel minus 2 is channel 1 or 2 on
+				   the preceding counter subdevice, for
+				   the first counter subdevice the
+				   preceding counter subdevice is the
+				   last counter subdevice) */
+	AMPLC_DIO_GAT_RESERVED4,
+	AMPLC_DIO_GAT_RESERVED5,
+	AMPLC_DIO_GAT_RESERVED6,
+	AMPLC_DIO_GAT_RESERVED7
+};
 
 #endif /* _COMEDI_H */
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index e82126407e95..c2a32cf95a82 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -370,7 +370,8 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 			return -ENOMEM;
 
 		if (copy_from_user(aux_data,
-				   comedi_aux_data(it.options, 0), aux_len)) {
+				   (unsigned char __user *
+				    )comedi_aux_data(it.options, 0), aux_len)) {
 			vfree(aux_data);
 			return -EFAULT;
 		}
@@ -426,7 +427,7 @@ static int do_bufconfig_ioctl(struct comedi_device *dev,
 	if (bc.subdevice >= dev->n_subdevices || bc.subdevice < 0)
 		return -EINVAL;
 
-	s = dev->subdevices + bc.subdevice;
+	s = &dev->subdevices[bc.subdevice];
 	async = s->async;
 
 	if (!async) {
@@ -539,7 +540,7 @@ static int do_subdinfo_ioctl(struct comedi_device *dev,
 
 	/* fill subdinfo structs */
 	for (i = 0; i < dev->n_subdevices; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		us = tmp + i;
 
 		us->type = s->type;
@@ -617,7 +618,7 @@ static int do_chaninfo_ioctl(struct comedi_device *dev,
 
 	if (it.subdev >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + it.subdev;
+	s = &dev->subdevices[it.subdev];
 
 	if (it.maxdata_list) {
 		if (s->maxdata || !s->maxdata_list)
@@ -685,7 +686,7 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 	if (bi.subdevice >= dev->n_subdevices || bi.subdevice < 0)
 		return -EINVAL;
 
-	s = dev->subdevices + bi.subdevice;
+	s = &dev->subdevices[bi.subdevice];
 
 	if (s->lock && s->lock != file)
 		return -EACCES;
@@ -882,14 +883,12 @@ static int check_insn_config_length(struct comedi_insn *insn,
 		/* by default we allow the insn since we don't have checks for
 		 * all possible cases yet */
 	default:
-		printk(KERN_WARNING
-		       "comedi: no check for data length of config insn id "
-		       "%i is implemented.\n"
-		       " Add a check to %s in %s.\n"
-		       " Assuming n=%i is correct.\n", data[0], __func__,
-		       __FILE__, insn->n);
+		pr_warn("comedi: No check for data length of config insn id %i is implemented.\n",
+			data[0]);
+		pr_warn("comedi: Add a check to %s in %s.\n",
+			__func__, __FILE__);
+		pr_warn("comedi: Assuming n=%i is correct.\n", insn->n);
 		return 0;
-		break;
 	}
 	return -EINVAL;
 }
@@ -940,7 +939,7 @@ static int parse_insn(struct comedi_device *dev, struct comedi_insn *insn,
 				ret = -EINVAL;
 				break;
 			}
-			s = dev->subdevices + insn->subdev;
+			s = &dev->subdevices[insn->subdev];
 			if (!s->async) {
 				DPRINTK("no async\n");
 				ret = -EINVAL;
@@ -951,7 +950,7 @@ static int parse_insn(struct comedi_device *dev, struct comedi_insn *insn,
 				ret = -EAGAIN;
 				break;
 			}
-			ret = s->async->inttrig(dev, s, insn->data[0]);
+			ret = s->async->inttrig(dev, s, data[0]);
 			if (ret >= 0)
 				ret = 1;
 			break;
@@ -969,7 +968,7 @@ static int parse_insn(struct comedi_device *dev, struct comedi_insn *insn,
 			ret = -EINVAL;
 			goto out;
 		}
-		s = dev->subdevices + insn->subdev;
+		s = &dev->subdevices[insn->subdev];
 
 		if (s->type == COMEDI_SUBD_UNUSED) {
 			DPRINTK("%d not usable subdevice\n", insn->subdev);
@@ -1133,37 +1132,37 @@ static void comedi_set_subdevice_runflags(struct comedi_subdevice *s,
 }
 
 static int do_cmd_ioctl(struct comedi_device *dev,
-			struct comedi_cmd __user *cmd, void *file)
+			struct comedi_cmd __user *arg, void *file)
 {
-	struct comedi_cmd user_cmd;
+	struct comedi_cmd cmd;
 	struct comedi_subdevice *s;
 	struct comedi_async *async;
 	int ret = 0;
-	unsigned int __user *chanlist_saver = NULL;
+	unsigned int __user *user_chanlist;
 
-	if (copy_from_user(&user_cmd, cmd, sizeof(struct comedi_cmd))) {
+	if (copy_from_user(&cmd, arg, sizeof(struct comedi_cmd))) {
 		DPRINTK("bad cmd address\n");
 		return -EFAULT;
 	}
 	/* save user's chanlist pointer so it can be restored later */
-	chanlist_saver = user_cmd.chanlist;
+	user_chanlist = (unsigned int __user *)cmd.chanlist;
 
-	if (user_cmd.subdev >= dev->n_subdevices) {
-		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+	if (cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", cmd.subdev);
 		return -ENODEV;
 	}
 
-	s = dev->subdevices + user_cmd.subdev;
+	s = &dev->subdevices[cmd.subdev];
 	async = s->async;
 
 	if (s->type == COMEDI_SUBD_UNUSED) {
-		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		DPRINTK("%d not valid subdevice\n", cmd.subdev);
 		return -EIO;
 	}
 
 	if (!s->do_cmd || !s->do_cmdtest || !s->async) {
 		DPRINTK("subdevice %i does not support commands\n",
-			user_cmd.subdev);
+			cmd.subdev);
 		return -EIO;
 	}
 
@@ -1181,23 +1180,22 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 	s->busy = file;
 
 	/* make sure channel/gain list isn't too long */
-	if (user_cmd.chanlist_len > s->len_chanlist) {
+	if (cmd.chanlist_len > s->len_chanlist) {
 		DPRINTK("channel/gain list too long %u > %d\n",
-			user_cmd.chanlist_len, s->len_chanlist);
+			cmd.chanlist_len, s->len_chanlist);
 		ret = -EINVAL;
 		goto cleanup;
 	}
 
 	/* make sure channel/gain list isn't too short */
-	if (user_cmd.chanlist_len < 1) {
+	if (cmd.chanlist_len < 1) {
 		DPRINTK("channel/gain list too short %u < 1\n",
-			user_cmd.chanlist_len);
+			cmd.chanlist_len);
 		ret = -EINVAL;
 		goto cleanup;
 	}
 
-	kfree(async->cmd.chanlist);
-	async->cmd = user_cmd;
+	async->cmd = cmd;
 	async->cmd.data = NULL;
 	/* load channel/gain list */
 	async->cmd.chanlist =
@@ -1208,7 +1206,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 		goto cleanup;
 	}
 
-	if (copy_from_user(async->cmd.chanlist, user_cmd.chanlist,
+	if (copy_from_user(async->cmd.chanlist, user_chanlist,
 			   async->cmd.chanlist_len * sizeof(int))) {
 		DPRINTK("fault reading chanlist\n");
 		ret = -EFAULT;
@@ -1228,11 +1226,11 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 
 	if (async->cmd.flags & TRIG_BOGUS || ret) {
 		DPRINTK("test returned %d\n", ret);
-		user_cmd = async->cmd;
+		cmd = async->cmd;
 		/* restore chanlist pointer before copying back */
-		user_cmd.chanlist = chanlist_saver;
-		user_cmd.data = NULL;
-		if (copy_to_user(cmd, &user_cmd, sizeof(struct comedi_cmd))) {
+		cmd.chanlist = (unsigned int __force *)user_chanlist;
+		cmd.data = NULL;
+		if (copy_to_user(arg, &cmd, sizeof(struct comedi_cmd))) {
 			DPRINTK("fault writing cmd\n");
 			ret = -EFAULT;
 			goto cleanup;
@@ -1285,77 +1283,77 @@ cleanup:
 static int do_cmdtest_ioctl(struct comedi_device *dev,
 			    struct comedi_cmd __user *arg, void *file)
 {
-	struct comedi_cmd user_cmd;
+	struct comedi_cmd cmd;
 	struct comedi_subdevice *s;
 	int ret = 0;
 	unsigned int *chanlist = NULL;
-	unsigned int __user *chanlist_saver = NULL;
+	unsigned int __user *user_chanlist;
 
-	if (copy_from_user(&user_cmd, arg, sizeof(struct comedi_cmd))) {
+	if (copy_from_user(&cmd, arg, sizeof(struct comedi_cmd))) {
 		DPRINTK("bad cmd address\n");
 		return -EFAULT;
 	}
 	/* save user's chanlist pointer so it can be restored later */
-	chanlist_saver = user_cmd.chanlist;
+	user_chanlist = (unsigned int __user *)cmd.chanlist;
 
-	if (user_cmd.subdev >= dev->n_subdevices) {
-		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+	if (cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", cmd.subdev);
 		return -ENODEV;
 	}
 
-	s = dev->subdevices + user_cmd.subdev;
+	s = &dev->subdevices[cmd.subdev];
 	if (s->type == COMEDI_SUBD_UNUSED) {
-		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		DPRINTK("%d not valid subdevice\n", cmd.subdev);
 		return -EIO;
 	}
 
 	if (!s->do_cmd || !s->do_cmdtest) {
 		DPRINTK("subdevice %i does not support commands\n",
-			user_cmd.subdev);
+			cmd.subdev);
 		return -EIO;
 	}
 
 	/* make sure channel/gain list isn't too long */
-	if (user_cmd.chanlist_len > s->len_chanlist) {
+	if (cmd.chanlist_len > s->len_chanlist) {
 		DPRINTK("channel/gain list too long %d > %d\n",
-			user_cmd.chanlist_len, s->len_chanlist);
+			cmd.chanlist_len, s->len_chanlist);
 		ret = -EINVAL;
 		goto cleanup;
 	}
 
 	/* load channel/gain list */
-	if (user_cmd.chanlist) {
+	if (cmd.chanlist) {
 		chanlist =
-		    kmalloc(user_cmd.chanlist_len * sizeof(int), GFP_KERNEL);
+		    kmalloc(cmd.chanlist_len * sizeof(int), GFP_KERNEL);
 		if (!chanlist) {
 			DPRINTK("allocation failed\n");
 			ret = -ENOMEM;
 			goto cleanup;
 		}
 
-		if (copy_from_user(chanlist, user_cmd.chanlist,
-				   user_cmd.chanlist_len * sizeof(int))) {
+		if (copy_from_user(chanlist, user_chanlist,
+				   cmd.chanlist_len * sizeof(int))) {
 			DPRINTK("fault reading chanlist\n");
 			ret = -EFAULT;
 			goto cleanup;
 		}
 
 		/* make sure each element in channel/gain list is valid */
-		ret = comedi_check_chanlist(s, user_cmd.chanlist_len, chanlist);
+		ret = comedi_check_chanlist(s, cmd.chanlist_len, chanlist);
 		if (ret < 0) {
 			DPRINTK("bad chanlist\n");
 			goto cleanup;
 		}
 
-		user_cmd.chanlist = chanlist;
+		cmd.chanlist = chanlist;
 	}
 
-	ret = s->do_cmdtest(dev, s, &user_cmd);
+	ret = s->do_cmdtest(dev, s, &cmd);
 
 	/* restore chanlist pointer before copying back */
-	user_cmd.chanlist = chanlist_saver;
+	cmd.chanlist = (unsigned int __force *)user_chanlist;
 
-	if (copy_to_user(arg, &user_cmd, sizeof(struct comedi_cmd))) {
+	if (copy_to_user(arg, &cmd, sizeof(struct comedi_cmd))) {
 		DPRINTK("bad cmd address\n");
 		ret = -EFAULT;
 		goto cleanup;
@@ -1390,7 +1388,7 @@ static int do_lock_ioctl(struct comedi_device *dev, unsigned int arg,
 
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + arg;
+	s = &dev->subdevices[arg];
 
 	spin_lock_irqsave(&s->spin_lock, flags);
 	if (s->busy || s->lock)
@@ -1433,7 +1431,7 @@ static int do_unlock_ioctl(struct comedi_device *dev, unsigned int arg,
 
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + arg;
+	s = &dev->subdevices[arg];
 
 	if (s->busy)
 		return -EBUSY;
@@ -1474,7 +1472,7 @@ static int do_cancel_ioctl(struct comedi_device *dev, unsigned int arg,
 
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + arg;
+	s = &dev->subdevices[arg];
 	if (s->async == NULL)
 		return -EINVAL;
 
@@ -1511,7 +1509,7 @@ static int do_poll_ioctl(struct comedi_device *dev, unsigned int arg,
 
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + arg;
+	s = &dev->subdevices[arg];
 
 	if (s->lock && s->lock != file)
 		return -EACCES;
@@ -2025,7 +2023,8 @@ done:
 /*
    This function restores a subdevice to an idle state.
  */
-void do_become_nonbusy(struct comedi_device *dev, struct comedi_subdevice *s)
+static void do_become_nonbusy(struct comedi_device *dev,
+			      struct comedi_subdevice *s)
 {
 	struct comedi_async *async = s->async;
 
@@ -2033,9 +2032,11 @@ void do_become_nonbusy(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (async) {
 		comedi_reset_async_buf(async);
 		async->inttrig = NULL;
+		kfree(async->cmd.chanlist);
+		async->cmd.chanlist = NULL;
 	} else {
-		printk(KERN_ERR
-		       "BUG: (?) do_become_nonbusy called with async=0\n");
+		dev_err(dev->class_dev,
+			"BUG: (?) do_become_nonbusy called with async=NULL\n");
 	}
 
 	s->busy = NULL;
@@ -2140,7 +2141,7 @@ static int comedi_close(struct inode *inode, struct file *file)
 
 	if (dev->subdevices) {
 		for (i = 0; i < dev->n_subdevices; i++) {
-			s = dev->subdevices + i;
+			s = &dev->subdevices[i];
 
 			if (s->busy == file)
 				do_cancel(dev, s);
@@ -2211,14 +2212,12 @@ static int __init comedi_init(void)
 	int i;
 	int retval;
 
-	printk(KERN_INFO "comedi: version " COMEDI_RELEASE
-	       " - http://www.comedi.org\n");
+	pr_info("comedi: version " COMEDI_RELEASE " - http://www.comedi.org\n");
 
 	if (comedi_num_legacy_minors < 0 ||
 	    comedi_num_legacy_minors > COMEDI_NUM_BOARD_MINORS) {
-		printk(KERN_ERR "comedi: error: invalid value for module "
-		       "parameter \"comedi_num_legacy_minors\".  Valid values "
-		       "are 0 through %i.\n", COMEDI_NUM_BOARD_MINORS);
+		pr_err("comedi: error: invalid value for module parameter \"comedi_num_legacy_minors\".  Valid values are 0 through %i.\n",
+		       COMEDI_NUM_BOARD_MINORS);
 		return -EINVAL;
 	}
 
@@ -2247,7 +2246,7 @@ static int __init comedi_init(void)
 	}
 	comedi_class = class_create(THIS_MODULE, "comedi");
 	if (IS_ERR(comedi_class)) {
-		printk(KERN_ERR "comedi: failed to create class");
+		pr_err("comedi: failed to create class\n");
 		cdev_del(&comedi_cdev);
 		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
 					 COMEDI_NUM_MINORS);
@@ -2295,8 +2294,7 @@ module_exit(comedi_cleanup);
 
 void comedi_error(const struct comedi_device *dev, const char *s)
 {
-	printk(KERN_ERR "comedi%d: %s: %s\n", dev->minor,
-	       dev->driver->driver_name, s);
+	dev_err(dev->class_dev, "%s: %s\n", dev->driver->driver_name, s);
 }
 EXPORT_SYMBOL(comedi_error);
 
@@ -2364,7 +2362,7 @@ static int is_device_busy(struct comedi_device *dev)
 		return 0;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		if (s->busy)
 			return 1;
 		if (s->async && s->async->mmap_count)
@@ -2420,9 +2418,7 @@ int comedi_alloc_board_minor(struct device *hardware_device)
 		comedi_device_cleanup(info->device);
 		kfree(info->device);
 		kfree(info);
-		printk(KERN_ERR
-		       "comedi: error: "
-		       "ran out of minor numbers for board device files.\n");
+		pr_err("comedi: error: ran out of minor numbers for board device files.\n");
 		return -EBUSY;
 	}
 	info->device->minor = i;
@@ -2499,9 +2495,7 @@ int comedi_alloc_subdevice_minor(struct comedi_device *dev,
 	spin_unlock(&comedi_file_info_table_lock);
 	if (i == COMEDI_NUM_MINORS) {
 		kfree(info);
-		printk(KERN_ERR
-		       "comedi: error: "
-		       "ran out of minor numbers for board device files.\n");
+		pr_err("comedi: error: ran out of minor numbers for board device files.\n");
 		return -EBUSY;
 	}
 	s->minor = i;
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index f713783ef624..cb67a5cb9c82 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -46,7 +46,7 @@
 
 #define DPRINTK(format, args...)	do {		\
 	if (comedi_debug)				\
-		printk(KERN_DEBUG "comedi: " format , ## args);	\
+		pr_debug("comedi: " format, ## args);	\
 } while (0)
 
 #define COMEDI_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c))
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index 2359151af7e1..1db6bfdbf13b 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -71,7 +71,7 @@ int comedi_alloc_subdevices(struct comedi_device *dev, int num_subdevices)
 	dev->n_subdevices = num_subdevices;
 
 	for (i = 0; i < num_subdevices; ++i) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		s->device = dev;
 		s->async_dma_dir = DMA_NONE;
 		spin_lock_init(&s->spin_lock);
@@ -88,7 +88,7 @@ static void cleanup_device(struct comedi_device *dev)
 
 	if (dev->subdevices) {
 		for (i = 0; i < dev->n_subdevices; i++) {
-			s = dev->subdevices + i;
+			s = &dev->subdevices[i];
 			comedi_free_subdevice_minor(s);
 			if (s->async) {
 				comedi_buf_alloc(dev, s, 0);
@@ -119,8 +119,8 @@ static void __comedi_device_detach(struct comedi_device *dev)
 	if (dev->driver)
 		dev->driver->detach(dev);
 	else
-		printk(KERN_WARNING
-		       "BUG: dev->driver=NULL in comedi_device_detach()\n");
+		dev_warn(dev->class_dev,
+			 "BUG: dev->driver=NULL in comedi_device_detach()\n");
 	cleanup_device(dev);
 }
 
@@ -142,8 +142,7 @@ static int comedi_device_postconfig(struct comedi_device *dev)
 		return ret;
 	}
 	if (!dev->board_name) {
-		printk(KERN_WARNING "BUG: dev->board_name=<%p>\n",
-		       dev->board_name);
+		dev_warn(dev->class_dev, "BUG: dev->board_name=NULL\n");
 		dev->board_name = "BUG";
 	}
 	smp_wmb();
@@ -160,10 +159,8 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -EBUSY;
 
 	for (driv = comedi_drivers; driv; driv = driv->next) {
-		if (!try_module_get(driv->module)) {
-			printk(KERN_INFO "comedi: failed to increment module count, skipping\n");
+		if (!try_module_get(driv->module))
 			continue;
-		}
 		if (driv->num_names) {
 			dev->board_ptr = comedi_recognize(driv, it->board_name);
 			if (dev->board_ptr)
@@ -176,16 +173,21 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		/*  recognize has failed if we get here */
 		/*  report valid board names before returning error */
 		for (driv = comedi_drivers; driv; driv = driv->next) {
-			if (!try_module_get(driv->module)) {
-				printk(KERN_INFO
-				       "comedi: failed to increment module count\n");
+			if (!try_module_get(driv->module))
 				continue;
-			}
 			comedi_report_boards(driv);
 			module_put(driv->module);
 		}
 		return -EIO;
 	}
+	if (driv->attach == NULL) {
+		/* driver does not support manual configuration */
+		dev_warn(dev->class_dev,
+			 "driver '%s' does not support attach using comedi_config\n",
+			 driv->driver_name);
+		module_put(driv->module);
+		return -ENOSYS;
+	}
 	/* initialize dev->driver here so
 	 * comedi_error() can be called from attach */
 	dev->driver = driv;
@@ -225,8 +227,9 @@ int comedi_driver_unregister(struct comedi_driver *driver)
 		mutex_lock(&dev->mutex);
 		if (dev->attached && dev->driver == driver) {
 			if (dev->use_count)
-				printk(KERN_WARNING "BUG! detaching device with use_count=%d\n",
-						dev->use_count);
+				dev_warn(dev->class_dev,
+					 "BUG! detaching device with use_count=%d\n",
+					 dev->use_count);
 			comedi_device_detach(dev);
 		}
 		mutex_unlock(&dev->mutex);
@@ -255,7 +258,7 @@ static int postconfig(struct comedi_device *dev)
 	int ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 
 		if (s->type == COMEDI_SUBD_UNUSED)
 			continue;
@@ -273,8 +276,8 @@ static int postconfig(struct comedi_device *dev)
 			async =
 			    kzalloc(sizeof(struct comedi_async), GFP_KERNEL);
 			if (async == NULL) {
-				printk(KERN_INFO
-				       "failed to allocate async struct\n");
+				dev_warn(dev->class_dev,
+					 "failed to allocate async struct\n");
 				return -ENOMEM;
 			}
 			init_waitqueue_head(&async->wait_head);
@@ -290,7 +293,8 @@ static int postconfig(struct comedi_device *dev)
 			async->prealloc_buf = NULL;
 			async->prealloc_bufsz = 0;
 			if (comedi_buf_alloc(dev, s, buf_size) < 0) {
-				printk(KERN_INFO "Buffer allocation failed\n");
+				dev_warn(dev->class_dev,
+					 "Buffer allocation failed\n");
 				return -ENOMEM;
 			}
 			if (s->buf_change) {
@@ -370,17 +374,17 @@ static void comedi_report_boards(struct comedi_driver *driv)
 	unsigned int i;
 	const char *const *name_ptr;
 
-	printk(KERN_INFO "comedi: valid board names for %s driver are:\n",
-	       driv->driver_name);
+	pr_info("comedi: valid board names for %s driver are:\n",
+		driv->driver_name);
 
 	name_ptr = driv->board_name;
 	for (i = 0; i < driv->num_names; i++) {
-		printk(KERN_INFO " %s\n", *name_ptr);
+		pr_info(" %s\n", *name_ptr);
 		name_ptr = (const char **)((char *)name_ptr + driv->offset);
 	}
 
 	if (driv->num_names == 0)
-		printk(KERN_INFO " %s\n", driv->driver_name);
+		pr_info(" %s\n", driv->driver_name);
 }
 
 static int poll_invalid(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -411,7 +415,6 @@ static int insn_rw_emulate_bits(struct comedi_device *dev,
 	new_insn.insn = INSN_BITS;
 	new_insn.chanspec = base_bitfield_channel;
 	new_insn.n = 2;
-	new_insn.data = new_data;
 	new_insn.subdev = insn->subdev;
 
 	if (insn->insn == INSN_WRITE) {
@@ -584,9 +587,9 @@ static unsigned int comedi_buf_munge(struct comedi_async *async,
 
 		block_size = num_bytes - count;
 		if (block_size < 0) {
-			printk(KERN_WARNING
-			       "%s: %s: bug! block_size is negative\n",
-			       __FILE__, __func__);
+			dev_warn(s->device->class_dev,
+				 "%s: %s: bug! block_size is negative\n",
+				 __FILE__, __func__);
 			break;
 		}
 		if ((int)(async->munge_ptr + block_size -
@@ -667,7 +670,8 @@ unsigned comedi_buf_write_free(struct comedi_async *async, unsigned int nbytes)
 {
 	if ((int)(async->buf_write_count + nbytes -
 		  async->buf_write_alloc_count) > 0) {
-		printk(KERN_INFO "comedi: attempted to write-free more bytes than have been write-allocated.\n");
+		dev_info(async->subdevice->device->class_dev,
+			 "attempted to write-free more bytes than have been write-allocated.\n");
 		nbytes = async->buf_write_alloc_count - async->buf_write_count;
 	}
 	async->buf_write_count += nbytes;
@@ -703,8 +707,8 @@ unsigned comedi_buf_read_free(struct comedi_async *async, unsigned int nbytes)
 	smp_mb();
 	if ((int)(async->buf_read_count + nbytes -
 		  async->buf_read_alloc_count) > 0) {
-		printk(KERN_INFO
-		       "comedi: attempted to read-free more bytes than have been read-allocated.\n");
+		dev_info(async->subdevice->device->class_dev,
+			 "attempted to read-free more bytes than have been read-allocated.\n");
 		nbytes = async->buf_read_alloc_count - async->buf_read_count;
 	}
 	async->buf_read_count += nbytes;
@@ -853,10 +857,9 @@ comedi_auto_config_helper(struct device *hardware_device,
 	mutex_lock(&comedi_dev->mutex);
 	if (comedi_dev->attached)
 		ret = -EBUSY;
-	else if (!try_module_get(driver->module)) {
-		printk(KERN_INFO "comedi: failed to increment module count\n");
+	else if (!try_module_get(driver->module))
 		ret = -EIO;
-	} else {
+	else {
 		/* set comedi_dev->driver here for attach wrapper */
 		comedi_dev->driver = driver;
 		ret = (*attach_wrapper)(comedi_dev, context);
@@ -884,14 +887,19 @@ static int comedi_auto_config_wrapper(struct comedi_device *dev, void *context)
 		 * has already been copied to it->board_name */
 		dev->board_ptr = comedi_recognize(driv, it->board_name);
 		if (dev->board_ptr == NULL) {
-			printk(KERN_WARNING
-			       "comedi: auto config failed to find board entry"
-			       " '%s' for driver '%s'\n", it->board_name,
-			       driv->driver_name);
+			dev_warn(dev->class_dev,
+				 "auto config failed to find board entry '%s' for driver '%s'\n",
+				 it->board_name, driv->driver_name);
 			comedi_report_boards(driv);
 			return -EINVAL;
 		}
 	}
+	if (!driv->attach) {
+		dev_warn(dev->class_dev,
+			 "BUG! driver '%s' using old-style auto config but has no attach handler\n",
+			 driv->driver_name);
+		return -EINVAL;
+	}
 	return driv->attach(dev, it);
 }
 
diff --git a/drivers/staging/comedi/drivers/8253.h b/drivers/staging/comedi/drivers/8253.h
index 3eb45d46e05b..429e0d60c0a3 100644
--- a/drivers/staging/comedi/drivers/8253.h
+++ b/drivers/staging/comedi/drivers/8253.h
@@ -262,8 +262,10 @@ static inline int i8254_load(unsigned long base_address, unsigned int regshift,
 	return 0;
 }
 
-static inline int i8254_mm_load(void *base_address, unsigned int regshift,
-				unsigned int counter_number, unsigned int count,
+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;
@@ -311,7 +313,8 @@ static inline int i8254_read(unsigned long base_address, unsigned int regshift,
 	return ret;
 }
 
-static inline int i8254_mm_read(void *base_address, unsigned int regshift,
+static inline int i8254_mm_read(void __iomem *base_address,
+				unsigned int regshift,
 				unsigned int counter_number)
 {
 	unsigned int byte;
@@ -348,7 +351,7 @@ static inline void i8254_write(unsigned long base_address,
 	outb(byte, base_address + (counter_number << regshift));
 }
 
-static inline void i8254_mm_write(void *base_address,
+static inline void i8254_mm_write(void __iomem *base_address,
 				  unsigned int regshift,
 				  unsigned int counter_number,
 				  unsigned int count)
@@ -390,7 +393,7 @@ static inline int i8254_set_mode(unsigned long base_address,
 	return 0;
 }
 
-static inline int i8254_mm_set_mode(void *base_address,
+static inline int i8254_mm_set_mode(void __iomem *base_address,
 				    unsigned int regshift,
 				    unsigned int counter_number,
 				    unsigned int mode)
@@ -419,7 +422,7 @@ static inline int i8254_status(unsigned long base_address,
 	return inb(base_address + (counter_number << regshift));
 }
 
-static inline int i8254_mm_status(void *base_address,
+static inline int i8254_mm_status(void __iomem *base_address,
 				  unsigned int regshift,
 				  unsigned int counter_number)
 {
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 4c9977b8a5ae..a256622e2dd7 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -82,6 +82,8 @@ I/O port base address can be found in the output of 'lspci -v'.
 
 #include <linux/ioport.h>
 #include <linux/slab.h>
+
+#include "comedi_fc.h"
 #include "8255.h"
 
 #define _8255_SIZE	4
@@ -229,39 +231,20 @@ static int subdev_8255_cmdtest(struct comedi_device *dev,
 			       struct comedi_cmd *cmd)
 {
 	int err = 0;
-	unsigned int tmp;
-
-	/* step 1 */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2 */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -403,7 +386,7 @@ static int dev_8255_attach(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		iobase = it->options[i];
 
 		if (!request_region(iobase, _8255_SIZE, "8255")) {
@@ -429,7 +412,7 @@ static void dev_8255_detach(struct comedi_device *dev)
 	int i;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		if (s->type != COMEDI_SUBD_UNUSED) {
 			spriv = s->private;
 			release_region(spriv->iobase, _8255_SIZE);
diff --git a/drivers/staging/comedi/drivers/8255_pci.c b/drivers/staging/comedi/drivers/8255_pci.c
new file mode 100644
index 000000000000..7dff3c01dc29
--- /dev/null
+++ b/drivers/staging/comedi/drivers/8255_pci.c
@@ -0,0 +1,353 @@
+/*
+ * COMEDI driver for generic PCI based 8255 digital i/o boards
+ * Copyright (C) 2012 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * Based on the tested adl_pci7296 driver written by:
+ *	Jon Grierson <jd@renko.co.uk>
+ * and the experimental cb_pcidio driver written by:
+ *	Yoshiya Matsuzaka
+ *
+ * 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.
+ *
+ * You 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.
+ */
+
+/*
+Driver: 8255_pci
+Description: Generic PCI based 8255 Digital I/O boards
+Devices: (ADLink) PCI-7224 [adl_pci-7224] - 24 channels
+	 (ADLink) PCI-7248 [adl_pci-7248] - 48 channels
+	 (ADLink) PCI-7296 [adl_pci-7296] - 96 channels
+	 (Measurement Computing) PCI-DIO24 [cb_pci-dio24] - 24 channels
+	 (Measurement Computing) PCI-DIO24H [cb_pci-dio24h] - 24 channels
+	 (Measurement Computing) PCI-DIO48H [cb_pci-dio48h] - 48 channels
+	 (Measurement Computing) PCI-DIO96H [cb_pci-dio96h] - 96 channels
+	 (National Instruments) PCI-DIO-96 [ni_pci-dio-96] - 96 channels
+	 (National Instruments) PCI-DIO-96B [ni_pci-dio-96b] - 96 channels
+	 (National Instruments) PXI-6508 [ni_pxi-6508] - 96 channels
+	 (National Instruments) PCI-6503 [ni_pci-6503] - 24 channels
+	 (National Instruments) PCI-6503B [ni_pci-6503b] - 24 channels
+	 (National Instruments) PCI-6503X [ni_pci-6503x] - 24 channels
+	 (National Instruments) PXI-6503 [ni_pxi-6503] - 24 channels
+Author: H Hartley Sweeten <hsweeten@visionengravers.com>
+Updated: Wed, 12 Sep 2012 11:52:01 -0700
+Status: untested
+
+Some of these boards also have an 8254 programmable timer/counter
+chip. This chip is not currently supported by this driver.
+
+Interrupt support for these boards is also not currently supported.
+
+Configuration Options: not applicable, uses PCI auto config
+*/
+
+#include "../comedidev.h"
+
+#include "8255.h"
+
+/*
+ * PCI Device ID's supported by this driver
+ */
+#define PCI_DEVICE_ID_ADLINK_PCI7224	0x7224
+#define PCI_DEVICE_ID_ADLINK_PCI7248	0x7248
+#define PCI_DEVICE_ID_ADLINK_PCI7296	0x7296
+
+/* ComputerBoards is now known as Measurement Computing */
+#define PCI_VENDOR_ID_CB		0x1307
+
+#define PCI_DEVICE_ID_CB_PCIDIO48H	0x000b
+#define PCI_DEVICE_ID_CB_PCIDIO24H	0x0014
+#define PCI_DEVICE_ID_CB_PCIDIO96H	0x0017
+#define PCI_DEVICE_ID_CB_PCIDIO24	0x0028
+
+#define PCI_DEVICE_ID_NI_PCIDIO96	0x0160
+#define PCI_DEVICE_ID_NI_PCI6503	0x0400
+#define PCI_DEVICE_ID_NI_PCI6503B	0x1250
+#define PCI_DEVICE_ID_NI_PXI6508	0x13c0
+#define PCI_DEVICE_ID_NI_PCIDIO96B	0x1630
+#define PCI_DEVICE_ID_NI_PCI6503X	0x17d0
+#define PCI_DEVICE_ID_NI_PXI_6503	0x1800
+
+struct pci_8255_boardinfo {
+	const char *name;
+	unsigned short vendor;
+	unsigned short device;
+	int dio_badr;
+	int is_mmio;
+	int n_8255;
+};
+
+static const struct pci_8255_boardinfo pci_8255_boards[] = {
+	{
+		.name		= "adl_pci-7224",
+		.vendor		= PCI_VENDOR_ID_ADLINK,
+		.device		= PCI_DEVICE_ID_ADLINK_PCI7224,
+		.dio_badr	= 2,
+		.n_8255		= 1,
+	}, {
+		.name		= "adl_pci-7248",
+		.vendor		= PCI_VENDOR_ID_ADLINK,
+		.device		= PCI_DEVICE_ID_ADLINK_PCI7248,
+		.dio_badr	= 2,
+		.n_8255		= 2,
+	}, {
+		.name		= "adl_pci-7296",
+		.vendor		= PCI_VENDOR_ID_ADLINK,
+		.device		= PCI_DEVICE_ID_ADLINK_PCI7296,
+		.dio_badr	= 2,
+		.n_8255		= 4,
+	}, {
+		.name		= "cb_pci-dio24",
+		.vendor		= PCI_VENDOR_ID_CB,
+		.device		= PCI_DEVICE_ID_CB_PCIDIO24,
+		.dio_badr	= 2,
+		.n_8255		= 1,
+	}, {
+		.name		= "cb_pci-dio24h",
+		.vendor		= PCI_VENDOR_ID_CB,
+		.device		= PCI_DEVICE_ID_CB_PCIDIO24H,
+		.dio_badr	= 2,
+		.n_8255		= 1,
+	}, {
+		.name		= "cb_pci-dio48h",
+		.vendor		= PCI_VENDOR_ID_CB,
+		.device		= PCI_DEVICE_ID_CB_PCIDIO48H,
+		.dio_badr	= 1,
+		.n_8255		= 2,
+	}, {
+		.name		= "cb_pci-dio96h",
+		.vendor		= PCI_VENDOR_ID_CB,
+		.device		= PCI_DEVICE_ID_CB_PCIDIO96H,
+		.dio_badr	= 2,
+		.n_8255		= 4,
+	}, {
+		.name		= "ni_pci-dio-96",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PCIDIO96,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 4,
+	}, {
+		.name		= "ni_pci-dio-96b",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PCIDIO96B,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 4,
+	}, {
+		.name		= "ni_pxi-6508",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PXI6508,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 4,
+	}, {
+		.name		= "ni_pci-6503",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PCI6503,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 1,
+	}, {
+		.name		= "ni_pci-6503b",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PCI6503B,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 1,
+	}, {
+		.name		= "ni_pci-6503x",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PCI6503X,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 1,
+	}, {
+		.name		= "ni_pxi-6503",
+		.vendor		= PCI_VENDOR_ID_NI,
+		.device		= PCI_DEVICE_ID_NI_PXI_6503,
+		.dio_badr	= 1,
+		.is_mmio	= 1,
+		.n_8255		= 1,
+	},
+};
+
+struct pci_8255_private {
+	void __iomem *mmio_base;
+};
+
+static int pci_8255_mmio(int dir, int port, int data, unsigned long iobase)
+{
+	void __iomem *mmio_base = (void __iomem *)iobase;
+
+	if (dir) {
+		writeb(data, mmio_base + port);
+		return 0;
+	} else {
+		return readb(mmio_base  + port);
+	}
+}
+
+static const void *pci_8255_find_boardinfo(struct comedi_device *dev,
+					      struct pci_dev *pcidev)
+{
+	const struct pci_8255_boardinfo *board;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pci_8255_boards); i++) {
+		board = &pci_8255_boards[i];
+		if (pcidev->vendor == board->vendor &&
+		    pcidev->device == board->device)
+			return board;
+	}
+	return NULL;
+}
+
+static int pci_8255_attach_pci(struct comedi_device *dev,
+			       struct pci_dev *pcidev)
+{
+	const struct pci_8255_boardinfo *board;
+	struct pci_8255_private *devpriv;
+	struct comedi_subdevice *s;
+	resource_size_t iobase;
+	unsigned long len;
+	int ret;
+	int i;
+
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	board = pci_8255_find_boardinfo(dev, pcidev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	iobase = pci_resource_start(pcidev, board->dio_badr);
+	len = pci_resource_len(pcidev, board->dio_badr);
+
+	if (board->is_mmio) {
+		devpriv->mmio_base = ioremap(iobase, len);
+		if (!devpriv->mmio_base)
+			return -ENOMEM;
+	}
+	dev->iobase = iobase;
+
+	/*
+	 * One, two, or four subdevices are setup by this driver depending
+	 * on the number of channels provided by the board. Each subdevice
+	 * has 24 channels supported by the 8255 module.
+	 */
+	ret = comedi_alloc_subdevices(dev, board->n_8255);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < board->n_8255; i++) {
+		s = &dev->subdevices[i];
+		if (board->is_mmio) {
+			iobase = (unsigned long)(devpriv->mmio_base + (i * 4));
+			ret = subdev_8255_init(dev, s, pci_8255_mmio, iobase);
+		} else {
+			iobase = dev->iobase + (i * 4);
+			ret = subdev_8255_init(dev, s, NULL, iobase);
+		}
+		if (ret)
+			return ret;
+	}
+
+	dev_info(dev->class_dev, "%s attached (%d digital i/o channels)\n",
+		dev->board_name, board->n_8255 * 24);
+
+	return 0;
+}
+
+static void pci_8255_detach(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct pci_8255_boardinfo *board = comedi_board(dev);
+	struct pci_8255_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	int i;
+
+	if (dev->subdevices) {
+		for (i = 0; i < board->n_8255; i++) {
+			s = &dev->subdevices[i];
+			subdev_8255_cleanup(dev, s);
+		}
+	}
+	if (pcidev) {
+		if (devpriv->mmio_base)
+			iounmap(devpriv->mmio_base);
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+	}
+}
+
+static struct comedi_driver pci_8255_driver = {
+	.driver_name	= "8255_pci",
+	.module		= THIS_MODULE,
+	.attach_pci	= pci_8255_attach_pci,
+	.detach		= pci_8255_detach,
+};
+
+static int __devinit pci_8255_pci_probe(struct pci_dev *dev,
+					const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &pci_8255_driver);
+}
+
+static void __devexit pci_8255_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(pci_8255_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7224) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7248) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_ADLINK_PCI7296) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO24) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO24H) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO48H) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, PCI_DEVICE_ID_CB_PCIDIO96H) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCIDIO96) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCIDIO96B) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PXI6508) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503B) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PCI6503X) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, PCI_DEVICE_ID_NI_PXI_6503) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, pci_8255_pci_table);
+
+static struct pci_driver pci_8255_pci_driver = {
+	.name		= "8255_pci",
+	.id_table	= pci_8255_pci_table,
+	.probe		= pci_8255_pci_probe,
+	.remove		= __devexit_p(pci_8255_pci_remove),
+};
+module_comedi_pci_driver(pci_8255_driver, pci_8255_pci_driver);
+
+MODULE_DESCRIPTION("COMEDI - Generic PCI based 8255 Digital I/O boards");
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 57b19e44d867..a2787c0ca327 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -55,6 +55,7 @@ obj-$(CONFIG_COMEDI_MULTIQ3)		+= multiq3.o
 obj-$(CONFIG_COMEDI_POC)		+= poc.o
 
 # Comedi PCI drivers
+obj-$(CONFIG_COMEDI_8255_PCI)		+= 8255_pci.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_035)	+= addi_apci_035.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1032)	+= addi_apci_1032.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1500)	+= addi_apci_1500.o
@@ -69,9 +70,7 @@ obj-$(CONFIG_COMEDI_ADDI_APCI_3120)	+= addi_apci_3120.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_3501)	+= addi_apci_3501.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_3XXX)	+= addi_apci_3xxx.o
 obj-$(CONFIG_COMEDI_ADL_PCI6208)	+= adl_pci6208.o
-obj-$(CONFIG_COMEDI_ADL_PCI7230)	+= adl_pci7230.o
-obj-$(CONFIG_COMEDI_ADL_PCI7296)	+= adl_pci7296.o
-obj-$(CONFIG_COMEDI_ADL_PCI7432)	+= adl_pci7432.o
+obj-$(CONFIG_COMEDI_ADL_PCI7X3X)	+= adl_pci7x3x.o
 obj-$(CONFIG_COMEDI_ADL_PCI8164)	+= adl_pci8164.o
 obj-$(CONFIG_COMEDI_ADL_PCI9111)	+= adl_pci9111.o
 obj-$(CONFIG_COMEDI_ADL_PCI9118)	+= adl_pci9118.o
@@ -96,7 +95,6 @@ obj-$(CONFIG_COMEDI_KE_COUNTER)		+= ke_counter.o
 obj-$(CONFIG_COMEDI_CB_PCIDAS64)	+= cb_pcidas64.o
 obj-$(CONFIG_COMEDI_CB_PCIDAS)		+= cb_pcidas.o
 obj-$(CONFIG_COMEDI_CB_PCIDDA)		+= cb_pcidda.o
-obj-$(CONFIG_COMEDI_CB_PCIDIO)		+= cb_pcidio.o
 obj-$(CONFIG_COMEDI_CB_PCIMDAS)		+= cb_pcimdas.o
 obj-$(CONFIG_COMEDI_CB_PCIMDDA)		+= cb_pcimdda.o
 obj-$(CONFIG_COMEDI_ME4000)		+= me4000.o
diff --git a/drivers/staging/comedi/drivers/acl7225b.c b/drivers/staging/comedi/drivers/acl7225b.c
index ddba5db1e2e1..28c7fd3a96b6 100644
--- a/drivers/staging/comedi/drivers/acl7225b.c
+++ b/drivers/staging/comedi/drivers/acl7225b.c
@@ -81,7 +81,7 @@ static int acl7225b_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* Relays outputs */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -91,7 +91,7 @@ static int acl7225b_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)ACL7225_RIO_LO;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* Relays status */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -101,7 +101,7 @@ static int acl7225b_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)ACL7225_RIO_LO;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* Isolated digital inputs */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.c b/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.c
deleted file mode 100644
index b973095146f9..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.c
+++ /dev/null
@@ -1,195 +0,0 @@
-/**
-@verbatim
-
-Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
-
-	ADDI-DATA GmbH
-	Dieselstrasse 3
-	D-77833 Ottersweier
-	Tel: +19(0)7223/9493-0
-	Fax: +49(0)7223/9493-92
-	http://www.addi-data.com
-	info@addi-data.com
-
-This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version.
-
-This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-
-You 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
-
-You should also find the complete GPL in the COPYING file accompanying this source code.
-
-@endverbatim
-*/
-/*
-  +-----------------------------------------------------------------------+
-  | (C) ADDI-DATA GmbH          Dieselstraße 3       D-77833 Ottersweier  |
-  +-----------------------------------------------------------------------+
-  | Tel : +49 (0) 7223/9493-0     | email    : info@addi-data.com         |
-  | Fax : +49 (0) 7223/9493-92    | Internet : http://www.addi-data.com   |
-  +-------------------------------+---------------------------------------+
-  | Project : ADDI HEADER READ WRITER |     Compiler   : Visual C++       |
-  | Module name : S5920.cpp           |     Version    : 6.0              |
-  +-------------------------------+---------------------------------------+
-  | Author : E. LIBS                      Date : 02/05/2002               |
-  +-----------------------------------------------------------------------+
-  | Description   : DLL with the S5920 PCI Controller functions           |
-  +-----------------------------------------------------------------------+
-  |                             UPDATE'S                                  |
-  +-----------------------------------------------------------------------+
-  |   Date   |   Author  |          Description of updates                |
-  +----------+-----------+------------------------------------------------+
-  | 28/08/02 | LIBS Eric | Add return codes each time a function of the   |
-  |          |           | Addi Library is called                         |
-  +-----------------------------------------------------------------------+
-  | 31/07/03 | KRAUTH J. | Changes for the MSX-Box                        |
-  +-----------------------------------------------------------------------+
-*/
-
-#include "addi_amcc_S5920.h"
-
-/*+----------------------------------------------------------------------------+*/
-/*| Function   Name   : int i_AddiHeaderRW_ReadEeprom                          |*/
-/*|                               (int    i_NbOfWordsToRead,                   |*/
-/*|                                unsigned int dw_PCIBoardEepromAddress,             |*/
-/*|                                unsigned short   w_EepromStartAddress,                |*/
-/*|                                unsigned short * pw_DataRead)                          |*/
-/*+----------------------------------------------------------------------------+*/
-/*| Task              : Read word from the 5920 eeprom.                        |*/
-/*+----------------------------------------------------------------------------+*/
-/*| Input Parameters  : int    i_NbOfWordsToRead : Nbr. of word to read        |*/
-/*|                     unsigned int dw_PCIBoardEepromAddress : Address of the eeprom |*/
-/*|                     unsigned short   w_EepromStartAddress : Eeprom start address     |*/
-/*+----------------------------------------------------------------------------+*/
-/*| Output Parameters : unsigned short * pw_DataRead : Read data                          |*/
-/*+----------------------------------------------------------------------------+*/
-/*| Return Value      : -                                                      |*/
-/*+----------------------------------------------------------------------------+*/
-
-int i_AddiHeaderRW_ReadEeprom(int i_NbOfWordsToRead,
-	unsigned int dw_PCIBoardEepromAddress,
-	unsigned short w_EepromStartAddress, unsigned short *pw_DataRead)
-{
-	unsigned int dw_eeprom_busy = 0;
-	int i_Counter = 0;
-	int i_WordCounter;
-	int i;
-	unsigned char pb_ReadByte[1];
-	unsigned char b_ReadLowByte = 0;
-	unsigned char b_ReadHighByte = 0;
-	unsigned char b_SelectedAddressLow = 0;
-	unsigned char b_SelectedAddressHigh = 0;
-	unsigned short w_ReadWord = 0;
-
-	for (i_WordCounter = 0; i_WordCounter < i_NbOfWordsToRead;
-		i_WordCounter++) {
-		do {
-			dw_eeprom_busy =
-				inl(dw_PCIBoardEepromAddress +
-				AMCC_OP_REG_MCSR);
-			dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-		} while (dw_eeprom_busy == EEPROM_BUSY);
-
-		for (i_Counter = 0; i_Counter < 2; i_Counter++) {
-			b_SelectedAddressLow = (w_EepromStartAddress + i_Counter) % 256;	/* Read the low 8 bit part */
-			b_SelectedAddressHigh = (w_EepromStartAddress + i_Counter) / 256;	/* Read the high 8 bit part */
-
-			/* Select the load low address mode */
-			outb(NVCMD_LOAD_LOW,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Load the low address */
-			outb(b_SelectedAddressLow,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the load high address mode */
-			outb(NVCMD_LOAD_HIGH,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Load the high address */
-			outb(b_SelectedAddressHigh,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the READ mode */
-			outb(NVCMD_BEGIN_READ,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Read data into the EEPROM */
-			*pb_ReadByte =
-				inb(dw_PCIBoardEepromAddress +
-				AMCC_OP_REG_MCSR + 2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the upper address part */
-			if (i_Counter == 0)
-				b_ReadLowByte = pb_ReadByte[0];
-			else
-				b_ReadHighByte = pb_ReadByte[0];
-
-			/* Sleep */
-			msleep(1);
-
-		}
-		w_ReadWord =
-			(b_ReadLowByte | (((unsigned short)b_ReadHighByte) *
-				256));
-
-		pw_DataRead[i_WordCounter] = w_ReadWord;
-
-		w_EepromStartAddress += 2;	/*  to read the next word */
-
-	}			/*  for (...) i_NbOfWordsToRead */
-	return 0;
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.h b/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.h
deleted file mode 100644
index 9afdb1357aa9..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/addi_amcc_S5920.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- *  Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
- *
- *	ADDI-DATA GmbH
- *	Dieselstrasse 3
- *	D-77833 Ottersweier
- *	Tel: +19(0)7223/9493-0
- *	Fax: +49(0)7223/9493-92
- *	http://www.addi-data.com
- *	info@addi-data.com
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- */
-
-#define AMCC_OP_REG_MCSR	0x3c
-#define EEPROM_BUSY		0x80000000
-#define NVCMD_LOAD_LOW		(0x4 << 5)	/* nvRam load low command */
-#define NVCMD_LOAD_HIGH		(0x5 << 5)	/* nvRam load high command */
-#define NVCMD_BEGIN_READ	(0x7 << 5)	/* nvRam begin read command */
-#define NVCMD_BEGIN_WRITE	(0x6 << 5)	/* EEPROM begin write command */
-
-int i_AddiHeaderRW_ReadEeprom(int i_NbOfWordsToRead,
-			      unsigned int dw_PCIBoardEepromAddress,
-			      unsigned short w_EepromStartAddress, unsigned short *pw_DataRead);
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index a3d4ed25fb0d..99a96bd96716 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -1669,7 +1669,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			return ret;
 
 		/*  Allocate and Initialise AI Subdevice Structures */
-		s = dev->subdevices + 0;
+		s = &dev->subdevices[0];
 		if ((devpriv->s_EeParameters.i_NbrAiChannel)
 			|| (this_board->i_NbrAiChannelDiff)) {
 			dev->read_subdev = s;
@@ -1705,7 +1705,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 
 		/*  Allocate and Initialise AO Subdevice Structures */
-		s = dev->subdevices + 1;
+		s = &dev->subdevices[1];
 		if (devpriv->s_EeParameters.i_NbrAoChannel) {
 			s->type = COMEDI_SUBD_AO;
 			s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
@@ -1720,7 +1720,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->type = COMEDI_SUBD_UNUSED;
 		}
 		/*  Allocate and Initialise DI Subdevice Structures */
-		s = dev->subdevices + 2;
+		s = &dev->subdevices[2];
 		if (devpriv->s_EeParameters.i_NbrDiChannel) {
 			s->type = COMEDI_SUBD_DI;
 			s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -1738,7 +1738,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->type = COMEDI_SUBD_UNUSED;
 		}
 		/*  Allocate and Initialise DO Subdevice Structures */
-		s = dev->subdevices + 3;
+		s = &dev->subdevices[3];
 		if (devpriv->s_EeParameters.i_NbrDoChannel) {
 			s->type = COMEDI_SUBD_DO;
 			s->subdev_flags =
@@ -1760,7 +1760,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 
 		/*  Allocate and Initialise Timer Subdevice Structures */
-		s = dev->subdevices + 4;
+		s = &dev->subdevices[4];
 		if (devpriv->s_EeParameters.i_Timer) {
 			s->type = COMEDI_SUBD_TIMER;
 			s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
@@ -1778,7 +1778,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 
 		/*  Allocate and Initialise TTL */
-		s = dev->subdevices + 5;
+		s = &dev->subdevices[5];
 		if (this_board->i_NbrTTLChannel) {
 			s->type = COMEDI_SUBD_TTLIO;
 			s->subdev_flags =
@@ -1797,7 +1797,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 
 		/* EEPROM */
-		s = dev->subdevices + 6;
+		s = &dev->subdevices[6];
 		if (this_board->i_PCIEeprom) {
 			s->type = COMEDI_SUBD_MEMORY;
 			s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
diff --git a/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h b/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h
deleted file mode 100644
index c26c28c31b97..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h
+++ /dev/null
@@ -1,453 +0,0 @@
-/*
-	Modified by umesh on 16th may 2001
-	Modified by sarath on 22nd may 2001
-*/
-
-/*
-    comedi/drivers/amcc_s5933_v_58.h
-
-    Stuff for AMCC S5933 PCI Controller
-
-    Author: Michal Dobes <majkl@tesnet.cz>
-
-    Inspirated from general-purpose AMCC S5933 PCI Matchmaker driver
-    made by Andrea Cisternino  <acister@pcape1.pi.infn.it>
-    and as result of espionage from MITE code made by David A. Schleef.
-    Thanks to AMCC for their on-line documentation and bus master DMA
-    example.
-*/
-
-#ifndef _AMCC_S5933_H_
-#define _AMCC_S5933_H_
-
-#include <linux/pci.h>
-#include "../../comedidev.h"
-
-/***********Added by sarath for compatibility with APCI3120
-
-*************************/
-
-#define FIFO_ADVANCE_ON_BYTE_2     0x20000000	/*  written on base0 */
-
-#define AMWEN_ENABLE                     0x02	/*  added for step 6 dma written on base2 */
-#define A2P_FIFO_WRITE_ENABLE            0x01
-
-#define AGCSTS_TC_ENABLE		   0x10000000	/*  Added for transfer count enable bit */
-
-/* ADDON RELATED ADDITIONS */
-/* Constant */
-#define     APCI3120_ENABLE_TRANSFER_ADD_ON_LOW       0x00
-#define     APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH      0x1200
-#define     APCI3120_A2P_FIFO_MANAGEMENT              0x04000400L
-#define     APCI3120_AMWEN_ENABLE                     0x02
-#define     APCI3120_A2P_FIFO_WRITE_ENABLE            0x01
-#define     APCI3120_FIFO_ADVANCE_ON_BYTE_2           0x20000000L
-#define     APCI3120_ENABLE_WRITE_TC_INT              0x00004000L
-#define     APCI3120_CLEAR_WRITE_TC_INT               0x00040000L
-#define     APCI3120_DISABLE_AMWEN_AND_A2P_FIFO_WRITE 0x0
-#define     APCI3120_DISABLE_BUS_MASTER_ADD_ON        0x0
-#define     APCI3120_DISABLE_BUS_MASTER_PCI           0x0
-
- /*  ADD_ON ::: this needed since apci supports 16 bit interface to add on */
-#define     APCI3120_ADD_ON_AGCSTS_LOW       0x3C
-#define     APCI3120_ADD_ON_AGCSTS_HIGH      APCI3120_ADD_ON_AGCSTS_LOW + 2
-#define     APCI3120_ADD_ON_MWAR_LOW         0x24
-#define     APCI3120_ADD_ON_MWAR_HIGH        APCI3120_ADD_ON_MWAR_LOW + 2
-#define     APCI3120_ADD_ON_MWTC_LOW         0x058
-#define     APCI3120_ADD_ON_MWTC_HIGH        APCI3120_ADD_ON_MWTC_LOW + 2
-
-/* AMCC */
-#define     APCI3120_AMCC_OP_MCSR            0x3C
-#define     APCI3120_AMCC_OP_REG_INTCSR      0x38
-
-/*******from here all upward definitions are added by sarath */
-
-/****************************************************************************/
-/* AMCC Operation Register Offsets - PCI                                    */
-/****************************************************************************/
-
-#define AMCC_OP_REG_OMB1         0x00
-#define AMCC_OP_REG_OMB2         0x04
-#define AMCC_OP_REG_OMB3         0x08
-#define AMCC_OP_REG_OMB4         0x0c
-#define AMCC_OP_REG_IMB1         0x10
-#define AMCC_OP_REG_IMB2         0x14
-#define AMCC_OP_REG_IMB3         0x18
-#define AMCC_OP_REG_IMB4         0x1c
-#define AMCC_OP_REG_FIFO         0x20
-#define AMCC_OP_REG_MWAR         0x24
-#define AMCC_OP_REG_MWTC         0x28
-#define AMCC_OP_REG_MRAR         0x2c
-#define AMCC_OP_REG_MRTC         0x30
-#define AMCC_OP_REG_MBEF         0x34
-#define AMCC_OP_REG_INTCSR       0x38
-#define  AMCC_OP_REG_INTCSR_SRC  (AMCC_OP_REG_INTCSR + 2)	/* int source */
-#define  AMCC_OP_REG_INTCSR_FEC  (AMCC_OP_REG_INTCSR + 3)	/* FIFO ctrl */
-#define AMCC_OP_REG_MCSR         0x3c
-#define  AMCC_OP_REG_MCSR_NVDATA (AMCC_OP_REG_MCSR + 2)	/* Data in byte 2 */
-#define  AMCC_OP_REG_MCSR_NVCMD  (AMCC_OP_REG_MCSR + 3)	/* Command in byte 3 */
-
-#define AMCC_FIFO_DEPTH_DWORD	8
-#define AMCC_FIFO_DEPTH_BYTES	(8 * sizeof (u32))
-
-/****************************************************************************/
-/* AMCC Operation Registers Size - PCI                                      */
-/****************************************************************************/
-
-#define AMCC_OP_REG_SIZE	 64	/* in bytes */
-
-/****************************************************************************/
-/* AMCC Operation Register Offsets - Add-on                                 */
-/****************************************************************************/
-
-#define AMCC_OP_REG_AIMB1         0x00
-#define AMCC_OP_REG_AIMB2         0x04
-#define AMCC_OP_REG_AIMB3         0x08
-#define AMCC_OP_REG_AIMB4         0x0c
-#define AMCC_OP_REG_AOMB1         0x10
-#define AMCC_OP_REG_AOMB2         0x14
-#define AMCC_OP_REG_AOMB3         0x18
-#define AMCC_OP_REG_AOMB4         0x1c
-#define AMCC_OP_REG_AFIFO         0x20
-#define AMCC_OP_REG_AMWAR         0x24
-#define AMCC_OP_REG_APTA          0x28
-#define AMCC_OP_REG_APTD          0x2c
-#define AMCC_OP_REG_AMRAR         0x30
-#define AMCC_OP_REG_AMBEF         0x34
-#define AMCC_OP_REG_AINT          0x38
-#define AMCC_OP_REG_AGCSTS        0x3c
-#define AMCC_OP_REG_AMWTC         0x58
-#define AMCC_OP_REG_AMRTC         0x5c
-
-/****************************************************************************/
-/* AMCC - Add-on General Control/Status Register                            */
-/****************************************************************************/
-
-#define AGCSTS_CONTROL_MASK	0xfffff000
-#define  AGCSTS_NV_ACC_MASK	0xe0000000
-#define  AGCSTS_RESET_MASK	0x0e000000
-#define  AGCSTS_NV_DA_MASK	0x00ff0000
-#define  AGCSTS_BIST_MASK	0x0000f000
-#define AGCSTS_STATUS_MASK	0x000000ff
-#define  AGCSTS_TCZERO_MASK	0x000000c0
-#define  AGCSTS_FIFO_ST_MASK	0x0000003f
-
-#define AGCSTS_RESET_MBFLAGS	0x08000000
-#define AGCSTS_RESET_P2A_FIFO	0x04000000
-#define AGCSTS_RESET_A2P_FIFO	0x02000000
-#define AGCSTS_RESET_FIFOS	(AGCSTS_RESET_A2P_FIFO | AGCSTS_RESET_P2A_FIFO)
-
-#define AGCSTS_A2P_TCOUNT	0x00000080
-#define AGCSTS_P2A_TCOUNT	0x00000040
-
-#define AGCSTS_FS_P2A_EMPTY	0x00000020
-#define AGCSTS_FS_P2A_HALF	0x00000010
-#define AGCSTS_FS_P2A_FULL	0x00000008
-
-#define AGCSTS_FS_A2P_EMPTY	0x00000004
-#define AGCSTS_FS_A2P_HALF	0x00000002
-#define AGCSTS_FS_A2P_FULL	0x00000001
-
-/****************************************************************************/
-/* AMCC - Add-on Interrupt Control/Status Register                            */
-/****************************************************************************/
-
-#define AINT_INT_MASK		0x00ff0000
-#define AINT_SEL_MASK		0x0000ffff
-#define  AINT_IS_ENSEL_MASK	0x00001f1f
-
-#define AINT_INT_ASSERTED	0x00800000
-#define AINT_BM_ERROR		0x00200000
-#define AINT_BIST_INT		0x00100000
-
-#define AINT_RT_COMPLETE	0x00080000
-#define AINT_WT_COMPLETE	0x00040000
-
-#define AINT_OUT_MB_INT		0x00020000
-#define AINT_IN_MB_INT		0x00010000
-
-#define AINT_READ_COMPL		0x00008000
-#define AINT_WRITE_COMPL	0x00004000
-
-#define AINT_OMB_ENABLE 	0x00001000
-#define AINT_OMB_SELECT 	0x00000c00
-#define AINT_OMB_BYTE		0x00000300
-
-#define AINT_IMB_ENABLE 	0x00000010
-#define AINT_IMB_SELECT 	0x0000000c
-#define AINT_IMB_BYTE		0x00000003
-
-/* Enable Bus Mastering */
-#define EN_A2P_TRANSFERS	0x00000400
-/* FIFO Flag Reset */
-#define RESET_A2P_FLAGS		0x04000000L
-/* FIFO Relative Priority */
-#define A2P_HI_PRIORITY		0x00000100L
-/* Identify Interrupt Sources */
-#define ANY_S593X_INT		0x00800000L
-#define READ_TC_INT		0x00080000L
-#define WRITE_TC_INT		0x00040000L
-#define IN_MB_INT		0x00020000L
-#define MASTER_ABORT_INT	0x00100000L
-#define TARGET_ABORT_INT	0x00200000L
-#define BUS_MASTER_INT		0x00200000L
-
-/****************************************************************************/
-
-struct pcilst_struct {
-	struct pcilst_struct *next;
-	int used;
-	struct pci_dev *pcidev;
-	unsigned short vendor;
-	unsigned short device;
-	unsigned int master;
-	unsigned char pci_bus;
-	unsigned char pci_slot;
-	unsigned char pci_func;
-	unsigned int io_addr[5];
-	unsigned int irq;
-};
-
-struct pcilst_struct *amcc_devices;	/*  ptr to root list of all amcc devices */
-
-/****************************************************************************/
-
-void v_pci_card_list_init(unsigned short pci_vendor, char display);
-void v_pci_card_list_cleanup(unsigned short pci_vendor);
-struct pcilst_struct *ptr_find_free_pci_card_by_device(unsigned short vendor_id,
-						       unsigned short
-						       device_id);
-int i_find_free_pci_card_by_position(unsigned short vendor_id,
-				     unsigned short device_id,
-				     unsigned short pci_bus,
-				     unsigned short pci_slot,
-				     struct pcilst_struct **card);
-struct pcilst_struct *ptr_select_and_alloc_pci_card(unsigned short vendor_id,
-						    unsigned short device_id,
-						    unsigned short pci_bus,
-						    unsigned short pci_slot);
-
-int i_pci_card_alloc(struct pcilst_struct *amcc);
-int i_pci_card_free(struct pcilst_struct *amcc);
-void v_pci_card_list_display(void);
-int i_pci_card_data(struct pcilst_struct *amcc,
-		    unsigned char *pci_bus, unsigned char *pci_slot,
-		    unsigned char *pci_func, unsigned short *io_addr,
-		    unsigned short *irq, unsigned short *master);
-
-/****************************************************************************/
-
-/* build list of amcc cards in this system */
-void v_pci_card_list_init(unsigned short pci_vendor, char display)
-{
-	struct pci_dev *pcidev;
-	struct pcilst_struct *amcc, *last;
-	int i;
-
-	amcc_devices = NULL;
-	last = NULL;
-
-	pci_for_each_dev(pcidev) {
-		if (pcidev->vendor == pci_vendor) {
-			amcc = kzalloc(sizeof(*amcc), GFP_KERNEL);
-			if (amcc == NULL)
-				continue;
-
-			amcc->pcidev = pcidev;
-			if (last) {
-				last->next = amcc;
-			} else {
-				amcc_devices = amcc;
-			}
-			last = amcc;
-
-			amcc->vendor = pcidev->vendor;
-			amcc->device = pcidev->device;
-#if 0
-			amcc->master = pcidev->master;	/*  how get this information under 2.4 kernels? */
-#endif
-			amcc->pci_bus = pcidev->bus->number;
-			amcc->pci_slot = PCI_SLOT(pcidev->devfn);
-			amcc->pci_func = PCI_FUNC(pcidev->devfn);
-			for (i = 0; i < 5; i++)
-				amcc->io_addr[i] =
-				    pcidev->resource[i].start & ~3UL;
-			amcc->irq = pcidev->irq;
-		}
-	}
-
-	if (display)
-		v_pci_card_list_display();
-}
-
-/****************************************************************************/
-/* free up list of amcc cards in this system */
-void v_pci_card_list_cleanup(unsigned short pci_vendor)
-{
-	struct pcilst_struct *amcc, *next;
-
-	for (amcc = amcc_devices; amcc; amcc = next) {
-		next = amcc->next;
-		kfree(amcc);
-	}
-
-	amcc_devices = NULL;
-}
-
-/****************************************************************************/
-/* find first unused card with this device_id */
-struct pcilst_struct *ptr_find_free_pci_card_by_device(unsigned short vendor_id,
-						       unsigned short device_id)
-{
-	struct pcilst_struct *amcc, *next;
-
-	for (amcc = amcc_devices; amcc; amcc = next) {
-		next = amcc->next;
-		if ((!amcc->used) && (amcc->device == device_id)
-		    && (amcc->vendor == vendor_id))
-			return amcc;
-
-	}
-
-	return NULL;
-}
-
-/****************************************************************************/
-/* find card on requested position */
-int i_find_free_pci_card_by_position(unsigned short vendor_id,
-				     unsigned short device_id,
-				     unsigned short pci_bus,
-				     unsigned short pci_slot,
-				     struct pcilst_struct **card)
-{
-	struct pcilst_struct *amcc, *next;
-
-	*card = NULL;
-	for (amcc = amcc_devices; amcc; amcc = next) {
-		next = amcc->next;
-		if ((amcc->vendor == vendor_id) && (amcc->device == device_id)
-		    && (amcc->pci_bus == pci_bus)
-		    && (amcc->pci_slot == pci_slot)) {
-			if (!(amcc->used)) {
-				*card = amcc;
-				return 0;	/*  ok, card is found */
-			} else {
-				printk
-				    (" - \nCard on requested position is used b:s %d:%d!\n",
-				     pci_bus, pci_slot);
-				return 2;	/*  card exist but is used */
-			}
-		}
-	}
-
-	return 1;		/*  no card found */
-}
-
-/****************************************************************************/
-/* mark card as used */
-int i_pci_card_alloc(struct pcilst_struct *amcc)
-{
-	if (!amcc)
-		return -1;
-
-	if (amcc->used)
-		return 1;
-	amcc->used = 1;
-	return 0;
-}
-
-/****************************************************************************/
-/* mark card as free */
-int i_pci_card_free(struct pcilst_struct *amcc)
-{
-	if (!amcc)
-		return -1;
-
-	if (!amcc->used)
-		return 1;
-	amcc->used = 0;
-	return 0;
-}
-
-/****************************************************************************/
-/* display list of found cards */
-void v_pci_card_list_display(void)
-{
-	struct pcilst_struct *amcc, *next;
-
-	printk("List of pci cards\n");
-	printk("bus:slot:func vendor device master io_amcc io_daq irq used\n");
-
-	for (amcc = amcc_devices; amcc; amcc = next) {
-		next = amcc->next;
-		printk
-		    ("%2d   %2d   %2d  0x%4x 0x%4x   %3s   0x%4x 0x%4x  %2d  %2d\n",
-		     amcc->pci_bus, amcc->pci_slot, amcc->pci_func,
-		     amcc->vendor, amcc->device, amcc->master ? "yes" : "no",
-		     amcc->io_addr[0], amcc->io_addr[2], amcc->irq, amcc->used);
-
-	}
-}
-
-/****************************************************************************/
-/* return all card information for driver */
-int i_pci_card_data(struct pcilst_struct *amcc,
-		    unsigned char *pci_bus, unsigned char *pci_slot,
-		    unsigned char *pci_func, unsigned short *io_addr,
-		    unsigned short *irq, unsigned short *master)
-{
-	int i;
-
-	if (!amcc)
-		return -1;
-	*pci_bus = amcc->pci_bus;
-	*pci_slot = amcc->pci_slot;
-	*pci_func = amcc->pci_func;
-	for (i = 0; i < 5; i++)
-		io_addr[i] = amcc->io_addr[i];
-	*irq = amcc->irq;
-	*master = amcc->master;
-	return 0;
-}
-
-/****************************************************************************/
-/* select and alloc card */
-struct pcilst_struct *ptr_select_and_alloc_pci_card(unsigned short vendor_id,
-						    unsigned short device_id,
-						    unsigned short pci_bus,
-						    unsigned short pci_slot)
-{
-	struct pcilst_struct *card;
-
-	if ((pci_bus < 1) & (pci_slot < 1)) {	/*  use autodetection */
-		card = ptr_find_free_pci_card_by_device(vendor_id, device_id);
-		if (card == NULL) {
-			printk(" - Unused card not found in system!\n");
-			return NULL;
-		}
-	} else {
-		switch (i_find_free_pci_card_by_position(vendor_id, device_id,
-							 pci_bus, pci_slot,
-							 &card)) {
-		case 1:
-			printk
-			    (" - Card not found on requested position b:s %d:%d!\n",
-			     pci_bus, pci_slot);
-			return NULL;
-		case 2:
-			printk
-			    (" - Card on requested position is used b:s %d:%d!\n",
-			     pci_bus, pci_slot);
-			return NULL;
-		}
-	}
-
-	if (i_pci_card_alloc(card) != 0) {
-		printk(" - Can't allocate card!\n");
-		return NULL;
-	}
-
-	return card;
-}
-
-#endif
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
index 595238feaf42..f9a8937be8ed 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
@@ -67,7 +67,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 		return;
 
 	/*  Allocate and Initialise Timer Subdevice Structures */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 
 	s->type = COMEDI_SUBD_TIMER;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
@@ -81,7 +81,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_bits = i_APCI1710_InsnBitsTimer;
 
 	/*  Allocate and Initialise DIO Subdevice Structures */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags =
@@ -96,7 +96,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_write = i_APCI1710_InsnWriteDigitalIOChlOnOff;
 
 	/*  Allocate and Initialise Chrono Subdevice Structures */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 
 	s->type = COMEDI_SUBD_CHRONO;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
@@ -110,7 +110,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_bits = i_APCI1710_InsnBitsChronoDigitalIO;
 
 	/*  Allocate and Initialise PWM Subdevice Structures */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_PWM;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -125,7 +125,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_bits = i_APCI1710_InsnBitsReadPWMInterrupt;
 
 	/*  Allocate and Initialise TTLIO Subdevice Structures */
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	s->type = COMEDI_SUBD_TTLIO;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -139,7 +139,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_read = i_APCI1710_InsnReadTTLIOAllPortValue;
 
 	/*  Allocate and Initialise TOR Subdevice Structures */
-	s = dev->subdevices + 5;
+	s = &dev->subdevices[5];
 	s->type = COMEDI_SUBD_TOR;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -154,7 +154,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_bits = i_APCI1710_InsnBitsGetTorCounterProgressStatusAndValue;
 
 	/*  Allocate and Initialise SSI Subdevice Structures */
-	s = dev->subdevices + 6;
+	s = &dev->subdevices[6];
 	s->type = COMEDI_SUBD_SSI;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -167,7 +167,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_bits = i_APCI1710_InsnBitsSSIDigitalIO;
 
 	/*  Allocate and Initialise PULSEENCODER Subdevice Structures */
-	s = dev->subdevices + 7;
+	s = &dev->subdevices[7];
 	s->type = COMEDI_SUBD_PULSEENCODER;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
@@ -181,7 +181,7 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	s->insn_read = i_APCI1710_InsnReadInterruptPulseEncoder;
 
 	/*  Allocate and Initialise INCREMENTALCOUNTER Subdevice Structures */
-	s = dev->subdevices + 8;
+	s = &dev->subdevices[8];
 	s->type = COMEDI_SUBD_INCREMENTALCOUNTER;
 	s->subdev_flags =
 		SDF_WRITEABLE | SDF_READABLE | SDF_GROUND | SDF_COMMON;
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
index ffe390c6da83..f406dfb2a677 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
@@ -479,57 +479,26 @@ int i_APCI3120_CommandTestAnalogInput(struct comedi_device *dev, struct comedi_s
 	struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;		/*  divisor1,divisor2; */
 
-	/*  step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW);
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-		cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-
-	if (cmd->convert_src != TRIG_TIMER)
-		err++;
+	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);
 
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1451,7 +1420,7 @@ void v_APCI3120_Interrupt(int irq, void *d)
 	unsigned short us_TmpValue;
 	unsigned char b_DummyRead;
 
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	ui_Check = 1;
 
 	int_daq = inw(dev->iobase + APCI3120_RD_STATUS) & 0xf000;	/*  get IRQ reasons */
@@ -1656,7 +1625,7 @@ void v_APCI3120_Interrupt(int irq, void *d)
 int i_APCI3120_InterruptHandleEos(struct comedi_device *dev)
 {
 	int n_chan, i;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int err = 1;
 
 	n_chan = devpriv->ui_AiNbrofChannels;
@@ -1698,7 +1667,7 @@ int i_APCI3120_InterruptHandleEos(struct comedi_device *dev)
 void v_APCI3120_InterruptDma(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	unsigned int next_dma_buf, samplesinbuf;
 	unsigned long low_word, high_word, var;
 
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
index f9545b064eaf..38ab49917d7e 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
@@ -57,11 +57,8 @@ You should also find the complete GPL in the COPYING file accompanying this sour
   +----------------------------------------------------------------------------+
 */
 #include "hwdrv_apci3200.h"
-/* Begin JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-#include "addi_amcc_S5920.h"
-/* #define PRINT_INFO */
 
-/* End JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
+/* #define PRINT_INFO */
 
 /* BEGIN JK 06.07.04: Management of sevrals boards */
 /*
@@ -90,7 +87,12 @@ You should also find the complete GPL in the COPYING file accompanying this sour
 struct str_BoardInfos s_BoardInfos[100];	/*  100 will be the max number of boards to be used */
 /* END JK 06.07.04: Management of sevrals boards */
 
-/* Begin JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
+#define AMCC_OP_REG_MCSR	0x3c
+#define EEPROM_BUSY		0x80000000
+#define NVCMD_LOAD_LOW		(0x4 << 5)	/* nvRam load low command */
+#define NVCMD_LOAD_HIGH		(0x5 << 5)	/* nvRam load high command */
+#define NVCMD_BEGIN_READ	(0x7 << 5)	/* nvRam begin read command */
+#define NVCMD_BEGIN_WRITE	(0x6 << 5)	/* EEPROM begin write command */
 
 /*+----------------------------------------------------------------------------+*/
 /*| Function   Name   : int i_AddiHeaderRW_ReadEeprom                          |*/
@@ -2558,7 +2560,6 @@ int i_APCI3200_CommandTestAnalogInput(struct comedi_device *dev, struct comedi_s
 {
 
 	int err = 0;
-	int tmp;		/*  divisor1,divisor2; */
 	unsigned int ui_ConvertTime = 0;
 	unsigned int ui_ConvertTimeBase = 0;
 	unsigned int ui_DelayTime = 0;
@@ -2569,41 +2570,32 @@ int i_APCI3200_CommandTestAnalogInput(struct comedi_device *dev, struct comedi_s
 	int i_Cpt = 0;
 	double d_ConversionTimeForAllChannels = 0.0;
 	double d_SCANTimeNewUnit = 0.0;
-	/*  step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
-	/* if(i_InterruptFlag==0) */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		err++;
-		/*           printk("\nThe interrupt should be enabled\n"); */
-	}
+
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW);
+	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);
+
+	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0)
+		err |= -EINVAL;
+
 	if (err) {
 		i_APCI3200_Reset(dev);
 		return 1;
 	}
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT) {
-		err++;
-	}
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
+
 	if (cmd->start_src == TRIG_EXT) {
 		i_TriggerEdge = cmd->start_arg & 0xFFFF;
 		i_Triggermode = cmd->start_arg >> 16;
@@ -2617,21 +2609,6 @@ int i_APCI3200_CommandTestAnalogInput(struct comedi_device *dev, struct comedi_s
 		}
 	}
 
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-		cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-
-	if (cmd->convert_src != TRIG_TIMER)
-		err++;
-
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
-
 	if (err) {
 		i_APCI3200_Reset(dev);
 		return 2;
@@ -3495,7 +3472,7 @@ void v_APCI3200_Interrupt(int irq, void *d)
 int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
 {
 	unsigned int ui_StatusRegister = 0;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 
 	/* BEGIN JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
 	/* comedi_async *async = s->async; */
diff --git a/drivers/staging/comedi/drivers/addi_apci_all.c b/drivers/staging/comedi/drivers/addi_apci_all.c
deleted file mode 100644
index aeb1b2688f3d..000000000000
--- a/drivers/staging/comedi/drivers/addi_apci_all.c
+++ /dev/null
@@ -1,18 +0,0 @@
-#define CONFIG_APCI_035  1
-#define CONFIG_APCI_1032 1
-#define CONFIG_APCI_1500 1
-#define CONFIG_APCI_1516 1
-#define CONFIG_APCI_1564 1
-#define CONFIG_APCI_16XX 1
-#define CONFIG_APCI_1710 1
-#define CONFIG_APCI_2016 1
-#define CONFIG_APCI_2032 1
-#define CONFIG_APCI_2200 1
-#define CONFIG_APCI_3001 1
-#define CONFIG_APCI_3120 1
-#define CONFIG_APCI_3200 1
-#define CONFIG_APCI_3300 1
-#define CONFIG_APCI_3501 1
-#define CONFIG_APCI_3XXX 1
-
-#include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index 3bec0f6e4a8c..3492ce1156e0 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -27,14 +27,14 @@
 */
 /*
 Driver: adl_pci6208
-Description: ADLink PCI-6208A
-Devices: [ADLink] PCI-6208A (adl_pci6208)
+Description: ADLink PCI-6208/6216 Series Multi-channel Analog Output Cards
+Devices: (ADLink) PCI-6208 [adl_pci6208]
+	 (ADLink) PCI-6216 [adl_pci6216]
 Author: nsyeow <nsyeow@pd.jaring.my>
 Updated: Fri, 30 Jan 2004 14:44:27 +0800
 Status: untested
 
-Configuration Options:
-  none
+Configuration Options: not applicable, uses PCI auto config
 
 References:
 	- ni_660x.c
@@ -45,6 +45,12 @@ References:
 #include "../comedidev.h"
 
 /*
+ * ADLINK PCI Device ID's supported by this driver
+ */
+#define PCI_DEVICE_ID_PCI6208		0x6208
+#define PCI_DEVICE_ID_PCI6216		0x6216
+
+/*
  * PCI-6208/6216-GL register map
  */
 #define PCI6208_AO_CONTROL(x)		(0x00 + (2 * (x)))
@@ -56,7 +62,7 @@ References:
 #define PCI6208_DIO_DI_MASK		(0xf0)
 #define PCI6208_DIO_DI_SHIFT		(4)
 
-#define PCI6208_MAX_AO_CHANNELS		8
+#define PCI6208_MAX_AO_CHANNELS		16
 
 struct pci6208_board {
 	const char *name;
@@ -66,9 +72,13 @@ struct pci6208_board {
 
 static const struct pci6208_board pci6208_boards[] = {
 	{
-		.name		= "pci6208a",
-		.dev_id		= 0x6208,
+		.name		= "adl_pci6208",
+		.dev_id		= PCI_DEVICE_ID_PCI6208,
 		.ao_chans	= 8,
+	}, {
+		.name		= "adl_pci6216",
+		.dev_id		= PCI_DEVICE_ID_PCI6216,
+		.ao_chans	= 16,
 	},
 };
 
@@ -115,141 +125,122 @@ static int pci6208_ao_rinsn(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int pci6208_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
+static int pci6208_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	unsigned int val;
+
+	val = inw(dev->iobase + PCI6208_DIO);
+	val = (val & PCI6208_DIO_DI_MASK) >> PCI6208_DIO_DI_SHIFT;
+
+	data[1] = val;
+
+	return insn->n;
+}
+
+static int pci6208_do_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	unsigned int mask = data[0] & PCI6208_DIO_DO_MASK;
+	unsigned int mask = data[0];
 	unsigned int bits = data[1];
 
 	if (mask) {
 		s->state &= ~mask;
-		s->state |= bits & mask;
+		s->state |= (bits & mask);
 
 		outw(s->state, dev->iobase + PCI6208_DIO);
 	}
 
-	s->state = inw(dev->iobase + PCI6208_DIO);
 	data[1] = s->state;
 
 	return insn->n;
 }
 
-static int pci6208_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
-{
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask = 1 << chan;
-
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = (s->io_bits & mask) ? COMEDI_OUTPUT : COMEDI_INPUT;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return insn->n;
-}
-
-static struct pci_dev *pci6208_find_device(struct comedi_device *dev,
-					   struct comedi_devconfig *it)
+static const void *pci6208_find_boardinfo(struct comedi_device *dev,
+					  struct pci_dev *pcidev)
 {
-	const struct pci6208_board *thisboard;
-	struct pci_dev *pci_dev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
+	const struct pci6208_board *boardinfo;
 	int i;
 
-	for_each_pci_dev(pci_dev) {
-		if (pci_dev->vendor != PCI_VENDOR_ID_ADLINK)
-			continue;
-		for (i = 0; i < ARRAY_SIZE(pci6208_boards); i++) {
-			thisboard = &pci6208_boards[i];
-			if (thisboard->dev_id != pci_dev->device)
-				continue;
-			/* was a particular bus/slot requested? */
-			if (bus || slot) {
-				/* are we on the wrong bus/slot? */
-				if (pci_dev->bus->number != bus ||
-				    PCI_SLOT(pci_dev->devfn) != slot)
-					continue;
-			}
-			dev_dbg(dev->class_dev,
-				"Found %s on bus %d, slot, %d, irq=%d\n",
-				thisboard->name,
-				pci_dev->bus->number,
-				PCI_SLOT(pci_dev->devfn),
-				pci_dev->irq);
-			dev->board_ptr = thisboard;
-			return pci_dev;
-		}
+	for (i = 0; i < ARRAY_SIZE(pci6208_boards); i++) {
+		boardinfo = &pci6208_boards[i];
+		if (boardinfo->dev_id == pcidev->device)
+			return boardinfo;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int pci6208_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int pci6208_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev)
 {
-	const struct pci6208_board *thisboard;
+	const struct pci6208_board *boardinfo;
 	struct pci6208_private *devpriv;
-	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
+	unsigned int val;
 	int ret;
 
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	boardinfo = pci6208_find_boardinfo(dev, pcidev);
+	if (!boardinfo)
+		return -ENODEV;
+	dev->board_ptr = boardinfo;
+	dev->board_name = boardinfo->name;
+
 	ret = alloc_private(dev, sizeof(*devpriv));
 	if (ret < 0)
 		return ret;
 	devpriv = dev->private;
 
-	pcidev = pci6208_find_device(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-	thisboard = comedi_board(dev);
-
-	dev->board_name = thisboard->name;
-
-	ret = comedi_pci_enable(pcidev, dev->driver->driver_name);
-	if (ret) {
-		dev_err(dev->class_dev,
-			"Failed to enable PCI device and request regions\n");
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
 		return ret;
-	}
 	dev->iobase = pci_resource_start(pcidev, 2);
 
-	ret = comedi_alloc_subdevices(dev, 2);
+	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog output subdevice */
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE;
-	s->n_chan	= thisboard->ao_chans;
+	s->n_chan	= boardinfo->ao_chans;
 	s->maxdata	= 0xffff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci6208_ao_winsn;
 	s->insn_read	= pci6208_ao_rinsn;
 
-	s = dev->subdevices + 1;
-	/* digital i/o subdevice */
-	s->type		= COMEDI_SUBD_DIO;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
-	s->n_chan	= 8;
+	s = &dev->subdevices[1];
+	/* digital input subdevice */
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 4;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
-	s->insn_bits	= pci6208_dio_insn_bits;
-	s->insn_config	= pci6208_dio_insn_config;
+	s->insn_bits	= pci6208_di_insn_bits;
 
+	s = &dev->subdevices[2];
+	/* digital output subdevice */
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci6208_do_insn_bits;
+
+	/*
+	 * Get the read back signals from the digital outputs
+	 * and save it as the initial state for the subdevice.
+	 */
+	val = inw(dev->iobase + PCI6208_DIO);
+	val = (val & PCI6208_DIO_DO_MASK) >> PCI6208_DIO_DO_SHIFT;
+	s->state	= val;
 	s->io_bits	= 0x0f;
-	s->state	= inw(dev->iobase + PCI6208_DIO);
 
 	dev_info(dev->class_dev, "%s: %s, I/O base=0x%04lx\n",
 		dev->driver->driver_name, dev->board_name, dev->iobase);
@@ -264,14 +255,13 @@ static void pci6208_detach(struct comedi_device *dev)
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver adl_pci6208_driver = {
 	.driver_name	= "adl_pci6208",
 	.module		= THIS_MODULE,
-	.attach		= pci6208_attach,
+	.attach_pci	= pci6208_attach_pci,
 	.detach		= pci6208_detach,
 };
 
@@ -287,7 +277,8 @@ static void __devexit adl_pci6208_pci_remove(struct pci_dev *dev)
 }
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci6208_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x6208) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI6208) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI6216) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci6208_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci7230.c b/drivers/staging/comedi/drivers/adl_pci7230.c
deleted file mode 100644
index 7df4c960d5e4..000000000000
--- a/drivers/staging/comedi/drivers/adl_pci7230.c
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
-    comedi/drivers/adl_pci7230.c
-
-    Hardware comedi driver fot PCI7230 Adlink card
-    Copyright (C) 2010 David Fernandez <dfcastelao@gmail.com>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program; if not, write to the Free Software
-    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
-
-*/
-/*
-Driver: adl_pci7230
-Description: Driver for the Adlink PCI-7230 32 ch. isolated digital io board
-Devices: [ADLink] PCI-7230 (adl_pci7230)
-Author: David Fernandez <dfcastelao@gmail.com>
-Status: experimental
-Updated: Mon, 14 Apr 2008 15:08:14 +0100
-
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
-*/
-
-#include "../comedidev.h"
-#include <linux/kernel.h>
-
-#define PCI7230_DI      0x00
-#define PCI7230_DO	    0x00
-
-#define PCI_DEVICE_ID_PCI7230 0x7230
-
-static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
-	struct comedi_subdevice *s,
-	struct comedi_insn *insn,
-	unsigned int *data)
-{
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		outl((s->state  << 16) & 0xffffffff, dev->iobase + PCI7230_DO);
-	}
-
-	return insn->n;
-}
-
-static int adl_pci7230_di_insn_bits(struct comedi_device *dev,
-	struct comedi_subdevice *s,
-	struct comedi_insn *insn,
-	unsigned int *data)
-{
-	data[1] = inl(dev->iobase + PCI7230_DI) & 0xffffffff;
-
-	return insn->n;
-}
-
-static struct pci_dev *adl_pci7230_find_pci(struct comedi_device *dev,
-	struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
-		    pcidev->device != PCI_DEVICE_ID_PCI7230)
-			continue;
-		if (bus || slot) {
-			/* requested particular bus/slot */
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		return pcidev;
-	}
-	printk(KERN_ERR "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-		dev->minor, bus, slot);
-	return NULL;
-}
-
-static int adl_pci7230_attach(struct comedi_device *dev,
-	struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	struct pci_dev *pcidev;
-	int ret;
-
-	printk(KERN_INFO "comedi%d: adl_pci7230\n", dev->minor);
-
-	dev->board_name = "pci7230";
-
-	ret = comedi_alloc_subdevices(dev, 2);
-	if (ret)
-		return ret;
-
-	pcidev = adl_pci7230_find_pci(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	if (comedi_pci_enable(pcidev, "adl_pci7230") < 0) {
-		printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
-			dev->minor);
-		return -EIO;
-	}
-	dev->iobase = pci_resource_start(pcidev, 2);
-	printk(KERN_DEBUG "comedi: base addr %4lx\n", dev->iobase);
-
-	s = dev->subdevices + 0;
-	/* Isolated do */
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = adl_pci7230_do_insn_bits;
-
-	s = dev->subdevices + 1;
-	/* Isolated di */
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = adl_pci7230_di_insn_bits;
-
-	printk(KERN_DEBUG "comedi: attached\n");
-
-	return 1;
-}
-
-static void adl_pci7230_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-}
-
-static struct comedi_driver adl_pci7230_driver = {
-	.driver_name	= "adl_pci7230",
-	.module		= THIS_MODULE,
-	.attach		= adl_pci7230_attach,
-	.detach		= adl_pci7230_detach,
-};
-
-static int __devinit adl_pci7230_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &adl_pci7230_driver);
-}
-
-static void __devexit adl_pci7230_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7230_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, adl_pci7230_pci_table);
-
-static struct pci_driver adl_pci7230_pci_driver = {
-	.name		= "adl_pci7230",
-	.id_table	= adl_pci7230_pci_table,
-	.probe		= adl_pci7230_pci_probe,
-	.remove		= __devexit_p(adl_pci7230_pci_remove),
-};
-module_comedi_pci_driver(adl_pci7230_driver, adl_pci7230_pci_driver);
-
-MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci7296.c b/drivers/staging/comedi/drivers/adl_pci7296.c
deleted file mode 100644
index 19b47af9c10e..000000000000
--- a/drivers/staging/comedi/drivers/adl_pci7296.c
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
-    comedi/drivers/adl_pci7296.c
-
-    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.
-
-    You 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.
-
-*/
-/*
-Driver: adl_pci7296
-Description: Driver for the Adlink PCI-7296 96 ch. digital io board
-Devices: [ADLink] PCI-7296 (adl_pci7296)
-Author: Jon Grierson <jd@renko.co.uk>
-Updated: Mon, 14 Apr 2008 15:05:56 +0100
-Status: testing
-
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
-*/
-
-#include "../comedidev.h"
-#include <linux/kernel.h>
-
-#include "8255.h"
-/* #include "8253.h" */
-
-#define PORT1A 0
-#define PORT2A 4
-#define PORT3A 8
-#define PORT4A 12
-
-#define PCI_DEVICE_ID_PCI7296 0x7296
-
-static struct pci_dev *adl_pci7296_find_pci(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
-		    pcidev->device != PCI_DEVICE_ID_PCI7296)
-			continue;
-		if (bus || slot) {
-			/* requested particular bus/slot */
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		return pcidev;
-	}
-	printk(KERN_ERR
-		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-	       dev->minor, bus, slot);
-	return NULL;
-}
-
-static int adl_pci7296_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *s;
-	int ret;
-
-	printk(KERN_INFO "comedi%d: attach adl_pci7432\n", dev->minor);
-
-	dev->board_name = "pci7432";
-
-	ret = comedi_alloc_subdevices(dev, 4);
-	if (ret)
-		return ret;
-
-	pcidev = adl_pci7296_find_pci(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	if (comedi_pci_enable(pcidev, "adl_pci7296") < 0) {
-		printk(KERN_ERR
-			"comedi%d: Failed to enable PCI device and request regions\n",
-			dev->minor);
-		return -EIO;
-	}
-
-	dev->iobase = pci_resource_start(pcidev, 2);
-	printk(KERN_INFO "comedi: base addr %4lx\n", dev->iobase);
-
-	/*  four 8255 digital io subdevices */
-	s = dev->subdevices + 0;
-	subdev_8255_init(dev, s, NULL, (unsigned long)(dev->iobase));
-
-	s = dev->subdevices + 1;
-	ret = subdev_8255_init(dev, s, NULL,
-				(unsigned long)(dev->iobase + PORT2A));
-	if (ret < 0)
-		return ret;
-
-	s = dev->subdevices + 2;
-	ret = subdev_8255_init(dev, s, NULL,
-				(unsigned long)(dev->iobase + PORT3A));
-	if (ret < 0)
-		return ret;
-
-	s = dev->subdevices + 3;
-	ret = subdev_8255_init(dev, s, NULL,
-				(unsigned long)(dev->iobase + PORT4A));
-	if (ret < 0)
-		return ret;
-
-	printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n", dev->minor);
-
-	return 0;
-}
-
-static void adl_pci7296_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-	if (dev->subdevices) {
-		subdev_8255_cleanup(dev, dev->subdevices + 0);
-		subdev_8255_cleanup(dev, dev->subdevices + 1);
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
-		subdev_8255_cleanup(dev, dev->subdevices + 3);
-	}
-}
-
-static struct comedi_driver adl_pci7296_driver = {
-	.driver_name	= "adl_pci7296",
-	.module		= THIS_MODULE,
-	.attach		= adl_pci7296_attach,
-	.detach		= adl_pci7296_detach,
-};
-
-static int __devinit adl_pci7296_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &adl_pci7296_driver);
-}
-
-static void __devexit adl_pci7296_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7296_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7296) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, adl_pci7296_pci_table);
-
-static struct pci_driver adl_pci7296_pci_driver = {
-	.name		= "adl_pci7296",
-	.id_table	= adl_pci7296_pci_table,
-	.probe		= adl_pci7296_pci_probe,
-	.remove		= __devexit_p(adl_pci7296_pci_remove),
-};
-module_comedi_pci_driver(adl_pci7296_driver, adl_pci7296_pci_driver);
-
-MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci7432.c b/drivers/staging/comedi/drivers/adl_pci7432.c
deleted file mode 100644
index 6b8d9408e3bc..000000000000
--- a/drivers/staging/comedi/drivers/adl_pci7432.c
+++ /dev/null
@@ -1,200 +0,0 @@
-/*
-    comedi/drivers/adl_pci7432.c
-
-    Hardware comedi driver fot PCI7432 Adlink card
-    Copyright (C) 2004 Michel Lachine <mike@mikelachaine.ca>
-
-    This program is free software; you can redistribute 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.
-
-*/
-/*
-Driver: adl_pci7432
-Description: Driver for the Adlink PCI-7432 64 ch. isolated digital io board
-Devices: [ADLink] PCI-7432 (adl_pci7432)
-Author: Michel Lachaine <mike@mikelachaine.ca>
-Status: experimental
-Updated: Mon, 14 Apr 2008 15:08:14 +0100
-
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
-*/
-
-#include "../comedidev.h"
-#include <linux/kernel.h>
-
-#define PCI7432_DI      0x00
-#define PCI7432_DO	    0x00
-
-#define PCI_DEVICE_ID_PCI7432 0x7432
-
-static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	printk(KERN_DEBUG "comedi: pci7432_do_insn_bits called\n");
-	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		printk(KERN_DEBUG "comedi: out: %8x on iobase %4lx\n", s->state,
-		       dev->iobase + PCI7432_DO);
-		outl(s->state & 0xffffffff, dev->iobase + PCI7432_DO);
-	}
-	return insn->n;
-}
-
-static int adl_pci7432_di_insn_bits(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	printk(KERN_DEBUG "comedi: pci7432_di_insn_bits called\n");
-	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
-
-	data[1] = inl(dev->iobase + PCI7432_DI) & 0xffffffff;
-	printk(KERN_DEBUG "comedi: data1 %8x\n", data[1]);
-
-	return insn->n;
-}
-
-static struct pci_dev *adl_pci7432_find_pci(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
-		    pcidev->device != PCI_DEVICE_ID_PCI7432)
-			continue;
-		if (bus || slot) {
-			/* requested particular bus/slot */
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		return pcidev;
-	}
-	printk(KERN_ERR
-		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-	       dev->minor, bus, slot);
-	return NULL;
-}
-
-static int adl_pci7432_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *s;
-	int ret;
-
-	printk(KERN_INFO "comedi%d: attach adl_pci7432\n", dev->minor);
-
-	dev->board_name = "pci7432";
-
-	ret = comedi_alloc_subdevices(dev, 2);
-	if (ret)
-		return ret;
-
-	pcidev = adl_pci7432_find_pci(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	if (comedi_pci_enable(pcidev, "adl_pci7432") < 0) {
-		printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
-			dev->minor);
-		return -EIO;
-	}
-	dev->iobase = pci_resource_start(pcidev, 2);
-	printk(KERN_INFO "comedi: base addr %4lx\n", dev->iobase);
-
-	s = dev->subdevices + 0;
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 32;
-	s->maxdata = 1;
-	s->len_chanlist = 32;
-	s->io_bits = 0x00000000;
-	s->range_table = &range_digital;
-	s->insn_bits = adl_pci7432_di_insn_bits;
-
-	s = dev->subdevices + 1;
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 32;
-	s->maxdata = 1;
-	s->len_chanlist = 32;
-	s->io_bits = 0xffffffff;
-	s->range_table = &range_digital;
-	s->insn_bits = adl_pci7432_do_insn_bits;
-
-	printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n", dev->minor);
-	return 0;
-}
-
-static void adl_pci7432_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-}
-
-static struct comedi_driver adl_pci7432_driver = {
-	.driver_name	= "adl_pci7432",
-	.module		= THIS_MODULE,
-	.attach		= adl_pci7432_attach,
-	.detach		= adl_pci7432_detach,
-};
-
-static int __devinit adl_pci7432_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &adl_pci7432_driver);
-}
-
-static void __devexit adl_pci7432_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7432_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, adl_pci7432_pci_table);
-
-static struct pci_driver adl_pci7432_pci_driver = {
-	.name		= "adl_pci7432",
-	.id_table	= adl_pci7432_pci_table,
-	.probe		= adl_pci7432_pci_probe,
-	.remove		= __devexit_p(adl_pci7432_pci_remove),
-};
-module_comedi_pci_driver(adl_pci7432_driver, adl_pci7432_pci_driver);
-
-MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci7x3x.c b/drivers/staging/comedi/drivers/adl_pci7x3x.c
new file mode 100644
index 000000000000..599714e978b5
--- /dev/null
+++ b/drivers/staging/comedi/drivers/adl_pci7x3x.c
@@ -0,0 +1,332 @@
+/*
+ * COMEDI driver for the ADLINK PCI-723x/743x series boards.
+ * Copyright (C) 2012 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * Based on the adl_pci7230 driver written by:
+ *	David Fernandez <dfcastelao@gmail.com>
+ * and the adl_pci7432 driver written by:
+ *	Michel Lachaine <mike@mikelachaine.ca>
+ *
+ * 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.
+ *
+ * You 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.
+ */
+
+/*
+Driver: adl_pci7x3x
+Description: 32/64-Channel Isolated Digital I/O Boards
+Devices: (ADLink) PCI-7230 [adl_pci7230] - 16 input / 16 output
+	 (ADLink) PCI-7233 [adl_pci7233] - 32 input
+	 (ADLink) PCI-7234 [adl_pci7234] - 32 output
+	 (ADLink) PCI-7432 [adl_pci7432] - 32 input / 32 output
+	 (ADLink) PCI-7433 [adl_pci7433] - 64 input
+	 (ADLink) PCI-7434 [adl_pci7434] - 64 output
+Author: H Hartley Sweeten <hsweeten@visionengravers.com>
+Updated: Thu, 02 Aug 2012 14:27:46 -0700
+Status: untested
+
+This driver only attaches using the PCI PnP auto config support
+in the comedi core. The module parameter 'comedi_autoconfig'
+must be 1 (default) to enable this feature. The COMEDI_DEVCONFIG
+ioctl, used by the comedi_config utility, is not supported by
+this driver.
+
+The PCI-7230, PCI-7432 and PCI-7433 boards also support external
+interrupt signals on digital input channels 0 and 1. The PCI-7233
+has dual-interrupt sources for change-of-state (COS) on any 16
+digital input channels of LSB and for COS on any 16 digital input
+lines of MSB. Interrupts are not currently supported by this
+driver.
+
+Configuration Options: not applicable
+*/
+
+#include "../comedidev.h"
+
+/*
+ * PCI Device ID's supported by this driver
+ */
+#define PCI_DEVICE_ID_PCI7230	0x7230
+#define PCI_DEVICE_ID_PCI7233	0x7233
+#define PCI_DEVICE_ID_PCI7234	0x7234
+#define PCI_DEVICE_ID_PCI7432	0x7432
+#define PCI_DEVICE_ID_PCI7433	0x7433
+#define PCI_DEVICE_ID_PCI7434	0x7434
+
+/*
+ * Register I/O map (32-bit access only)
+ */
+#define PCI7X3X_DIO_REG		0x00
+#define PCI743X_DIO_REG		0x04
+
+struct adl_pci7x3x_boardinfo {
+	const char *name;
+	unsigned short device;
+	int nsubdevs;
+	int di_nchan;
+	int do_nchan;
+};
+
+static const struct adl_pci7x3x_boardinfo adl_pci7x3x_boards[] = {
+	{
+		.name		= "adl_pci7230",
+		.device		= PCI_DEVICE_ID_PCI7230,
+		.nsubdevs	= 2,
+		.di_nchan	= 16,
+		.do_nchan	= 16,
+	}, {
+		.name		= "adl_pci7233",
+		.device		= PCI_DEVICE_ID_PCI7233,
+		.nsubdevs	= 1,
+		.di_nchan	= 32,
+	}, {
+		.name		= "adl_pci7234",
+		.device		= PCI_DEVICE_ID_PCI7234,
+		.nsubdevs	= 1,
+		.do_nchan	= 32,
+	}, {
+		.name		= "adl_pci7432",
+		.device		= PCI_DEVICE_ID_PCI7432,
+		.nsubdevs	= 2,
+		.di_nchan	= 32,
+		.do_nchan	= 32,
+	}, {
+		.name		= "adl_pci7433",
+		.device		= PCI_DEVICE_ID_PCI7433,
+		.nsubdevs	= 2,
+		.di_nchan	= 64,
+	}, {
+		.name		= "adl_pci7434",
+		.device		= PCI_DEVICE_ID_PCI7434,
+		.nsubdevs	= 2,
+		.do_nchan	= 64,
+	}
+};
+
+static int adl_pci7x3x_do_insn_bits(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	unsigned long reg = (unsigned long)s->private;
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
+
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
+
+		outl(s->state, dev->iobase + reg);
+	}
+
+	/*
+	 * NOTE: The output register is not readable.
+	 * This returned state will not be correct until all the
+	 * outputs have been updated.
+	 */
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int adl_pci7x3x_di_insn_bits(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	unsigned long reg = (unsigned long)s->private;
+
+	data[1] = inl(dev->iobase + reg);
+
+	return insn->n;
+}
+
+static const void *adl_pci7x3x_find_boardinfo(struct comedi_device *dev,
+					      struct pci_dev *pcidev)
+{
+	const struct adl_pci7x3x_boardinfo *board;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(adl_pci7x3x_boards); i++) {
+		board = &adl_pci7x3x_boards[i];
+		if (pcidev->device == board->device)
+			return board;
+	}
+	return NULL;
+}
+
+static int adl_pci7x3x_attach_pci(struct comedi_device *dev,
+				  struct pci_dev *pcidev)
+{
+	const struct adl_pci7x3x_boardinfo *board;
+	struct comedi_subdevice *s;
+	int subdev;
+	int nchan;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	board = adl_pci7x3x_find_boardinfo(dev, pcidev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, 2);
+
+	/*
+	 * One or two subdevices are setup by this driver depending on
+	 * the number of digital inputs and/or outputs provided by the
+	 * board. Each subdevice has a maximum of 32 channels.
+	 *
+	 *	PCI-7230 - 2 subdevices: 0 - 16 input, 1 - 16 output
+	 *	PCI-7233 - 1 subdevice: 0 - 32 input
+	 *	PCI-7234 - 1 subdevice: 0 - 32 output
+	 *	PCI-7432 - 2 subdevices: 0 - 32 input, 1 - 32 output
+	 *	PCI-7433 - 2 subdevices: 0 - 32 input, 1 - 32 input
+	 *	PCI-7434 - 2 subdevices: 0 - 32 output, 1 - 32 output
+	 */
+	ret = comedi_alloc_subdevices(dev, board->nsubdevs);
+	if (ret)
+		return ret;
+
+	subdev = 0;
+
+	if (board->di_nchan) {
+		nchan = min(board->di_nchan, 32);
+
+		s = &dev->subdevices[subdev];
+		/* Isolated digital inputs 0 to 15/31 */
+		s->type		= COMEDI_SUBD_DI;
+		s->subdev_flags	= SDF_READABLE;
+		s->n_chan	= nchan;
+		s->maxdata	= 1;
+		s->insn_bits	= adl_pci7x3x_di_insn_bits;
+		s->range_table	= &range_digital;
+
+		s->private	= (void *)PCI7X3X_DIO_REG;
+
+		subdev++;
+
+		nchan = board->di_nchan - nchan;
+		if (nchan) {
+			s = &dev->subdevices[subdev];
+			/* Isolated digital inputs 32 to 63 */
+			s->type		= COMEDI_SUBD_DI;
+			s->subdev_flags	= SDF_READABLE;
+			s->n_chan	= nchan;
+			s->maxdata	= 1;
+			s->insn_bits	= adl_pci7x3x_di_insn_bits;
+			s->range_table	= &range_digital;
+
+			s->private	= (void *)PCI743X_DIO_REG;
+
+			subdev++;
+		}
+	}
+
+	if (board->do_nchan) {
+		nchan = min(board->do_nchan, 32);
+
+		s = &dev->subdevices[subdev];
+		/* Isolated digital outputs 0 to 15/31 */
+		s->type		= COMEDI_SUBD_DO;
+		s->subdev_flags	= SDF_WRITABLE;
+		s->n_chan	= nchan;
+		s->maxdata	= 1;
+		s->insn_bits	= adl_pci7x3x_do_insn_bits;
+		s->range_table	= &range_digital;
+
+		s->private	= (void *)PCI7X3X_DIO_REG;
+
+		subdev++;
+
+		nchan = board->do_nchan - nchan;
+		if (nchan) {
+			s = &dev->subdevices[subdev];
+			/* Isolated digital outputs 32 to 63 */
+			s->type		= COMEDI_SUBD_DO;
+			s->subdev_flags	= SDF_WRITABLE;
+			s->n_chan	= nchan;
+			s->maxdata	= 1;
+			s->insn_bits	= adl_pci7x3x_do_insn_bits;
+			s->range_table	= &range_digital;
+
+			s->private	= (void *)PCI743X_DIO_REG;
+
+			subdev++;
+		}
+	}
+
+	dev_info(dev->class_dev, "%s attached (%d inputs/%d outputs)\n",
+		dev->board_name, board->di_nchan, board->do_nchan);
+
+	return 0;
+}
+
+static void adl_pci7x3x_detach(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+	}
+}
+
+static struct comedi_driver adl_pci7x3x_driver = {
+	.driver_name	= "adl_pci7x3x",
+	.module		= THIS_MODULE,
+	.attach_pci	= adl_pci7x3x_attach_pci,
+	.detach		= adl_pci7x3x_detach,
+};
+
+static int __devinit adl_pci7x3x_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &adl_pci7x3x_driver);
+}
+
+static void __devexit adl_pci7x3x_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(adl_pci7x3x_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7233) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7234) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7433) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7434) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, adl_pci7x3x_pci_table);
+
+static struct pci_driver adl_pci7x3x_pci_driver = {
+	.name		= "adl_pci7x3x",
+	.id_table	= adl_pci7x3x_pci_table,
+	.probe		= adl_pci7x3x_pci_probe,
+	.remove		= __devexit_p(adl_pci7x3x_pci_remove),
+};
+module_comedi_pci_driver(adl_pci7x3x_driver, adl_pci7x3x_pci_driver);
+
+MODULE_DESCRIPTION("ADLINK PCI-723x/743x Isolated Digital I/O boards");
+MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index 247ef00a7c6c..05e06e7ba9f7 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -27,11 +27,7 @@ Author: Michel Lachaine <mike@mikelachaine.ca>
 Status: experimental
 Updated: Mon, 14 Apr 2008 15:10:32 +0100
 
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+Configuration Options: not applicable, uses PCI auto config
 */
 
 #include "../comedidev.h"
@@ -216,61 +212,26 @@ static int adl_pci8164_insn_write_buf1(struct comedi_device *dev,
 	return 2;
 }
 
-static struct pci_dev *adl_pci8164_find_pci(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
+static int adl_pci8164_attach_pci(struct comedi_device *dev,
+				  struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
-		    pcidev->device != PCI_DEVICE_ID_PCI8164)
-			continue;
-		if (bus || slot) {
-			/* requested particular bus/slot */
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		return pcidev;
-	}
-	printk(KERN_ERR
-		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-		dev->minor, bus, slot);
-	return NULL;
-}
-
-static int adl_pci8164_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret;
 
-	printk(KERN_INFO "comedi: attempt to attach...\n");
-	printk(KERN_INFO "comedi%d: adl_pci8164\n", dev->minor);
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	dev->board_name = "pci8164";
+	dev->board_name = dev->driver->driver_name;
 
-	ret = comedi_alloc_subdevices(dev, 4);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
 	if (ret)
 		return ret;
-
-	pcidev = adl_pci8164_find_pci(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	if (comedi_pci_enable(pcidev, "adl_pci8164") < 0) {
-		printk(KERN_ERR "comedi%d: Failed to enable "
-		"PCI device and request regions\n", dev->minor);
-		return -EIO;
-	}
 	dev->iobase = pci_resource_start(pcidev, 2);
-	printk(KERN_DEBUG "comedi: base addr %4lx\n", dev->iobase);
 
-	s = dev->subdevices + 0;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_PROC;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 4;
@@ -280,7 +241,7 @@ static int adl_pci8164_attach(struct comedi_device *dev,
 	s->insn_read = adl_pci8164_insn_read_msts;
 	s->insn_write = adl_pci8164_insn_write_cmd;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_PROC;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 4;
@@ -290,7 +251,7 @@ static int adl_pci8164_attach(struct comedi_device *dev,
 	s->insn_read = adl_pci8164_insn_read_ssts;
 	s->insn_write = adl_pci8164_insn_write_otp;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_PROC;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 4;
@@ -300,7 +261,7 @@ static int adl_pci8164_attach(struct comedi_device *dev,
 	s->insn_read = adl_pci8164_insn_read_buf0;
 	s->insn_write = adl_pci8164_insn_write_buf0;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_PROC;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 4;
@@ -310,7 +271,8 @@ static int adl_pci8164_attach(struct comedi_device *dev,
 	s->insn_read = adl_pci8164_insn_read_buf1;
 	s->insn_write = adl_pci8164_insn_write_buf1;
 
-	printk(KERN_INFO "comedi: attached\n");
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+
 	return 0;
 }
 
@@ -321,14 +283,13 @@ static void adl_pci8164_detach(struct comedi_device *dev)
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver adl_pci8164_driver = {
 	.driver_name	= "adl_pci8164",
 	.module		= THIS_MODULE,
-	.attach		= adl_pci8164_attach,
+	.attach_pci	= adl_pci8164_attach_pci,
 	.detach		= adl_pci8164_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index a31dae6e07df..a87192ac2846 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -47,14 +47,7 @@ Supports:
 The scanned channels must be consecutive and start from 0. They must
 all have the same range and aref.
 
-Configuration options:
-
-	[0] - PCI bus number (optional)
-	[1] - PCI slot number (optional)
-
-If bus/slot is not specified, the first available PCI
-device will be used.
-
+Configuration options: not applicable, uses PCI auto config
 */
 
 /*
@@ -86,29 +79,9 @@ TODO:
 #define PCI9111_DRIVER_NAME	"adl_pci9111"
 #define PCI9111_HR_DEVICE_ID	0x9111
 
-/*  TODO: Add other pci9111 board id */
-
-#define PCI9111_IO_RANGE	0x0100
-
 #define PCI9111_FIFO_HALF_SIZE	512
 
-#define PCI9111_AI_CHANNEL_NBR			16
-
-#define PCI9111_AI_RESOLUTION			12
-#define PCI9111_AI_RESOLUTION_MASK		0x0FFF
-#define PCI9111_AI_RESOLUTION_2_CMP_BIT		0x0800
-
-#define PCI9111_HR_AI_RESOLUTION		16
-#define PCI9111_HR_AI_RESOLUTION_MASK		0xFFFF
-#define PCI9111_HR_AI_RESOLUTION_2_CMP_BIT	0x8000
-
 #define PCI9111_AI_ACQUISITION_PERIOD_MIN_NS	10000
-#define PCI9111_AO_CHANNEL_NBR			1
-#define	PCI9111_AO_RESOLUTION			12
-#define PCI9111_AO_RESOLUTION_MASK		0x0FFF
-#define PCI9111_DI_CHANNEL_NBR			16
-#define	PCI9111_DO_CHANNEL_NBR			16
-#define PCI9111_DO_MASK				0xFFFF
 
 #define PCI9111_RANGE_SETTING_DELAY		10
 #define PCI9111_AI_INSTANT_READ_UDELAY_US	2
@@ -116,233 +89,49 @@ TODO:
 
 #define PCI9111_8254_CLOCK_PERIOD_NS		500
 
-#define PCI9111_8254_COUNTER_0			0x00
-#define PCI9111_8254_COUNTER_1			0x40
-#define PCI9111_8254_COUNTER_2			0x80
-#define PCI9111_8254_COUNTER_LATCH		0x00
-#define PCI9111_8254_READ_LOAD_LSB_ONLY		0x10
-#define PCI9111_8254_READ_LOAD_MSB_ONLY		0x20
-#define PCI9111_8254_READ_LOAD_LSB_MSB		0x30
-#define PCI9111_8254_MODE_0			0x00
-#define PCI9111_8254_MODE_1			0x02
-#define PCI9111_8254_MODE_2			0x04
-#define PCI9111_8254_MODE_3			0x06
-#define PCI9111_8254_MODE_4			0x08
-#define PCI9111_8254_MODE_5			0x0A
-#define PCI9111_8254_BINARY_COUNTER		0x00
-#define PCI9111_8254_BCD_COUNTER		0x01
-
-/* IO address map */
-
-#define PCI9111_REGISTER_AD_FIFO_VALUE			0x00 /* AD Data stored
-								in FIFO */
-#define PCI9111_REGISTER_DA_OUTPUT			0x00
-#define PCI9111_REGISTER_DIGITAL_IO			0x02
-#define PCI9111_REGISTER_EXTENDED_IO_PORTS		0x04
-#define PCI9111_REGISTER_AD_CHANNEL_CONTROL		0x06 /* Channel
-								selection */
-#define PCI9111_REGISTER_AD_CHANNEL_READBACK		0x06
-#define PCI9111_REGISTER_INPUT_SIGNAL_RANGE		0x08
-#define PCI9111_REGISTER_RANGE_STATUS_READBACK		0x08
-#define PCI9111_REGISTER_TRIGGER_MODE_CONTROL		0x0A
-#define PCI9111_REGISTER_AD_MODE_INTERRUPT_READBACK	0x0A
-#define PCI9111_REGISTER_SOFTWARE_TRIGGER		0x0E
-#define PCI9111_REGISTER_INTERRUPT_CONTROL		0x0C
-#define PCI9111_REGISTER_8254_COUNTER_0			0x40
-#define PCI9111_REGISTER_8254_COUNTER_1			0x42
-#define PCI9111_REGISTER_8254_COUNTER_2			0X44
-#define PCI9111_REGISTER_8254_CONTROL			0x46
-#define PCI9111_REGISTER_INTERRUPT_CLEAR		0x48
-
-#define PCI9111_TRIGGER_MASK				0x0F
-#define PCI9111_PTRG_OFF				(0 << 3)
-#define PCI9111_PTRG_ON					(1 << 3)
-#define PCI9111_EITS_EXTERNAL				(1 << 2)
-#define PCI9111_EITS_INTERNAL				(0 << 2)
-#define PCI9111_TPST_SOFTWARE_TRIGGER			(0 << 1)
-#define PCI9111_TPST_TIMER_PACER			(1 << 1)
-#define PCI9111_ASCAN_ON				(1 << 0)
-#define PCI9111_ASCAN_OFF				(0 << 0)
-
-#define PCI9111_ISC0_SET_IRQ_ON_ENDING_OF_AD_CONVERSION (0 << 0)
-#define PCI9111_ISC0_SET_IRQ_ON_FIFO_HALF_FULL		(1 << 0)
-#define PCI9111_ISC1_SET_IRQ_ON_TIMER_TICK		(0 << 1)
-#define PCI9111_ISC1_SET_IRQ_ON_EXT_TRG			(1 << 1)
-#define PCI9111_FFEN_SET_FIFO_ENABLE			(0 << 2)
-#define PCI9111_FFEN_SET_FIFO_DISABLE			(1 << 2)
-
-#define PCI9111_CHANNEL_MASK				0x0F
-
-#define PCI9111_RANGE_MASK				0x07
-#define PCI9111_FIFO_EMPTY_MASK				0x10
-#define PCI9111_FIFO_HALF_FULL_MASK			0x20
-#define PCI9111_FIFO_FULL_MASK				0x40
-#define PCI9111_AD_BUSY_MASK				0x80
-
-#define PCI9111_IO_BASE (dev->iobase)
-
 /*
- * Define inlined function
+ * IO address map and bit defines
  */
-
-#define pci9111_trigger_and_autoscan_get() \
-	(inb(PCI9111_IO_BASE+PCI9111_REGISTER_AD_MODE_INTERRUPT_READBACK)&0x0F)
-
-#define pci9111_trigger_and_autoscan_set(flags) \
-	outb(flags, PCI9111_IO_BASE+PCI9111_REGISTER_TRIGGER_MODE_CONTROL)
-
-#define pci9111_interrupt_and_fifo_get() \
-	((inb(PCI9111_IO_BASE+PCI9111_REGISTER_AD_MODE_INTERRUPT_READBACK) \
-		>> 4) & 0x03)
-
-#define pci9111_interrupt_and_fifo_set(flags) \
-	outb(flags, PCI9111_IO_BASE+PCI9111_REGISTER_INTERRUPT_CONTROL)
-
-#define pci9111_interrupt_clear() \
-	outb(0, PCI9111_IO_BASE+PCI9111_REGISTER_INTERRUPT_CLEAR)
-
-#define pci9111_software_trigger() \
-	outb(0, PCI9111_IO_BASE+PCI9111_REGISTER_SOFTWARE_TRIGGER)
-
-#define pci9111_fifo_reset() do { \
-	outb(PCI9111_FFEN_SET_FIFO_ENABLE, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_INTERRUPT_CONTROL); \
-	outb(PCI9111_FFEN_SET_FIFO_DISABLE, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_INTERRUPT_CONTROL); \
-	outb(PCI9111_FFEN_SET_FIFO_ENABLE, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_INTERRUPT_CONTROL); \
-	} while (0)
-
-#define pci9111_is_fifo_full() \
-	((inb(PCI9111_IO_BASE+PCI9111_REGISTER_RANGE_STATUS_READBACK)& \
-		PCI9111_FIFO_FULL_MASK) == 0)
-
-#define pci9111_is_fifo_half_full() \
-	((inb(PCI9111_IO_BASE+PCI9111_REGISTER_RANGE_STATUS_READBACK)& \
-		PCI9111_FIFO_HALF_FULL_MASK) == 0)
-
-#define pci9111_is_fifo_empty() \
-	((inb(PCI9111_IO_BASE+PCI9111_REGISTER_RANGE_STATUS_READBACK)& \
-		PCI9111_FIFO_EMPTY_MASK) == 0)
-
-#define pci9111_ai_channel_set(channel) \
-	outb((channel)&PCI9111_CHANNEL_MASK, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_AD_CHANNEL_CONTROL)
-
-#define pci9111_ai_channel_get() \
-	(inb(PCI9111_IO_BASE+PCI9111_REGISTER_AD_CHANNEL_READBACK) \
-		&PCI9111_CHANNEL_MASK)
-
-#define pci9111_ai_range_set(range) \
-	outb((range)&PCI9111_RANGE_MASK, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_INPUT_SIGNAL_RANGE)
-
-#define pci9111_ai_range_get() \
-	(inb(PCI9111_IO_BASE+PCI9111_REGISTER_RANGE_STATUS_READBACK) \
-		&PCI9111_RANGE_MASK)
-
-#define pci9111_ai_get_data() \
-	(((inw(PCI9111_IO_BASE+PCI9111_REGISTER_AD_FIFO_VALUE)>>4) \
-		&PCI9111_AI_RESOLUTION_MASK) \
-			^ PCI9111_AI_RESOLUTION_2_CMP_BIT)
-
-#define pci9111_hr_ai_get_data() \
-	((inw(PCI9111_IO_BASE+PCI9111_REGISTER_AD_FIFO_VALUE) \
-		&PCI9111_HR_AI_RESOLUTION_MASK) \
-			^ PCI9111_HR_AI_RESOLUTION_2_CMP_BIT)
-
-#define pci9111_ao_set_data(data) \
-	outw(data&PCI9111_AO_RESOLUTION_MASK, \
-		PCI9111_IO_BASE+PCI9111_REGISTER_DA_OUTPUT)
-
-#define pci9111_di_get_bits() \
-	inw(PCI9111_IO_BASE+PCI9111_REGISTER_DIGITAL_IO)
-
-#define pci9111_do_set_bits(bits) \
-	outw(bits, PCI9111_IO_BASE+PCI9111_REGISTER_DIGITAL_IO)
-
-#define pci9111_8254_control_set(flags) \
-	outb(flags, PCI9111_IO_BASE+PCI9111_REGISTER_8254_CONTROL)
-
-#define pci9111_8254_counter_0_set(data) \
-	do { \
-		outb(data & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_0); \
-		outb((data >> 8) & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_0); \
-	} while (0)
-
-#define pci9111_8254_counter_1_set(data) \
-	do { \
-		outb(data & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_1); \
-		outb((data >> 8) & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_1); \
-	} while (0)
-
-#define pci9111_8254_counter_2_set(data) \
-	do { \
-		outb(data & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_2); \
-		outb((data >> 8) & 0xFF, \
-			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_2); \
-	} while (0)
-
-static const struct comedi_lrange pci9111_hr_ai_range = {
+#define PCI9111_AI_FIFO_REG		0x00
+#define PCI9111_AO_REG			0x00
+#define PCI9111_DIO_REG			0x02
+#define PCI9111_EDIO_REG		0x04
+#define PCI9111_AI_CHANNEL_REG		0x06
+#define PCI9111_AI_RANGE_STAT_REG	0x08
+#define PCI9111_AI_STAT_AD_BUSY		(1 << 7)
+#define PCI9111_AI_STAT_FF_FF		(1 << 6)
+#define PCI9111_AI_STAT_FF_HF		(1 << 5)
+#define PCI9111_AI_STAT_FF_EF		(1 << 4)
+#define PCI9111_AI_RANGE_MASK		(7 << 0)
+#define PCI9111_AI_TRIG_CTRL_REG	0x0a
+#define PCI9111_AI_TRIG_CTRL_TRGEVENT	(1 << 5)
+#define PCI9111_AI_TRIG_CTRL_POTRG	(1 << 4)
+#define PCI9111_AI_TRIG_CTRL_PTRG	(1 << 3)
+#define PCI9111_AI_TRIG_CTRL_ETIS	(1 << 2)
+#define PCI9111_AI_TRIG_CTRL_TPST	(1 << 1)
+#define PCI9111_AI_TRIG_CTRL_ASCAN	(1 << 0)
+#define PCI9111_INT_CTRL_REG		0x0c
+#define PCI9111_INT_CTRL_ISC2		(1 << 3)
+#define PCI9111_INT_CTRL_FFEN		(1 << 2)
+#define PCI9111_INT_CTRL_ISC1		(1 << 1)
+#define PCI9111_INT_CTRL_ISC0		(1 << 0)
+#define PCI9111_SOFT_TRIG_REG		0x0e
+#define PCI9111_8254_BASE_REG		0x40
+#define PCI9111_INT_CLR_REG		0x48
+
+static const struct comedi_lrange pci9111_ai_range = {
 	5,
 	{
-	 BIP_RANGE(10),
-	 BIP_RANGE(5),
-	 BIP_RANGE(2.5),
-	 BIP_RANGE(1.25),
-	 BIP_RANGE(0.625)
-	 }
-};
-
-/*  */
-/*  Board specification structure */
-/*  */
-
-struct pci9111_board {
-	const char *name;	/*  driver name */
-	int device_id;
-	int ai_channel_nbr;	/*  num of A/D chans */
-	int ao_channel_nbr;	/*  num of D/A chans */
-	int ai_resolution;	/*  resolution of A/D */
-	int ai_resolution_mask;
-	int ao_resolution;	/*  resolution of D/A */
-	int ao_resolution_mask;
-	const struct comedi_lrange *ai_range_list;	/*  rangelist for A/D */
-	const struct comedi_lrange *ao_range_list;	/*  rangelist for D/A */
-	unsigned int ai_acquisition_period_min_ns;
-};
-
-static const struct pci9111_board pci9111_boards[] = {
-	{
-	 .name = "pci9111_hr",
-	 .device_id = PCI9111_HR_DEVICE_ID,
-	 .ai_channel_nbr = PCI9111_AI_CHANNEL_NBR,
-	 .ao_channel_nbr = PCI9111_AO_CHANNEL_NBR,
-	 .ai_resolution = PCI9111_HR_AI_RESOLUTION,
-	 .ai_resolution_mask = PCI9111_HR_AI_RESOLUTION_MASK,
-	 .ao_resolution = PCI9111_AO_RESOLUTION,
-	 .ao_resolution_mask = PCI9111_AO_RESOLUTION_MASK,
-	 .ai_range_list = &pci9111_hr_ai_range,
-	 .ao_range_list = &range_bipolar10,
-	 .ai_acquisition_period_min_ns = PCI9111_AI_ACQUISITION_PERIOD_MIN_NS}
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25),
+		BIP_RANGE(0.625)
+	}
 };
 
-#define pci9111_board_nbr \
-	(sizeof(pci9111_boards)/sizeof(struct pci9111_board))
-
-/*  Private data structure */
-
 struct pci9111_private_data {
-	unsigned long io_range;	/*  PCI6503 io range */
-
-	unsigned long lcr_io_base; /* Local configuration register base
-				    * address */
-	unsigned long lcr_io_range;
+	unsigned long lcr_io_base;
 
 	int stop_counter;
 	int stop_is_none;
@@ -352,23 +141,14 @@ struct pci9111_private_data {
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
 
-	int ao_readback;	/*  Last written analog output data */
+	int ao_readback;
 
-	unsigned int timer_divisor_1; /* Divisor values for the 8254 timer
-				       * pacer */
-	unsigned int timer_divisor_2;
-
-	int is_valid;		/*  Is device valid */
+	unsigned int div1;
+	unsigned int div2;
 
 	short ai_bounce_buffer[2 * PCI9111_FIFO_HALF_SIZE];
 };
 
-#define dev_private	((struct pci9111_private_data *)dev->private)
-
-/*  ------------------------------------------------------------------ */
-/*  PLX9050 SECTION */
-/*  ------------------------------------------------------------------ */
-
 #define PLX9050_REGISTER_INTERRUPT_CONTROL 0x4c
 
 #define PLX9050_LINTI1_ENABLE		(1 << 0)
@@ -404,33 +184,19 @@ static void plx9050_interrupt_control(unsigned long io_base,
 	outb(flags, io_base + PLX9050_REGISTER_INTERRUPT_CONTROL);
 }
 
-/*  ------------------------------------------------------------------ */
-/*  MISCELLANEOUS SECTION */
-/*  ------------------------------------------------------------------ */
-
-/*  8254 timer */
-
 static void pci9111_timer_set(struct comedi_device *dev)
 {
-	pci9111_8254_control_set(PCI9111_8254_COUNTER_0 |
-				 PCI9111_8254_READ_LOAD_LSB_MSB |
-				 PCI9111_8254_MODE_0 |
-				 PCI9111_8254_BINARY_COUNTER);
+	struct pci9111_private_data *dev_private = dev->private;
+	unsigned long timer_base = dev->iobase + PCI9111_8254_BASE_REG;
 
-	pci9111_8254_control_set(PCI9111_8254_COUNTER_1 |
-				 PCI9111_8254_READ_LOAD_LSB_MSB |
-				 PCI9111_8254_MODE_2 |
-				 PCI9111_8254_BINARY_COUNTER);
-
-	pci9111_8254_control_set(PCI9111_8254_COUNTER_2 |
-				 PCI9111_8254_READ_LOAD_LSB_MSB |
-				 PCI9111_8254_MODE_2 |
-				 PCI9111_8254_BINARY_COUNTER);
+	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);
 
-	pci9111_8254_counter_2_set(dev_private->timer_divisor_2);
-	pci9111_8254_counter_1_set(dev_private->timer_divisor_1);
+	i8254_write(timer_base, 1, 2, dev_private->div2);
+	i8254_write(timer_base, 1, 1, dev_private->div1);
 }
 
 enum pci9111_trigger_sources {
@@ -444,47 +210,55 @@ static void pci9111_trigger_source_set(struct comedi_device *dev,
 {
 	int flags;
 
-	flags = pci9111_trigger_and_autoscan_get() & 0x09;
+	/* Read the current trigger mode control bits */
+	flags = inb(dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
+	/* Mask off the EITS and TPST bits */
+	flags &= 0x9;
 
 	switch (source) {
 	case software:
-		flags |= PCI9111_EITS_INTERNAL | PCI9111_TPST_SOFTWARE_TRIGGER;
 		break;
 
 	case timer_pacer:
-		flags |= PCI9111_EITS_INTERNAL | PCI9111_TPST_TIMER_PACER;
+		flags |= PCI9111_AI_TRIG_CTRL_TPST;
 		break;
 
 	case external:
-		flags |= PCI9111_EITS_EXTERNAL;
+		flags |= PCI9111_AI_TRIG_CTRL_ETIS;
 		break;
 	}
 
-	pci9111_trigger_and_autoscan_set(flags);
+	outb(flags, dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
 }
 
 static void pci9111_pretrigger_set(struct comedi_device *dev, bool pretrigger)
 {
 	int flags;
 
-	flags = pci9111_trigger_and_autoscan_get() & 0x07;
+	/* Read the current trigger mode control bits */
+	flags = inb(dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
+	/* Mask off the PTRG bit */
+	flags &= 0x7;
 
 	if (pretrigger)
-		flags |= PCI9111_PTRG_ON;
+		flags |= PCI9111_AI_TRIG_CTRL_PTRG;
 
-	pci9111_trigger_and_autoscan_set(flags);
+	outb(flags, dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
 }
 
 static void pci9111_autoscan_set(struct comedi_device *dev, bool autoscan)
 {
 	int flags;
 
-	flags = pci9111_trigger_and_autoscan_get() & 0x0e;
+	/* Read the current trigger mode control bits */
+	flags = inb(dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
+	/* Mask off the ASCAN bit */
+	flags &= 0xe;
 
 	if (autoscan)
-		flags |= PCI9111_ASCAN_ON;
+		flags |= PCI9111_AI_TRIG_CTRL_ASCAN;
 
-	pci9111_trigger_and_autoscan_set(flags);
+	outb(flags, dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
 }
 
 enum pci9111_ISC0_sources {
@@ -503,30 +277,39 @@ static void pci9111_interrupt_source_set(struct comedi_device *dev,
 {
 	int flags;
 
-	flags = pci9111_interrupt_and_fifo_get() & 0x04;
+	/* Read the current interrupt control bits */
+	flags = inb(dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
+	/* Shift the bits so they are compatible with the write register */
+	flags >>= 4;
+	/* Mask off the ISCx bits */
+	flags &= 0xc0;
 
+	/* Now set the new ISCx bits */
 	if (irq_0_source == irq_on_fifo_half_full)
-		flags |= PCI9111_ISC0_SET_IRQ_ON_FIFO_HALF_FULL;
+		flags |= PCI9111_INT_CTRL_ISC0;
 
 	if (irq_1_source == irq_on_external_trigger)
-		flags |= PCI9111_ISC1_SET_IRQ_ON_EXT_TRG;
+		flags |= PCI9111_INT_CTRL_ISC1;
 
-	pci9111_interrupt_and_fifo_set(flags);
+	outb(flags, dev->iobase + PCI9111_INT_CTRL_REG);
 }
 
-/*  ------------------------------------------------------------------ */
-/*  HARDWARE TRIGGERED ANALOG INPUT SECTION */
-/*  ------------------------------------------------------------------ */
-
-/*  Cancel analog input autoscan */
+static void pci9111_fifo_reset(struct comedi_device *dev)
+{
+	unsigned long int_ctrl_reg = dev->iobase + PCI9111_INT_CTRL_REG;
 
-#undef AI_DO_CMD_DEBUG
+	/* To reset the FIFO, set FFEN sequence as 0 -> 1 -> 0 */
+	outb(0, int_ctrl_reg);
+	outb(PCI9111_INT_CTRL_FFEN, int_ctrl_reg);
+	outb(0, int_ctrl_reg);
+}
 
 static int pci9111_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
-	/*  Disable interrupts */
+	struct pci9111_private_data *dev_private = dev->private;
 
+	/*  Disable interrupts */
 	plx9050_interrupt_control(dev_private->lcr_io_base, true, true, true,
 				  true, false);
 
@@ -534,97 +317,65 @@ static int pci9111_ai_cancel(struct comedi_device *dev,
 
 	pci9111_autoscan_set(dev, false);
 
-	pci9111_fifo_reset();
-
-#ifdef AI_DO_CMD_DEBUG
-	printk(PCI9111_DRIVER_NAME ": ai_cancel\n");
-#endif
+	pci9111_fifo_reset(dev);
 
 	return 0;
 }
 
-/*  Test analog input command */
-
-#define pci9111_check_trigger_src(src, flags)	do {			\
-		tmp = src;						\
-		src &= flags;						\
-		if (!src || tmp != src)					\
-			error++;					\
-	} while (false);
-
-static int
-pci9111_ai_do_cmd_test(struct comedi_device *dev,
-		       struct comedi_subdevice *s, struct comedi_cmd *cmd)
+static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_cmd *cmd)
 {
+	struct pci9111_private_data *dev_private = dev->private;
 	int tmp;
 	int error = 0;
 	int range, reference;
 	int i;
-	struct pci9111_board *board = (struct pci9111_board *)dev->board_ptr;
 
-	/*  Step 1 : check if trigger are trivialy valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	pci9111_check_trigger_src(cmd->start_src, TRIG_NOW);
-	pci9111_check_trigger_src(cmd->scan_begin_src,
-				  TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	pci9111_check_trigger_src(cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	pci9111_check_trigger_src(cmd->scan_end_src, TRIG_COUNT);
-	pci9111_check_trigger_src(cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+	error |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	error |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
+	error |= cfc_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	error |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	error |= cfc_check_trigger_src(&cmd->stop_src,
+					TRIG_COUNT | TRIG_NONE);
 
 	if (error)
 		return 1;
 
-	/*  step 2 : make sure trigger sources are unique and mutually
-	 *  compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW)
-		error++;
+	error |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	error |= cfc_check_trigger_is_unique(cmd->convert_src);
+	error |= cfc_check_trigger_is_unique(cmd->stop_src);
 
-	if ((cmd->scan_begin_src != TRIG_TIMER) &&
-	    (cmd->scan_begin_src != TRIG_FOLLOW) &&
-	    (cmd->scan_begin_src != TRIG_EXT))
-		error++;
+	/* Step 2b : and mutually compatible */
 
-	if ((cmd->convert_src != TRIG_TIMER) && (cmd->convert_src != TRIG_EXT))
-		error++;
 	if ((cmd->convert_src == TRIG_TIMER) &&
 	    !((cmd->scan_begin_src == TRIG_TIMER) ||
 	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error++;
+		error |= -EINVAL;
 	if ((cmd->convert_src == TRIG_EXT) &&
 	    !((cmd->scan_begin_src == TRIG_EXT) ||
 	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error++;
-
-
-	if (cmd->scan_end_src != TRIG_COUNT)
-		error++;
-	if ((cmd->stop_src != TRIG_COUNT) && (cmd->stop_src != TRIG_NONE))
-		error++;
+		error |= -EINVAL;
 
 	if (error)
 		return 2;
 
 	/*  Step 3 : make sure arguments are trivialy compatible */
 
-	if (cmd->chanlist_len < 1) {
-		cmd->chanlist_len = 1;
-		error++;
-	}
-
-	if (cmd->chanlist_len > board->ai_channel_nbr) {
-		cmd->chanlist_len = board->ai_channel_nbr;
-		error++;
-	}
-
 	if ((cmd->start_src == TRIG_NOW) && (cmd->start_arg != 0)) {
 		cmd->start_arg = 0;
 		error++;
 	}
 
 	if ((cmd->convert_src == TRIG_TIMER) &&
-	    (cmd->convert_arg < board->ai_acquisition_period_min_ns)) {
-		cmd->convert_arg = board->ai_acquisition_period_min_ns;
+	    (cmd->convert_arg < PCI9111_AI_ACQUISITION_PERIOD_MIN_NS)) {
+		cmd->convert_arg = PCI9111_AI_ACQUISITION_PERIOD_MIN_NS;
 		error++;
 	}
 	if ((cmd->convert_src == TRIG_EXT) && (cmd->convert_arg != 0)) {
@@ -633,8 +384,8 @@ pci9111_ai_do_cmd_test(struct comedi_device *dev,
 	}
 
 	if ((cmd->scan_begin_src == TRIG_TIMER) &&
-	    (cmd->scan_begin_arg < board->ai_acquisition_period_min_ns)) {
-		cmd->scan_begin_arg = board->ai_acquisition_period_min_ns;
+	    (cmd->scan_begin_arg < PCI9111_AI_ACQUISITION_PERIOD_MIN_NS)) {
+		cmd->scan_begin_arg = PCI9111_AI_ACQUISITION_PERIOD_MIN_NS;
 		error++;
 	}
 	if ((cmd->scan_begin_src == TRIG_FOLLOW)
@@ -670,9 +421,9 @@ pci9111_ai_do_cmd_test(struct comedi_device *dev,
 	if (cmd->convert_src == TRIG_TIMER) {
 		tmp = cmd->convert_arg;
 		i8253_cascade_ns_to_timer_2div(PCI9111_8254_CLOCK_PERIOD_NS,
-					       &(dev_private->timer_divisor_1),
-					       &(dev_private->timer_divisor_2),
-					       &(cmd->convert_arg),
+					       &dev_private->div1,
+					       &dev_private->div2,
+					       &cmd->convert_arg,
 					       cmd->flags & TRIG_ROUND_MASK);
 		if (tmp != cmd->convert_arg)
 			error++;
@@ -733,14 +484,6 @@ pci9111_ai_do_cmd_test(struct comedi_device *dev,
 					error++;
 				}
 			}
-		} else {
-			if ((CR_CHAN(cmd->chanlist[0]) >
-			     (board->ai_channel_nbr - 1))
-			    || (CR_CHAN(cmd->chanlist[0]) < 0)) {
-				comedi_error(dev,
-					     "channel number is out of limits\n");
-				error++;
-			}
 		}
 	}
 
@@ -751,12 +494,11 @@ pci9111_ai_do_cmd_test(struct comedi_device *dev,
 
 }
 
-/*  Analog input command */
-
 static int pci9111_ai_do_cmd(struct comedi_device *dev,
-			     struct comedi_subdevice *subdevice)
+			     struct comedi_subdevice *s)
 {
-	struct comedi_cmd *async_cmd = &subdevice->async->cmd;
+	struct pci9111_private_data *dev_private = dev->private;
+	struct comedi_cmd *async_cmd = &s->async->cmd;
 
 	if (!dev->irq) {
 		comedi_error(dev,
@@ -768,17 +510,20 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	/*  TODO: handle the case of an external multiplexer */
 
 	if (async_cmd->chanlist_len > 1) {
-		pci9111_ai_channel_set((async_cmd->chanlist_len) - 1);
+		outb(async_cmd->chanlist_len - 1,
+			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, true);
 	} else {
-		pci9111_ai_channel_set(CR_CHAN(async_cmd->chanlist[0]));
+		outb(CR_CHAN(async_cmd->chanlist[0]),
+			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, false);
 	}
 
 	/*  Set gain */
 	/*  This is the same gain on every channel */
 
-	pci9111_ai_range_set(CR_RANGE(async_cmd->chanlist[0]));
+	outb(CR_RANGE(async_cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
+		dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 
 	/* Set counter */
 
@@ -804,21 +549,9 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	dev_private->scan_delay = 0;
 	switch (async_cmd->convert_src) {
 	case TRIG_TIMER:
-		i8253_cascade_ns_to_timer_2div(PCI9111_8254_CLOCK_PERIOD_NS,
-					       &(dev_private->timer_divisor_1),
-					       &(dev_private->timer_divisor_2),
-					       &(async_cmd->convert_arg),
-					       async_cmd->
-					       flags & TRIG_ROUND_MASK);
-#ifdef AI_DO_CMD_DEBUG
-		printk(PCI9111_DRIVER_NAME ": divisors = %d, %d\n",
-		       dev_private->timer_divisor_1,
-		       dev_private->timer_divisor_2);
-#endif
-
 		pci9111_trigger_source_set(dev, software);
 		pci9111_timer_set(dev);
-		pci9111_fifo_reset();
+		pci9111_fifo_reset(dev);
 		pci9111_interrupt_source_set(dev, irq_on_fifo_half_full,
 					     irq_on_timer_tick);
 		pci9111_trigger_source_set(dev, timer_pacer);
@@ -837,7 +570,7 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	case TRIG_EXT:
 
 		pci9111_trigger_source_set(dev, external);
-		pci9111_fifo_reset();
+		pci9111_fifo_reset(dev);
 		pci9111_interrupt_source_set(dev, irq_on_fifo_half_full,
 					     irq_on_timer_tick);
 		plx9050_interrupt_control(dev_private->lcr_io_base, true, true,
@@ -856,23 +589,6 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	dev_private->chunk_num_samples =
 	    dev_private->chanlist_len * (1 + dev_private->scan_delay);
 
-#ifdef AI_DO_CMD_DEBUG
-	printk(PCI9111_DRIVER_NAME ": start interruptions!\n");
-	printk(PCI9111_DRIVER_NAME ": trigger source = %2x\n",
-	       pci9111_trigger_and_autoscan_get());
-	printk(PCI9111_DRIVER_NAME ": irq source     = %2x\n",
-	       pci9111_interrupt_and_fifo_get());
-	printk(PCI9111_DRIVER_NAME ": ai_do_cmd\n");
-	printk(PCI9111_DRIVER_NAME ": stop counter   = %d\n",
-	       dev_private->stop_counter);
-	printk(PCI9111_DRIVER_NAME ": scan delay     = %d\n",
-	       dev_private->scan_delay);
-	printk(PCI9111_DRIVER_NAME ": chanlist_len   = %d\n",
-	       dev_private->chanlist_len);
-	printk(PCI9111_DRIVER_NAME ": chunk num samples = %d\n",
-	       dev_private->chunk_num_samples);
-#endif
-
 	return 0;
 }
 
@@ -881,34 +597,24 @@ static void pci9111_ai_munge(struct comedi_device *dev,
 			     unsigned int num_bytes,
 			     unsigned int start_chan_index)
 {
-	unsigned int i, num_samples = num_bytes / sizeof(short);
 	short *array = data;
-	int resolution =
-	    ((struct pci9111_board *)dev->board_ptr)->ai_resolution;
-
-	for (i = 0; i < num_samples; i++) {
-		if (resolution == PCI9111_HR_AI_RESOLUTION)
-			array[i] =
-			    (array[i] & PCI9111_HR_AI_RESOLUTION_MASK) ^
-			    PCI9111_HR_AI_RESOLUTION_2_CMP_BIT;
-		else
-			array[i] =
-			    ((array[i] >> 4) & PCI9111_AI_RESOLUTION_MASK) ^
-			    PCI9111_AI_RESOLUTION_2_CMP_BIT;
-	}
+	unsigned int maxdata = s->maxdata;
+	unsigned int invert = (maxdata + 1) >> 1;
+	unsigned int shift = (maxdata == 0xffff) ? 0 : 4;
+	unsigned int num_samples = num_bytes / sizeof(short);
+	unsigned int i;
+
+	for (i = 0; i < num_samples; i++)
+		array[i] = ((array[i] >> shift) & maxdata) ^ invert;
 }
 
-/*  ------------------------------------------------------------------ */
-/*  INTERRUPT SECTION */
-/*  ------------------------------------------------------------------ */
-
-#undef INTERRUPT_DEBUG
-
 static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 {
 	struct comedi_device *dev = p_device;
-	struct comedi_subdevice *subdevice = dev->read_subdev;
+	struct pci9111_private_data *dev_private = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
+	unsigned int status;
 	unsigned long irq_flags;
 	unsigned char intcsr;
 
@@ -918,7 +624,7 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 		return IRQ_NONE;
 	}
 
-	async = subdevice->async;
+	async = s->async;
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 
@@ -940,37 +646,37 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 	    (PLX9050_LINTI1_ENABLE | PLX9050_LINTI1_STATUS)) {
 		/*  Interrupt comes from fifo_half-full signal */
 
-		if (pci9111_is_fifo_full()) {
+		status = inb(dev->iobase + PCI9111_AI_RANGE_STAT_REG);
+
+		/* '0' means FIFO is full, data may have been lost */
+		if (!(status & PCI9111_AI_STAT_FF_FF)) {
 			spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 			comedi_error(dev, PCI9111_DRIVER_NAME " fifo overflow");
-			pci9111_interrupt_clear();
-			pci9111_ai_cancel(dev, subdevice);
+			outb(0, dev->iobase + PCI9111_INT_CLR_REG);
+			pci9111_ai_cancel(dev, s);
 			async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-			comedi_event(dev, subdevice);
+			comedi_event(dev, s);
 
 			return IRQ_HANDLED;
 		}
 
-		if (pci9111_is_fifo_half_full()) {
+		/* '0' means FIFO is half-full */
+		if (!(status & PCI9111_AI_STAT_FF_HF)) {
 			unsigned int num_samples;
 			unsigned int bytes_written = 0;
 
-#ifdef INTERRUPT_DEBUG
-			printk(PCI9111_DRIVER_NAME ": fifo is half full\n");
-#endif
-
 			num_samples =
 			    PCI9111_FIFO_HALF_SIZE >
 			    dev_private->stop_counter
 			    && !dev_private->
 			    stop_is_none ? dev_private->stop_counter :
 			    PCI9111_FIFO_HALF_SIZE;
-			insw(PCI9111_IO_BASE + PCI9111_REGISTER_AD_FIFO_VALUE,
+			insw(dev->iobase + PCI9111_AI_FIFO_REG,
 			     dev_private->ai_bounce_buffer, num_samples);
 
 			if (dev_private->scan_delay < 1) {
 				bytes_written =
-				    cfc_write_array_to_buffer(subdevice,
+				    cfc_write_array_to_buffer(s,
 							      dev_private->
 							      ai_bounce_buffer,
 							      num_samples *
@@ -994,7 +700,7 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 
 						bytes_written +=
 						    cfc_write_array_to_buffer
-						    (subdevice,
+						    (s,
 						     dev_private->ai_bounce_buffer
 						     + position,
 						     to_read * sizeof(short));
@@ -1029,168 +735,135 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 
 	if ((dev_private->stop_counter == 0) && (!dev_private->stop_is_none)) {
 		async->events |= COMEDI_CB_EOA;
-		pci9111_ai_cancel(dev, subdevice);
+		pci9111_ai_cancel(dev, s);
 	}
 
-	/* Very important, otherwise another interrupt request will be inserted
-	 * and will cause driver hangs on processing interrupt event. */
-
-	pci9111_interrupt_clear();
+	outb(0, dev->iobase + PCI9111_INT_CLR_REG);
 
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
-	comedi_event(dev, subdevice);
+	comedi_event(dev, s);
 
 	return IRQ_HANDLED;
 }
 
-/*  ------------------------------------------------------------------ */
-/*  INSTANT ANALOG INPUT OUTPUT SECTION */
-/*  ------------------------------------------------------------------ */
-
-/*  analog instant input */
-
-#undef AI_INSN_DEBUG
-
 static int pci9111_ai_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *subdevice,
+				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	int resolution =
-	    ((struct pci9111_board *)dev->board_ptr)->ai_resolution;
-
-	int timeout, i;
-
-#ifdef AI_INSN_DEBUG
-	printk(PCI9111_DRIVER_NAME ": ai_insn set c/r/n = %2x/%2x/%2x\n",
-	       CR_CHAN((&insn->chanspec)[0]),
-	       CR_RANGE((&insn->chanspec)[0]), insn->n);
-#endif
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int maxdata = s->maxdata;
+	unsigned int invert = (maxdata + 1) >> 1;
+	unsigned int shift = (maxdata == 0xffff) ? 0 : 4;
+	unsigned int status;
+	int timeout;
+	int i;
 
-	pci9111_ai_channel_set(CR_CHAN((&insn->chanspec)[0]));
+	outb(chan, dev->iobase + PCI9111_AI_CHANNEL_REG);
 
-	if ((pci9111_ai_range_get()) != CR_RANGE((&insn->chanspec)[0]))
-		pci9111_ai_range_set(CR_RANGE((&insn->chanspec)[0]));
+	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);
+	}
 
-	pci9111_fifo_reset();
+	pci9111_fifo_reset(dev);
 
 	for (i = 0; i < insn->n; i++) {
-		pci9111_software_trigger();
+		/* Generate a software trigger */
+		outb(0, dev->iobase + PCI9111_SOFT_TRIG_REG);
 
 		timeout = PCI9111_AI_INSTANT_READ_TIMEOUT;
 
 		while (timeout--) {
-			if (!pci9111_is_fifo_empty())
+			status = inb(dev->iobase + PCI9111_AI_RANGE_STAT_REG);
+			/* '1' means FIFO is not empty */
+			if (status & PCI9111_AI_STAT_FF_EF)
 				goto conversion_done;
 		}
 
 		comedi_error(dev, "A/D read timeout");
 		data[i] = 0;
-		pci9111_fifo_reset();
+		pci9111_fifo_reset(dev);
 		return -ETIME;
 
 conversion_done:
 
-		if (resolution == PCI9111_HR_AI_RESOLUTION)
-			data[i] = pci9111_hr_ai_get_data();
-		else
-			data[i] = pci9111_ai_get_data();
+		data[i] = inw(dev->iobase + PCI9111_AI_FIFO_REG);
+		data[i] = ((data[i] >> shift) & maxdata) ^ invert;
 	}
 
-#ifdef AI_INSN_DEBUG
-	printk(PCI9111_DRIVER_NAME ": ai_insn get c/r/t = %2x/%2x/%2x\n",
-	       pci9111_ai_channel_get(),
-	       pci9111_ai_range_get(), pci9111_trigger_and_autoscan_get());
-#endif
-
 	return i;
 }
 
-/*  Analog instant output */
-
-static int
-pci9111_ao_insn_write(struct comedi_device *dev,
-		      struct comedi_subdevice *s, struct comedi_insn *insn,
-		      unsigned int *data)
+static int pci9111_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
+	struct pci9111_private_data *dev_private = dev->private;
+	unsigned int val = 0;
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		pci9111_ao_set_data(data[i]);
-		dev_private->ao_readback = data[i];
+		val = data[i];
+		outw(val, dev->iobase + PCI9111_AO_REG);
 	}
+	dev_private->ao_readback = val;
 
-	return i;
+	return insn->n;
 }
 
-/*  Analog output readback */
-
 static int pci9111_ao_insn_read(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
+	struct pci9111_private_data *dev_private = dev->private;
 	int i;
 
 	for (i = 0; i < insn->n; i++)
-		data[i] = dev_private->ao_readback & PCI9111_AO_RESOLUTION_MASK;
+		data[i] = dev_private->ao_readback;
 
-	return i;
+	return insn->n;
 }
 
-/*  ------------------------------------------------------------------ */
-/*  DIGITAL INPUT OUTPUT SECTION */
-/*  ------------------------------------------------------------------ */
-
-/*  Digital inputs */
-
 static int pci9111_di_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *subdevice,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	unsigned int bits;
-
-	bits = pci9111_di_get_bits();
-	data[1] = bits;
+	data[1] = inw(dev->iobase + PCI9111_DIO_REG);
 
 	return insn->n;
 }
 
-/*  Digital outputs */
-
 static int pci9111_do_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *subdevice,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	unsigned int bits;
-
-	/*  Only set bits that have been masked */
-	/*  data[0] = mask */
-	/*  data[1] = bit state */
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
 
-	data[0] &= PCI9111_DO_MASK;
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
 
-	bits = subdevice->state;
-	bits &= ~data[0];
-	bits |= data[0] & data[1];
-	subdevice->state = bits;
-
-	pci9111_do_set_bits(bits);
+		outw(s->state, dev->iobase + PCI9111_DIO_REG);
+	}
 
-	data[1] = bits;
+	data[1] = s->state;
 
 	return insn->n;
 }
 
-/*  ------------------------------------------------------------------ */
-/*  INITIALISATION SECTION */
-/*  ------------------------------------------------------------------ */
-
-/*  Reset device */
-
 static int pci9111_reset(struct comedi_device *dev)
 {
-	/*  Set trigger source to software */
+	struct pci9111_private_data *dev_private = dev->private;
 
+	/*  Set trigger source to software */
 	plx9050_interrupt_control(dev_private->lcr_io_base, true, true, true,
 				  true, false);
 
@@ -1198,177 +871,90 @@ static int pci9111_reset(struct comedi_device *dev)
 	pci9111_pretrigger_set(dev, false);
 	pci9111_autoscan_set(dev, false);
 
-	/*  Reset 8254 chip */
-
-	dev_private->timer_divisor_1 = 0;
-	dev_private->timer_divisor_2 = 0;
-
+	/* Reset 8254 chip */
+	dev_private->div1 = 0;
+	dev_private->div2 = 0;
 	pci9111_timer_set(dev);
 
 	return 0;
 }
 
-static struct pci_dev *pci9111_find_pci(struct comedi_device *dev,
-					struct comedi_devconfig *it)
+static int pci9111_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK)
-			continue;
-		for (i = 0; i < pci9111_board_nbr; i++) {
-			if (pcidev->device != pci9111_boards[i].device_id)
-				continue;
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus ||
-				    PCI_SLOT(pcidev->devfn) != slot)
-					continue;
-			}
-			dev->board_ptr = pci9111_boards + i;
-			printk(KERN_ERR
-				"comedi%d: found %s (b:s:f=%d:%d:%d), irq=%d\n",
-				dev->minor, pci9111_boards[i].name,
-				pcidev->bus->number, PCI_SLOT(pcidev->devfn),
-				PCI_FUNC(pcidev->devfn), pcidev->irq);
-			return pcidev;
-		}
-	}
-	printk(KERN_ERR
-		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-		dev->minor, bus, slot);
-	return NULL;
-}
+	struct pci9111_private_data *dev_private;
+	struct comedi_subdevice *s;
+	int ret;
 
-static int pci9111_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *subdevice;
-	unsigned long io_base, io_range, lcr_io_base, lcr_io_range;
-	int error;
-	const struct pci9111_board *board;
-
-	if (alloc_private(dev, sizeof(struct pci9111_private_data)) < 0)
-		return -ENOMEM;
-	/*  Probe the device to determine what device in the series it is. */
-
-	printk(KERN_ERR "comedi%d: " PCI9111_DRIVER_NAME " driver\n",
-								dev->minor);
-
-	pcidev = pci9111_find_pci(dev, it);
-	if (!pcidev)
-		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
-	board = (struct pci9111_board *)dev->board_ptr;
-
-	/*  TODO: Warn about non-tested boards. */
-
-	/*  Read local configuration register base address
-	 *  [PCI_BASE_ADDRESS #1]. */
-
-	lcr_io_base = pci_resource_start(pcidev, 1);
-	lcr_io_range = pci_resource_len(pcidev, 1);
-
-	printk
-	    ("comedi%d: local configuration registers at address 0x%4lx [0x%4lx]\n",
-	     dev->minor, lcr_io_base, lcr_io_range);
-
-	/*  Enable PCI device and request regions */
-	if (comedi_pci_enable(pcidev, PCI9111_DRIVER_NAME) < 0) {
-		printk
-		    ("comedi%d: Failed to enable PCI device and request regions\n",
-		     dev->minor);
-		return -EIO;
-	}
-	/*  Read PCI6308 register base address [PCI_BASE_ADDRESS #2]. */
+	dev->board_name = dev->driver->driver_name;
 
-	io_base = pci_resource_start(pcidev, 2);
-	io_range = pci_resource_len(pcidev, 2);
+	ret = alloc_private(dev, sizeof(*dev_private));
+	if (ret)
+		return ret;
+	dev_private = dev->private;
 
-	printk(KERN_ERR "comedi%d: 6503 registers at address 0x%4lx [0x%4lx]\n",
-	       dev->minor, io_base, io_range);
-
-	dev->iobase = io_base;
-	dev->board_name = board->name;
-	dev_private->io_range = io_range;
-	dev_private->is_valid = 0;
-	dev_private->lcr_io_base = lcr_io_base;
-	dev_private->lcr_io_range = lcr_io_range;
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	dev_private->lcr_io_base = pci_resource_start(pcidev, 1);
+	dev->iobase = pci_resource_start(pcidev, 2);
 
 	pci9111_reset(dev);
 
-	/*  Irq setup */
-
-	dev->irq = 0;
 	if (pcidev->irq > 0) {
+		ret = request_irq(dev->irq, pci9111_interrupt,
+				  IRQF_SHARED, dev->board_name, dev);
+		if (ret)
+			return ret;
 		dev->irq = pcidev->irq;
-
-		if (request_irq(dev->irq, pci9111_interrupt,
-				IRQF_SHARED, PCI9111_DRIVER_NAME, dev) != 0) {
-			printk(KERN_ERR
-				"comedi%d: unable to allocate irq  %u\n",
-					dev->minor, dev->irq);
-			return -EINVAL;
-		}
 	}
 
-	/*  TODO: Add external multiplexer setup (according to option[2]). */
-
-	error = comedi_alloc_subdevices(dev, 4);
-	if (error)
-		return error;
-
-	subdevice = dev->subdevices + 0;
-	dev->read_subdev = subdevice;
-
-	subdevice->type = COMEDI_SUBD_AI;
-	subdevice->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
-
-	/*  TODO: Add external multiplexer data */
-	/*     if (devpriv->usemux) { subdevice->n_chan = devpriv->usemux; } */
-	/*     else { subdevice->n_chan = this_board->n_aichan; } */
-
-	subdevice->n_chan = board->ai_channel_nbr;
-	subdevice->maxdata = board->ai_resolution_mask;
-	subdevice->len_chanlist = board->ai_channel_nbr;
-	subdevice->range_table = board->ai_range_list;
-	subdevice->cancel = pci9111_ai_cancel;
-	subdevice->insn_read = pci9111_ai_insn_read;
-	subdevice->do_cmdtest = pci9111_ai_do_cmd_test;
-	subdevice->do_cmd = pci9111_ai_do_cmd;
-	subdevice->munge = pci9111_ai_munge;
-
-	subdevice = dev->subdevices + 1;
-	subdevice->type = COMEDI_SUBD_AO;
-	subdevice->subdev_flags = SDF_WRITABLE | SDF_COMMON;
-	subdevice->n_chan = board->ao_channel_nbr;
-	subdevice->maxdata = board->ao_resolution_mask;
-	subdevice->len_chanlist = board->ao_channel_nbr;
-	subdevice->range_table = board->ao_range_list;
-	subdevice->insn_write = pci9111_ao_insn_write;
-	subdevice->insn_read = pci9111_ao_insn_read;
-
-	subdevice = dev->subdevices + 2;
-	subdevice->type = COMEDI_SUBD_DI;
-	subdevice->subdev_flags = SDF_READABLE;
-	subdevice->n_chan = PCI9111_DI_CHANNEL_NBR;
-	subdevice->maxdata = 1;
-	subdevice->range_table = &range_digital;
-	subdevice->insn_bits = pci9111_di_insn_bits;
-
-	subdevice = dev->subdevices + 3;
-	subdevice->type = COMEDI_SUBD_DO;
-	subdevice->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	subdevice->n_chan = PCI9111_DO_CHANNEL_NBR;
-	subdevice->maxdata = 1;
-	subdevice->range_table = &range_digital;
-	subdevice->insn_bits = pci9111_do_insn_bits;
-
-	dev_private->is_valid = 1;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[0];
+	dev->read_subdev = s;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
+	s->n_chan	= 16;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 16;
+	s->range_table	= &pci9111_ai_range;
+	s->cancel	= pci9111_ai_cancel;
+	s->insn_read	= pci9111_ai_insn_read;
+	s->do_cmdtest	= pci9111_ai_do_cmd_test;
+	s->do_cmd	= pci9111_ai_do_cmd;
+	s->munge	= pci9111_ai_munge;
+
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_COMMON;
+	s->n_chan	= 1;
+	s->maxdata	= 0x0fff;
+	s->len_chanlist	= 1;
+	s->range_table	= &range_bipolar10;
+	s->insn_write	= pci9111_ao_insn_write;
+	s->insn_read	= pci9111_ao_insn_read;
+
+	s = &dev->subdevices[2];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci9111_di_insn_bits;
+
+	s = &dev->subdevices[3];
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci9111_do_insn_bits;
+
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
 	return 0;
 }
@@ -1377,23 +963,20 @@ static void pci9111_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	if (dev->private != NULL) {
-		if (dev_private->is_valid)
-			pci9111_reset(dev);
-	}
+	if (dev->iobase)
+		pci9111_reset(dev);
 	if (dev->irq != 0)
 		free_irq(dev->irq, dev);
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver adl_pci9111_driver = {
 	.driver_name	= "adl_pci9111",
 	.module		= THIS_MODULE,
-	.attach		= pci9111_attach,
+	.attach_pci	= pci9111_attach_pci,
 	.detach		= pci9111_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index a1f74c2590e8..06ff65c85c9f 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -81,18 +81,6 @@ Configuration options:
 				 * correct channel number on every 12 bit sample
 				 */
 
-#undef PCI9118_EXTDEBUG		/*
-				 * if defined then driver prints
-				 * a lot of messages
-				 */
-
-#undef DPRINTK
-#ifdef PCI9118_EXTDEBUG
-#define DPRINTK(fmt, args...) printk(fmt, ## args)
-#else
-#define DPRINTK(fmt, args...)
-#endif
-
 #define IORANGE_9118 	64	/* I hope */
 #define PCI9118_CHANLEN	255	/*
 				 * len of chanlist, some source say 256,
@@ -356,43 +344,170 @@ struct pci9118_private {
 	unsigned int ai_inttrig_start;	/* TRIG_INT for start */
 };
 
-#define devpriv ((struct pci9118_private *)dev->private)
-#define this_board ((struct boardtype *)dev->board_ptr)
-
-/*
-==============================================================================
-*/
-
 static int check_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s, int n_chan,
-			      unsigned int *chanlist, int frontadd,
-			      int backadd);
+			      unsigned int *chanlist, int frontadd, int backadd)
+{
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int i, differencial = 0, bipolar = 0;
+
+	/* correct channel and range number check itself comedi/range.c */
+	if (n_chan < 1) {
+		comedi_error(dev, "range/channel list is empty!");
+		return 0;
+	}
+	if ((frontadd + n_chan + backadd) > s->len_chanlist) {
+		printk
+		    ("comedi%d: range/channel list is too long for "
+						"actual configuration (%d>%d)!",
+		     dev->minor, n_chan, s->len_chanlist - frontadd - backadd);
+		return 0;
+	}
+
+	if (CR_AREF(chanlist[0]) == AREF_DIFF)
+		differencial = 1;	/* all input must be diff */
+	if (CR_RANGE(chanlist[0]) < PCI9118_BIPOLAR_RANGES)
+		bipolar = 1;	/* all input must be bipolar */
+	if (n_chan > 1)
+		for (i = 1; i < n_chan; i++) {	/* check S.E/diff */
+			if ((CR_AREF(chanlist[i]) == AREF_DIFF) !=
+			    (differencial)) {
+				comedi_error(dev,
+					     "Differencial and single ended "
+						"inputs can't be mixtured!");
+				return 0;
+			}
+			if ((CR_RANGE(chanlist[i]) < PCI9118_BIPOLAR_RANGES) !=
+			    (bipolar)) {
+				comedi_error(dev,
+					     "Bipolar and unipolar ranges "
+							"can't be mixtured!");
+				return 0;
+			}
+			if (!devpriv->usemux && differencial &&
+			    (CR_CHAN(chanlist[i]) >= this_board->n_aichand)) {
+				comedi_error(dev,
+					     "If AREF_DIFF is used then is "
+					"available only first 8 channels!");
+				return 0;
+			}
+		}
+
+	return 1;
+}
+
 static int setup_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s, int n_chan,
 			      unsigned int *chanlist, int rot, int frontadd,
-			      int backadd, int usedma, char eoshandle);
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2);
-static int pci9118_reset(struct comedi_device *dev);
-static int pci9118_exttrg_add(struct comedi_device *dev, unsigned char source);
-static int pci9118_exttrg_del(struct comedi_device *dev, unsigned char source);
-static int pci9118_ai_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
-static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  unsigned int *tim1, unsigned int *tim2,
-				  unsigned int flags, int chans,
-				  unsigned int *div1, unsigned int *div2,
-				  char usessh, unsigned int chnsshfront);
+			      int backadd, int usedma, char useeos)
+{
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int i, differencial = 0, bipolar = 0;
+	unsigned int scanquad, gain, ssh = 0x00;
+
+	if (usedma == 1) {
+		rot = 8;
+		usedma = 0;
+	}
+
+	if (CR_AREF(chanlist[0]) == AREF_DIFF)
+		differencial = 1;	/* all input must be diff */
+	if (CR_RANGE(chanlist[0]) < PCI9118_BIPOLAR_RANGES)
+		bipolar = 1;	/* all input must be bipolar */
+
+	/* All is ok, so we can setup channel/range list */
+
+	if (!bipolar) {
+		devpriv->AdControlReg |= AdControl_UniP;
+							/* set unibipolar */
+	} else {
+		devpriv->AdControlReg &= ((~AdControl_UniP) & 0xff);
+							/* enable bipolar */
+	}
+
+	if (differencial) {
+		devpriv->AdControlReg |= AdControl_Diff;
+							/* enable diff inputs */
+	} else {
+		devpriv->AdControlReg &= ((~AdControl_Diff) & 0xff);
+						/* set single ended inputs */
+	}
+
+	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
+								/* setup mode */
+
+	outl(2, dev->iobase + PCI9118_SCANMOD);
+					/* gods know why this sequence! */
+	outl(0, dev->iobase + PCI9118_SCANMOD);
+	outl(1, dev->iobase + PCI9118_SCANMOD);
+
+#ifdef PCI9118_PARANOIDCHECK
+	devpriv->chanlistlen = n_chan;
+	for (i = 0; i < (PCI9118_CHANLEN + 1); i++)
+		devpriv->chanlist[i] = 0x55aa;
+#endif
+
+	if (frontadd) {		/* insert channels for S&H */
+		ssh = devpriv->softsshsample;
+		for (i = 0; i < frontadd; i++) {
+						/* store range list to card */
+			scanquad = CR_CHAN(chanlist[0]);
+						/* get channel number; */
+			gain = CR_RANGE(chanlist[0]);
+						/* get gain number */
+			scanquad |= ((gain & 0x03) << 8);
+			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
+			ssh = devpriv->softsshhold;
+		}
+	}
+
+	for (i = 0; i < n_chan; i++) {	/* store range list to card */
+		scanquad = CR_CHAN(chanlist[i]);	/* get channel number */
+#ifdef PCI9118_PARANOIDCHECK
+		devpriv->chanlist[i ^ usedma] = (scanquad & 0xf) << rot;
+#endif
+		gain = CR_RANGE(chanlist[i]);		/* get gain number */
+		scanquad |= ((gain & 0x03) << 8);
+		outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
+	}
+
+	if (backadd) {		/* insert channels for fit onto 32bit DMA */
+		for (i = 0; i < backadd; i++) {	/* store range list to card */
+			scanquad = CR_CHAN(chanlist[0]);
+							/* get channel number */
+			gain = CR_RANGE(chanlist[0]);	/* get gain number */
+			scanquad |= ((gain & 0x03) << 8);
+			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
+		}
+	}
+#ifdef PCI9118_PARANOIDCHECK
+	devpriv->chanlist[n_chan ^ usedma] = devpriv->chanlist[0 ^ usedma];
+						/* for 32bit operations */
+	if (useeos) {
+		for (i = 1; i < n_chan; i++) {	/* store range list to card */
+			devpriv->chanlist[(n_chan + i) ^ usedma] =
+			    (CR_CHAN(chanlist[i]) & 0xf) << rot;
+		}
+		devpriv->chanlist[(2 * n_chan) ^ usedma] =
+						devpriv->chanlist[0 ^ usedma];
+						/* for 32bit operations */
+		useeos = 2;
+	} else {
+		useeos = 1;
+	}
+#endif
+	outl(0, dev->iobase + PCI9118_SCANMOD);	/* close scan queue */
+	/* udelay(100); important delay, or first sample will be crippled */
+
+	return 1;		/* we can serve this with scan logic */
+}
 
-/*
-==============================================================================
-*/
 static int pci9118_insn_read_ai(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-
+	struct pci9118_private *devpriv = dev->private;
 	int n, timeout;
 
 	devpriv->AdControlReg = AdControl_Int & 0xff;
@@ -442,13 +557,11 @@ conv_finish:
 
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_insn_write_ao(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci9118_private *devpriv = dev->private;
 	int n, chanreg, ch;
 
 	ch = CR_CHAN(insn->chanspec);
@@ -466,13 +579,11 @@ static int pci9118_insn_write_ao(struct comedi_device *dev,
 	return n;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_insn_read_ao(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci9118_private *devpriv = dev->private;
 	int n, chan;
 
 	chan = CR_CHAN(insn->chanspec);
@@ -482,9 +593,6 @@ static int pci9118_insn_read_ao(struct comedi_device *dev,
 	return n;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_insn_bits_di(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
@@ -494,9 +602,6 @@ static int pci9118_insn_bits_di(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_insn_bits_do(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
@@ -511,11 +616,10 @@ static int pci9118_insn_bits_do(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
 static void interrupt_pci9118_ai_mode4_switch(struct comedi_device *dev)
 {
+	struct pci9118_private *devpriv = dev->private;
+
 	devpriv->AdFunctionReg =
 	    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_AM;
 	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
@@ -533,6 +637,7 @@ static unsigned int defragment_dma_buffer(struct comedi_device *dev,
 					  short *dma_buffer,
 					  unsigned int num_samples)
 {
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int i = 0, j = 0;
 	unsigned int start_pos = devpriv->ai_add_front,
 	    stop_pos = devpriv->ai_add_front + devpriv->ai_n_chan;
@@ -551,14 +656,12 @@ static unsigned int defragment_dma_buffer(struct comedi_device *dev,
 	return j;
 }
 
-/*
-==============================================================================
-*/
 static int move_block_from_dma(struct comedi_device *dev,
 					struct comedi_subdevice *s,
 					short *dma_buffer,
 					unsigned int num_samples)
 {
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int num_bytes;
 
 	num_samples = defragment_dma_buffer(dev, s, dma_buffer, num_samples);
@@ -574,13 +677,153 @@ static int move_block_from_dma(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
+static int pci9118_exttrg_add(struct comedi_device *dev, unsigned char source)
+{
+	struct pci9118_private *devpriv = dev->private;
+
+	if (source > 3)
+		return -1;				/* incorrect source */
+	devpriv->exttrg_users |= (1 << source);
+	devpriv->IntControlReg |= Int_DTrg;
+	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
+	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
+					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+							/* allow INT in AMCC */
+	return 0;
+}
+
+static int pci9118_exttrg_del(struct comedi_device *dev, unsigned char source)
+{
+	struct pci9118_private *devpriv = dev->private;
+
+	if (source > 3)
+		return -1;			/* incorrect source */
+	devpriv->exttrg_users &= ~(1 << source);
+	if (!devpriv->exttrg_users) {	/* shutdown ext trg intterrupts */
+		devpriv->IntControlReg &= ~Int_DTrg;
+		if (!devpriv->IntControlReg)	/* all IRQ disabled */
+			outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) &
+					(~0x00001f00),
+					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+						/* disable int in AMCC */
+		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
+	}
+	return 0;
+}
+
+static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  unsigned int *tim1, unsigned int *tim2,
+				  unsigned int flags, int chans,
+				  unsigned int *div1, unsigned int *div2,
+				  char usessh, unsigned int chnsshfront)
+{
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci9118_private *devpriv = dev->private;
+
+	switch (mode) {
+	case 1:
+	case 4:
+		if (*tim2 < this_board->ai_ns_min)
+			*tim2 = this_board->ai_ns_min;
+		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base, div1, div2,
+					  tim2, flags & TRIG_ROUND_NEAREST);
+		break;
+	case 2:
+		if (*tim2 < this_board->ai_ns_min)
+			*tim2 = this_board->ai_ns_min;
+		*div1 = *tim2 / devpriv->i8254_osc_base;
+						/* convert timer (burst) */
+		if (*div1 < this_board->ai_pacer_min)
+			*div1 = this_board->ai_pacer_min;
+		*div2 = *tim1 / devpriv->i8254_osc_base;	/* scan timer */
+		*div2 = *div2 / *div1;		/* major timer is c1*c2 */
+		if (*div2 < chans)
+			*div2 = chans;
+
+		*tim2 = *div1 * devpriv->i8254_osc_base;
+							/* real convert timer */
+
+		if (usessh & (chnsshfront == 0))	/* use BSSH signal */
+			if (*div2 < (chans + 2))
+				*div2 = chans + 2;
+
+		*tim1 = *div1 * *div2 * devpriv->i8254_osc_base;
+		break;
+	}
+}
+
+static void start_pacer(struct comedi_device *dev, int mode,
+			unsigned int divisor1, unsigned int divisor2)
+{
+	outl(0x74, dev->iobase + PCI9118_CNTCTRL);
+	outl(0xb4, dev->iobase + PCI9118_CNTCTRL);
+/* outl(0x30, dev->iobase + PCI9118_CNTCTRL); */
+	udelay(1);
+
+	if ((mode == 1) || (mode == 2) || (mode == 4)) {
+		outl(divisor2 & 0xff, dev->iobase + PCI9118_CNT2);
+		outl((divisor2 >> 8) & 0xff, dev->iobase + PCI9118_CNT2);
+		outl(divisor1 & 0xff, dev->iobase + PCI9118_CNT1);
+		outl((divisor1 >> 8) & 0xff, dev->iobase + PCI9118_CNT1);
+	}
+}
+
+static int pci9118_ai_cancel(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	struct pci9118_private *devpriv = dev->private;
+
+	if (devpriv->usedma)
+		outl(inl(devpriv->iobase_a + AMCC_OP_REG_MCSR) &
+			(~EN_A2P_TRANSFERS),
+			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
+	pci9118_exttrg_del(dev, EXTTRG_AI);
+	start_pacer(dev, 0, 0, 0);	/* stop 8254 counters */
+	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
+	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
+					/*
+					 * positive triggers, no S&H, no burst,
+					 * burst stop, no post trigger,
+					 * no about trigger, trigger stop
+					 */
+	devpriv->AdControlReg = 0x00;
+	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
+					/*
+					 * bipolar, S.E., use 8254, stop 8354,
+					 * internal trigger, soft trigger,
+					 * disable INT and DMA
+					 */
+	outl(0, dev->iobase + PCI9118_BURST);
+	outl(1, dev->iobase + PCI9118_SCANMOD);
+	outl(2, dev->iobase + PCI9118_SCANMOD);	/* reset scan queue */
+	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
+
+	devpriv->ai_do = 0;
+	devpriv->usedma = 0;
+
+	devpriv->ai_act_scan = 0;
+	devpriv->ai_act_dmapos = 0;
+	s->async->cur_chan = 0;
+	s->async->inttrig = NULL;
+	devpriv->ai_buf_ptr = 0;
+	devpriv->ai_neverending = 0;
+	devpriv->dma_actbuf = 0;
+
+	if (!devpriv->IntControlReg)
+		outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
+					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+							/* allow INT in AMCC */
+
+	return 0;
+}
+
 static char pci9118_decode_error_status(struct comedi_device *dev,
 					struct comedi_subdevice *s,
 					unsigned char m)
 {
+	struct pci9118_private *devpriv = dev->private;
+
 	if (m & 0x100) {
 		comedi_error(dev, "A/D FIFO Full status (Fatal Error!)");
 		devpriv->ai_maskerr &= ~0x100L;
@@ -613,6 +856,7 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 			     unsigned int num_bytes,
 			     unsigned int start_chan_index)
 {
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int i, num_samples = num_bytes / sizeof(short);
 	short *array = data;
 
@@ -627,15 +871,13 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 	}
 }
 
-/*
-==============================================================================
-*/
 static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 					   struct comedi_subdevice *s,
 					   unsigned short int_adstat,
 					   unsigned int int_amcc,
 					   unsigned short int_daq)
 {
+	struct pci9118_private *devpriv = dev->private;
 	register short sampl;
 
 	s->async->events = 0;
@@ -680,15 +922,13 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 		comedi_event(dev, s);
 }
 
-/*
-==============================================================================
-*/
 static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     unsigned short int_adstat,
 				     unsigned int int_amcc,
 				     unsigned short int_daq)
 {
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int next_dma_buf, samplesinbuf, sampls, m;
 
 	if (int_amcc & MASTER_ABORT_INT) {
@@ -713,7 +953,6 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 
 	samplesinbuf = devpriv->dmabuf_use_size[devpriv->dma_actbuf] >> 1;
 					/* number of received real samples */
-/* DPRINTK("dma_actbuf=%d\n",devpriv->dma_actbuf); */
 
 	if (devpriv->dma_doublebuf) {	/*
 					 * switch DMA buffers if is used
@@ -735,17 +974,12 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 						 * how many samples is to
 						 * end of buffer
 						 */
-/*
- * DPRINTK("samps=%d m=%d %d %d\n",
- * samplesinbuf,m,s->async->buf_int_count,s->async->buf_int_ptr);
- */
 		sampls = m;
 		move_block_from_dma(dev, s,
 				    devpriv->dmabuf_virt[devpriv->dma_actbuf],
 				    samplesinbuf);
 		m = m - sampls;		/* m= how many samples was transferred */
 	}
-/* DPRINTK("YYY\n"); */
 
 	if (!devpriv->ai_neverending)
 		if (devpriv->ai_act_scan >= devpriv->ai_scans) {
@@ -768,12 +1002,10 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	comedi_event(dev, s);
 }
 
-/*
-==============================================================================
-*/
 static irqreturn_t interrupt_pci9118(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int int_daq = 0, int_amcc, int_adstat;
 
 	if (!dev->attached)
@@ -784,14 +1016,6 @@ static irqreturn_t interrupt_pci9118(int irq, void *d)
 	int_amcc = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 					/* get INT register from AMCC chip */
 
-/*
- * DPRINTK("INT daq=0x%01x amcc=0x%08x MWAR=0x%08x
- * MWTC=0x%08x ADSTAT=0x%02x ai_do=%d\n",
- * int_daq, int_amcc, inl(devpriv->iobase_a+AMCC_OP_REG_MWAR),
- * inl(devpriv->iobase_a+AMCC_OP_REG_MWTC),
- * inw(dev->iobase+PCI9118_ADSTAT)&0x1ff,devpriv->ai_do);
- */
-
 	if ((!int_daq) && (!(int_amcc & ANY_S593X_INT)))
 		return IRQ_NONE;	/* interrupt from other source */
 
@@ -837,19 +1061,18 @@ static irqreturn_t interrupt_pci9118(int irq, void *d)
 				}
 			}
 
-		(devpriv->int_ai_func) (dev, dev->subdevices + 0, int_adstat,
+		(devpriv->int_ai_func) (dev, &dev->subdevices[0], int_adstat,
 					int_amcc, int_daq);
 
 	}
 	return IRQ_HANDLED;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_ai_inttrig(struct comedi_device *dev,
 			      struct comedi_subdevice *s, unsigned int trignum)
 {
+	struct pci9118_private *devpriv = dev->private;
+
 	if (trignum != devpriv->ai_inttrig_start)
 		return -EINVAL;
 
@@ -868,119 +1091,64 @@ static int pci9118_ai_inttrig(struct comedi_device *dev,
 	return 1;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci9118_private *devpriv = dev->private;
 	int err = 0;
+	unsigned int flags;
 	int tmp;
 	unsigned int divisor1 = 0, divisor2 = 0;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src,
+					TRIG_NOW | TRIG_EXT | TRIG_INT);
 
-	tmp = cmd->scan_begin_src;
+	flags = TRIG_FOLLOW;
 	if (devpriv->master)
-		cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW;
-	else
-		cmd->scan_begin_src &= TRIG_FOLLOW;
+		flags |= TRIG_TIMER | TRIG_EXT;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
 
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
+	flags = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->master)
-		cmd->convert_src &= TRIG_TIMER | TRIG_EXT | TRIG_NOW;
-	else
-		cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+		flags |= TRIG_NOW;
+	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE | TRIG_EXT;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src,
+					TRIG_COUNT | TRIG_NONE | TRIG_EXT);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2:
-	 * make sure trigger sources are
-	 * unique and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW &&
-	    cmd->start_src != TRIG_INT && cmd->start_src != TRIG_EXT) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
-
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_INT &&
-	    cmd->scan_begin_src != TRIG_FOLLOW) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
-
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW) {
-		cmd->convert_src = TRIG_TIMER;
-		err++;
-	}
-
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
+	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);
 
-	if (cmd->stop_src != TRIG_NONE &&
-	    cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_INT && cmd->stop_src != TRIG_EXT) {
-		cmd->stop_src = TRIG_COUNT;
-		err++;
-	}
+	/* Step 2b : and mutually compatible */
 
-	if (cmd->start_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
+	if (cmd->start_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
+		err |= -EINVAL;
 
-	if (cmd->start_src == TRIG_INT && cmd->scan_begin_src == TRIG_INT) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
+	if (cmd->start_src == TRIG_INT && cmd->scan_begin_src == TRIG_INT)
+		err |= -EINVAL;
 
 	if ((cmd->scan_begin_src & (TRIG_TIMER | TRIG_EXT)) &&
-	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_NOW)))) {
-		cmd->convert_src = TRIG_TIMER;
-		err++;
-	}
+	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_NOW))))
+		err |= -EINVAL;
 
 	if ((cmd->scan_begin_src == TRIG_FOLLOW) &&
-	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_EXT)))) {
-		cmd->convert_src = TRIG_TIMER;
-		err++;
-	}
+	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_EXT))))
+		err |= -EINVAL;
 
-	if (cmd->stop_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT) {
-		cmd->stop_src = TRIG_COUNT;
-		err++;
-	}
+	if (cmd->stop_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -1074,11 +1242,9 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		tmp = cmd->scan_begin_arg;
-/* printk("S1 timer1=%u timer2=%u\n",cmd->scan_begin_arg,cmd->convert_arg); */
 		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base, &divisor1,
 					  &divisor2, &cmd->scan_begin_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
-/* printk("S2 timer1=%u timer2=%u\n",cmd->scan_begin_arg,cmd->convert_arg); */
 		if (cmd->scan_begin_arg < this_board->ai_ns_min)
 			cmd->scan_begin_arg = this_board->ai_ns_min;
 		if (tmp != cmd->scan_begin_arg)
@@ -1090,7 +1256,6 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base, &divisor1,
 					  &divisor2, &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
-/* printk("s1 timer1=%u timer2=%u\n",cmd->scan_begin_arg,cmd->convert_arg); */
 		if (cmd->convert_arg < this_board->ai_ns_min)
 			cmd->convert_arg = this_board->ai_ns_min;
 		if (tmp != cmd->convert_arg)
@@ -1104,7 +1269,6 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 					cmd->scan_begin_arg =
 					    this_board->ai_ns_min *
 					    (cmd->scan_end_arg + 2);
-/* printk("s2 timer1=%u timer2=%u\n",cmd->scan_begin_arg,cmd->convert_arg); */
 					err++;
 				}
 			} else {
@@ -1113,7 +1277,6 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 					cmd->scan_begin_arg =
 					    cmd->convert_arg *
 					    cmd->chanlist_len;
-/* printk("s3 timer1=%u timer2=%u\n",cmd->scan_begin_arg,cmd->convert_arg); */
 					err++;
 				}
 			}
@@ -1131,18 +1294,13 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
 static int Compute_and_setup_dma(struct comedi_device *dev)
 {
+	struct pci9118_private *devpriv = dev->private;
 	unsigned int dmalen0, dmalen1, i;
 
-	DPRINTK("adl_pci9118 EDBG: BGN: Compute_and_setup_dma()\n");
 	dmalen0 = devpriv->dmabuf_size[0];
 	dmalen1 = devpriv->dmabuf_size[1];
-	DPRINTK("1 dmalen0=%d dmalen1=%d ai_data_len=%d\n", dmalen0, dmalen1,
-		devpriv->ai_data_len);
 	/* isn't output buff smaller that our DMA buff? */
 	if (dmalen0 > (devpriv->ai_data_len)) {
 		dmalen0 = devpriv->ai_data_len & ~3L;	/*
@@ -1154,7 +1312,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 							 * align to 32bit down
 							 */
 	}
-	DPRINTK("2 dmalen0=%d dmalen1=%d\n", dmalen0, dmalen1);
 
 	/* we want wake up every scan? */
 	if (devpriv->ai_flags & TRIG_WAKE_EOS) {
@@ -1169,11 +1326,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short first DMA buffer to one scan */
 			dmalen0 = devpriv->ai_n_realscanlen << 1;
-			DPRINTK
-				("21 dmalen0=%d ai_n_realscanlen=%d "
-							"useeoshandle=%d\n",
-				dmalen0, devpriv->ai_n_realscanlen,
-				devpriv->useeoshandle);
 			if (devpriv->useeoshandle)
 				dmalen0 += 2;
 			if (dmalen0 < 4) {
@@ -1197,11 +1349,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short second DMA buffer to one scan */
 			dmalen1 = devpriv->ai_n_realscanlen << 1;
-			DPRINTK
-			    ("22 dmalen1=%d ai_n_realscanlen=%d "
-							"useeoshandle=%d\n",
-			     dmalen1, devpriv->ai_n_realscanlen,
-			     devpriv->useeoshandle);
 			if (devpriv->useeoshandle)
 				dmalen1 -= 2;
 			if (dmalen1 < 4) {
@@ -1214,7 +1361,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		}
 	}
 
-	DPRINTK("3 dmalen0=%d dmalen1=%d\n", dmalen0, dmalen1);
 	/* transfer without TRIG_WAKE_EOS */
 	if (!(devpriv->ai_flags & TRIG_WAKE_EOS)) {
 		/* if it's possible then align DMA buffers to length of scan */
@@ -1241,15 +1387,9 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 			if (dmalen0 >
 			    ((devpriv->ai_n_realscanlen << 1) *
 			     devpriv->ai_scans)) {
-				DPRINTK
-				    ("3.0 ai_n_realscanlen=%d ai_scans=%d\n",
-				     devpriv->ai_n_realscanlen,
-				     devpriv->ai_scans);
 				dmalen0 =
 				    (devpriv->ai_n_realscanlen << 1) *
 				    devpriv->ai_scans;
-				DPRINTK("3.1 dmalen0=%d dmalen1=%d\n", dmalen0,
-					dmalen1);
 				dmalen0 &= ~3L;
 			} else {	/*
 					 * fits whole measure into
@@ -1261,21 +1401,16 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 					dmalen1 =
 					    (devpriv->ai_n_realscanlen << 1) *
 					    devpriv->ai_scans - dmalen0;
-				DPRINTK("3.2 dmalen0=%d dmalen1=%d\n", dmalen0,
-					dmalen1);
 				dmalen1 &= ~3L;
 			}
 		}
 	}
 
-	DPRINTK("4 dmalen0=%d dmalen1=%d\n", dmalen0, dmalen1);
-
 	/* these DMA buffer size will be used */
 	devpriv->dma_actbuf = 0;
 	devpriv->dmabuf_use_size[0] = dmalen0;
 	devpriv->dmabuf_use_size[1] = dmalen1;
 
-	DPRINTK("5 dmalen0=%d dmalen1=%d\n", dmalen0, dmalen1);
 #if 0
 	if (devpriv->ai_n_scanlen < this_board->half_fifo_size) {
 		devpriv->dmabuf_panic_size[0] =
@@ -1308,18 +1443,14 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 			devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 						/* allow bus mastering */
 
-	DPRINTK("adl_pci9118 EDBG: END: Compute_and_setup_dma()\n");
 	return 0;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 				  struct comedi_subdevice *s)
 {
-	DPRINTK("adl_pci9118 EDBG: BGN: pci9118_ai_docmd_sampl(%d,) [%d]\n",
-		dev->minor, devpriv->ai_do);
+	struct pci9118_private *devpriv = dev->private;
+
 	switch (devpriv->ai_do) {
 	case 1:
 		devpriv->AdControlReg |= AdControl_TmrTr;
@@ -1366,18 +1497,14 @@ static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 	}
 
-	DPRINTK("adl_pci9118 EDBG: END: pci9118_ai_docmd_sampl()\n");
 	return 0;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
-	DPRINTK("adl_pci9118 EDBG: BGN: pci9118_ai_docmd_dma(%d,) [%d,%d]\n",
-		dev->minor, devpriv->ai_do, devpriv->usedma);
+	struct pci9118_private *devpriv = dev->private;
+
 	Compute_and_setup_dma(dev);
 
 	switch (devpriv->ai_do) {
@@ -1440,20 +1567,17 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 		outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
 	}
 
-	DPRINTK("adl_pci9118 EDBG: BGN: pci9118_ai_docmd_dma()\n");
 	return 0;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int addchans = 0;
 	int ret = 0;
 
-	DPRINTK("adl_pci9118 EDBG: BGN: pci9118_ai_cmd(%d,)\n", dev->minor);
 	devpriv->ai12_startstop = 0;
 	devpriv->ai_flags = cmd->flags;
 	devpriv->ai_n_chan = cmd->chanlist_len;
@@ -1502,10 +1626,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->usessh = 0;
 				/*  no */
 
-	DPRINTK("1 neverending=%d scans=%u usessh=%d ai_startstop=0x%2x\n",
-		devpriv->ai_neverending, devpriv->ai_scans, devpriv->usessh,
-		devpriv->ai12_startstop);
-
 	/*
 	 * use additional sample at end of every scan
 	 * to satisty DMA 32 bit transfer?
@@ -1586,12 +1706,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	     devpriv->ai_add_back) * (devpriv->ai_n_scanlen /
 				      devpriv->ai_n_chan);
 
-	DPRINTK("2 usedma=%d realscan=%d af=%u n_chan=%d ab=%d n_scanlen=%d\n",
-		devpriv->usedma,
-		devpriv->ai_n_realscanlen, devpriv->ai_add_front,
-		devpriv->ai_n_chan, devpriv->ai_add_back,
-		devpriv->ai_n_scanlen);
-
 	/* check and setup channel list */
 	if (!check_channel_list(dev, s, devpriv->ai_n_chan,
 				devpriv->ai_chanlist, devpriv->ai_add_front,
@@ -1688,371 +1802,13 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	else
 		ret = pci9118_ai_docmd_sampl(dev, s);
 
-	DPRINTK("adl_pci9118 EDBG: END: pci9118_ai_cmd()\n");
 	return ret;
 }
 
-/*
-==============================================================================
-*/
-static int check_channel_list(struct comedi_device *dev,
-			      struct comedi_subdevice *s, int n_chan,
-			      unsigned int *chanlist, int frontadd, int backadd)
-{
-	unsigned int i, differencial = 0, bipolar = 0;
-
-	/* correct channel and range number check itself comedi/range.c */
-	if (n_chan < 1) {
-		comedi_error(dev, "range/channel list is empty!");
-		return 0;
-	}
-	if ((frontadd + n_chan + backadd) > s->len_chanlist) {
-		printk
-		    ("comedi%d: range/channel list is too long for "
-						"actual configuration (%d>%d)!",
-		     dev->minor, n_chan, s->len_chanlist - frontadd - backadd);
-		return 0;
-	}
-
-	if (CR_AREF(chanlist[0]) == AREF_DIFF)
-		differencial = 1;	/* all input must be diff */
-	if (CR_RANGE(chanlist[0]) < PCI9118_BIPOLAR_RANGES)
-		bipolar = 1;	/* all input must be bipolar */
-	if (n_chan > 1)
-		for (i = 1; i < n_chan; i++) {	/* check S.E/diff */
-			if ((CR_AREF(chanlist[i]) == AREF_DIFF) !=
-			    (differencial)) {
-				comedi_error(dev,
-					     "Differencial and single ended "
-						"inputs can't be mixtured!");
-				return 0;
-			}
-			if ((CR_RANGE(chanlist[i]) < PCI9118_BIPOLAR_RANGES) !=
-			    (bipolar)) {
-				comedi_error(dev,
-					     "Bipolar and unipolar ranges "
-							"can't be mixtured!");
-				return 0;
-			}
-			if (!devpriv->usemux && differencial &&
-			    (CR_CHAN(chanlist[i]) >= this_board->n_aichand)) {
-				comedi_error(dev,
-					     "If AREF_DIFF is used then is "
-					"available only first 8 channels!");
-				return 0;
-			}
-		}
-
-	return 1;
-}
-
-/*
-==============================================================================
-*/
-static int setup_channel_list(struct comedi_device *dev,
-			      struct comedi_subdevice *s, int n_chan,
-			      unsigned int *chanlist, int rot, int frontadd,
-			      int backadd, int usedma, char useeos)
-{
-	unsigned int i, differencial = 0, bipolar = 0;
-	unsigned int scanquad, gain, ssh = 0x00;
-
-	DPRINTK
-	    ("adl_pci9118 EDBG: BGN: setup_channel_list"
-						"(%d,.,%d,.,%d,%d,%d,%d)\n",
-	     dev->minor, n_chan, rot, frontadd, backadd, usedma);
-
-	if (usedma == 1) {
-		rot = 8;
-		usedma = 0;
-	}
-
-	if (CR_AREF(chanlist[0]) == AREF_DIFF)
-		differencial = 1;	/* all input must be diff */
-	if (CR_RANGE(chanlist[0]) < PCI9118_BIPOLAR_RANGES)
-		bipolar = 1;	/* all input must be bipolar */
-
-	/* All is ok, so we can setup channel/range list */
-
-	if (!bipolar) {
-		devpriv->AdControlReg |= AdControl_UniP;
-							/* set unibipolar */
-	} else {
-		devpriv->AdControlReg &= ((~AdControl_UniP) & 0xff);
-							/* enable bipolar */
-	}
-
-	if (differencial) {
-		devpriv->AdControlReg |= AdControl_Diff;
-							/* enable diff inputs */
-	} else {
-		devpriv->AdControlReg &= ((~AdControl_Diff) & 0xff);
-						/* set single ended inputs */
-	}
-
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-								/* setup mode */
-
-	outl(2, dev->iobase + PCI9118_SCANMOD);
-					/* gods know why this sequence! */
-	outl(0, dev->iobase + PCI9118_SCANMOD);
-	outl(1, dev->iobase + PCI9118_SCANMOD);
-
-#ifdef PCI9118_PARANOIDCHECK
-	devpriv->chanlistlen = n_chan;
-	for (i = 0; i < (PCI9118_CHANLEN + 1); i++)
-		devpriv->chanlist[i] = 0x55aa;
-#endif
-
-	if (frontadd) {		/* insert channels for S&H */
-		ssh = devpriv->softsshsample;
-		DPRINTK("FA: %04x: ", ssh);
-		for (i = 0; i < frontadd; i++) {
-						/* store range list to card */
-			scanquad = CR_CHAN(chanlist[0]);
-						/* get channel number; */
-			gain = CR_RANGE(chanlist[0]);
-						/* get gain number */
-			scanquad |= ((gain & 0x03) << 8);
-			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
-			DPRINTK("%02x ", scanquad | ssh);
-			ssh = devpriv->softsshhold;
-		}
-		DPRINTK("\n ");
-	}
-
-	DPRINTK("SL: ", ssh);
-	for (i = 0; i < n_chan; i++) {	/* store range list to card */
-		scanquad = CR_CHAN(chanlist[i]);	/* get channel number */
-#ifdef PCI9118_PARANOIDCHECK
-		devpriv->chanlist[i ^ usedma] = (scanquad & 0xf) << rot;
-#endif
-		gain = CR_RANGE(chanlist[i]);		/* get gain number */
-		scanquad |= ((gain & 0x03) << 8);
-		outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
-		DPRINTK("%02x ", scanquad | ssh);
-	}
-	DPRINTK("\n ");
-
-	if (backadd) {		/* insert channels for fit onto 32bit DMA */
-		DPRINTK("BA: %04x: ", ssh);
-		for (i = 0; i < backadd; i++) {	/* store range list to card */
-			scanquad = CR_CHAN(chanlist[0]);
-							/* get channel number */
-			gain = CR_RANGE(chanlist[0]);	/* get gain number */
-			scanquad |= ((gain & 0x03) << 8);
-			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
-			DPRINTK("%02x ", scanquad | ssh);
-		}
-		DPRINTK("\n ");
-	}
-#ifdef PCI9118_PARANOIDCHECK
-	devpriv->chanlist[n_chan ^ usedma] = devpriv->chanlist[0 ^ usedma];
-						/* for 32bit operations */
-	if (useeos) {
-		for (i = 1; i < n_chan; i++) {	/* store range list to card */
-			devpriv->chanlist[(n_chan + i) ^ usedma] =
-			    (CR_CHAN(chanlist[i]) & 0xf) << rot;
-		}
-		devpriv->chanlist[(2 * n_chan) ^ usedma] =
-						devpriv->chanlist[0 ^ usedma];
-						/* for 32bit operations */
-		useeos = 2;
-	} else {
-		useeos = 1;
-	}
-#ifdef PCI9118_EXTDEBUG
-	DPRINTK("CHL: ");
-	for (i = 0; i <= (useeos * n_chan); i++)
-		DPRINTK("%04x ", devpriv->chanlist[i]);
-
-	DPRINTK("\n ");
-#endif
-#endif
-	outl(0, dev->iobase + PCI9118_SCANMOD);	/* close scan queue */
-	/* udelay(100); important delay, or first sample will be crippled */
-
-	DPRINTK("adl_pci9118 EDBG: END: setup_channel_list()\n");
-	return 1;		/* we can serve this with scan logic */
-}
-
-/*
-==============================================================================
-  calculate 8254 divisors if they are used for dual timing
-*/
-static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  unsigned int *tim1, unsigned int *tim2,
-				  unsigned int flags, int chans,
-				  unsigned int *div1, unsigned int *div2,
-				  char usessh, unsigned int chnsshfront)
-{
-	DPRINTK
-	    ("adl_pci9118 EDBG: BGN: pci9118_calc_divisors"
-					"(%d,%d,.,%u,%u,%u,%d,.,.,,%u,%u)\n",
-	     mode, dev->minor, *tim1, *tim2, flags, chans, usessh, chnsshfront);
-	switch (mode) {
-	case 1:
-	case 4:
-		if (*tim2 < this_board->ai_ns_min)
-			*tim2 = this_board->ai_ns_min;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base, div1, div2,
-					  tim2, flags & TRIG_ROUND_NEAREST);
-		DPRINTK("OSC base=%u div1=%u div2=%u timer1=%u\n",
-			devpriv->i8254_osc_base, *div1, *div2, *tim1);
-		break;
-	case 2:
-		if (*tim2 < this_board->ai_ns_min)
-			*tim2 = this_board->ai_ns_min;
-		DPRINTK("1 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		*div1 = *tim2 / devpriv->i8254_osc_base;
-						/* convert timer (burst) */
-		DPRINTK("2 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		if (*div1 < this_board->ai_pacer_min)
-			*div1 = this_board->ai_pacer_min;
-		DPRINTK("3 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		*div2 = *tim1 / devpriv->i8254_osc_base;	/* scan timer */
-		DPRINTK("4 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		*div2 = *div2 / *div1;		/* major timer is c1*c2 */
-		DPRINTK("5 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		if (*div2 < chans)
-			*div2 = chans;
-		DPRINTK("6 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-
-		*tim2 = *div1 * devpriv->i8254_osc_base;
-							/* real convert timer */
-
-		if (usessh & (chnsshfront == 0))	/* use BSSH signal */
-			if (*div2 < (chans + 2))
-				*div2 = chans + 2;
-
-		DPRINTK("7 div1=%u div2=%u timer1=%u timer2=%u\n", *div1, *div2,
-			*tim1, *tim2);
-		*tim1 = *div1 * *div2 * devpriv->i8254_osc_base;
-		DPRINTK("OSC base=%u div1=%u div2=%u timer1=%u timer2=%u\n",
-			devpriv->i8254_osc_base, *div1, *div2, *tim1, *tim2);
-		break;
-	}
-	DPRINTK("adl_pci9118 EDBG: END: pci9118_calc_divisors(%u,%u)\n",
-		*div1, *div2);
-}
-
-/*
-==============================================================================
-*/
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
-{
-	outl(0x74, dev->iobase + PCI9118_CNTCTRL);
-	outl(0xb4, dev->iobase + PCI9118_CNTCTRL);
-/* outl(0x30, dev->iobase + PCI9118_CNTCTRL); */
-	udelay(1);
-
-	if ((mode == 1) || (mode == 2) || (mode == 4)) {
-		outl(divisor2 & 0xff, dev->iobase + PCI9118_CNT2);
-		outl((divisor2 >> 8) & 0xff, dev->iobase + PCI9118_CNT2);
-		outl(divisor1 & 0xff, dev->iobase + PCI9118_CNT1);
-		outl((divisor1 >> 8) & 0xff, dev->iobase + PCI9118_CNT1);
-	}
-}
-
-/*
-==============================================================================
-*/
-static int pci9118_exttrg_add(struct comedi_device *dev, unsigned char source)
-{
-	if (source > 3)
-		return -1;				/* incorrect source */
-	devpriv->exttrg_users |= (1 << source);
-	devpriv->IntControlReg |= Int_DTrg;
-	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-							/* allow INT in AMCC */
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci9118_exttrg_del(struct comedi_device *dev, unsigned char source)
-{
-	if (source > 3)
-		return -1;			/* incorrect source */
-	devpriv->exttrg_users &= ~(1 << source);
-	if (!devpriv->exttrg_users) {	/* shutdown ext trg intterrupts */
-		devpriv->IntControlReg &= ~Int_DTrg;
-		if (!devpriv->IntControlReg)	/* all IRQ disabled */
-			outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) &
-					(~0x00001f00),
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-						/* disable int in AMCC */
-		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	}
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci9118_ai_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	if (devpriv->usedma)
-		outl(inl(devpriv->iobase_a + AMCC_OP_REG_MCSR) &
-			(~EN_A2P_TRANSFERS),
-			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
-	pci9118_exttrg_del(dev, EXTTRG_AI);
-	start_pacer(dev, 0, 0, 0);	/* stop 8254 counters */
-	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-					/*
-					 * positive triggers, no S&H, no burst,
-					 * burst stop, no post trigger,
-					 * no about trigger, trigger stop
-					 */
-	devpriv->AdControlReg = 0x00;
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-					/*
-					 * bipolar, S.E., use 8254, stop 8354,
-					 * internal trigger, soft trigger,
-					 * disable INT and DMA
-					 */
-	outl(0, dev->iobase + PCI9118_BURST);
-	outl(1, dev->iobase + PCI9118_SCANMOD);
-	outl(2, dev->iobase + PCI9118_SCANMOD);	/* reset scan queue */
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-
-	devpriv->ai_do = 0;
-	devpriv->usedma = 0;
-
-	devpriv->ai_act_scan = 0;
-	devpriv->ai_act_dmapos = 0;
-	s->async->cur_chan = 0;
-	s->async->inttrig = NULL;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->ai_neverending = 0;
-	devpriv->dma_actbuf = 0;
-
-	if (!devpriv->IntControlReg)
-		outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-							/* allow INT in AMCC */
-
-	return 0;
-}
-
-/*
-==============================================================================
-*/
 static int pci9118_reset(struct comedi_device *dev)
 {
+	struct pci9118_private *devpriv = dev->private;
+
 	devpriv->IntControlReg = 0;
 	devpriv->exttrg_users = 0;
 	inl(dev->iobase + PCI9118_INTCTRL);
@@ -2112,6 +1868,7 @@ static int pci9118_reset(struct comedi_device *dev)
 static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
 					struct comedi_devconfig *it)
 {
+	const struct boardtype *this_board = comedi_board(dev);
 	struct pci_dev *pcidev = NULL;
 	int bus = it->options[0];
 	int slot = it->options[1];
@@ -2150,6 +1907,8 @@ static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
 static int pci9118_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci9118_private *devpriv;
 	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret, pages, i;
@@ -2164,11 +1923,12 @@ static int pci9118_attach(struct comedi_device *dev,
 	else
 		master = 1;
 
-	ret = alloc_private(dev, sizeof(struct pci9118_private));
+	ret = alloc_private(dev, sizeof(*devpriv));
 	if (ret < 0) {
 		printk(" - Allocation failed!\n");
 		return -ENOMEM;
 	}
+	devpriv = dev->private;
 
 	pcidev = pci9118_find_pci(dev, it);
 	if (!pcidev)
@@ -2273,7 +2033,7 @@ static int pci9118_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
@@ -2294,7 +2054,7 @@ static int pci9118_attach(struct comedi_device *dev,
 		s->munge = pci9118_ai_munge;
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan = this_board->n_aochan;
@@ -2304,7 +2064,7 @@ static int pci9118_attach(struct comedi_device *dev,
 	s->insn_write = pci9118_insn_write_ao;
 	s->insn_read = pci9118_insn_read_ao;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan = 4;
@@ -2314,7 +2074,7 @@ static int pci9118_attach(struct comedi_device *dev,
 	s->io_bits = 0;		/* all bits input */
 	s->insn_bits = pci9118_insn_bits_di;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan = 4;
@@ -2345,8 +2105,9 @@ static int pci9118_attach(struct comedi_device *dev,
 static void pci9118_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct pci9118_private *devpriv = dev->private;
 
-	if (dev->private) {
+	if (devpriv) {
 		if (devpriv->valid)
 			pci9118_reset(dev);
 		if (dev->irq)
diff --git a/drivers/staging/comedi/drivers/adq12b.c b/drivers/staging/comedi/drivers/adq12b.c
index 6df51c8a602a..3a2aa5628be3 100644
--- a/drivers/staging/comedi/drivers/adq12b.c
+++ b/drivers/staging/comedi/drivers/adq12b.c
@@ -133,8 +133,6 @@ struct adq12b_private {
 	unsigned int digital_state;
 };
 
-#define devpriv ((struct adq12b_private *)dev->private)
-
 /*
  * "instructions" read/write data in "one-shot" or "software-triggered"
  * mode.
@@ -144,6 +142,7 @@ static int adq12b_ai_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	struct adq12b_private *devpriv = dev->private;
 	int n, i;
 	int range, channel;
 	unsigned char hi, lo, status;
@@ -200,6 +199,7 @@ static int adq12b_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct adq12b_private *devpriv = dev->private;
 	int channel;
 
 	for (channel = 0; channel < 8; channel++)
@@ -221,6 +221,7 @@ static int adq12b_do_insn_bits(struct comedi_device *dev,
 static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct adq12b_board *board = comedi_board(dev);
+	struct adq12b_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	int unipolar, differential;
@@ -252,19 +253,18 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = board->name;
 
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct adq12b_private)) < 0)
-		return -ENOMEM;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
-/* fill in devpriv structure */
 	devpriv->unipolar = unipolar;
 	devpriv->differential = differential;
 	devpriv->digital_state = 0;
-/* initialize channel and range to -1 so we make sure we always write
-   at least once to the CTREG in the instruction */
+	/*
+	 * initialize channel and range to -1 so we make sure we
+	 * always write at least once to the CTREG in the instruction
+	 */
 	devpriv->last_channel = -1;
 	devpriv->last_range = -1;
 
@@ -272,7 +272,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	if (differential) {
@@ -294,7 +294,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				   the board can handle */
 	s->insn_read = adq12b_ai_rinsn;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* digital input subdevice */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -303,7 +303,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->insn_bits = adq12b_di_insn_bits;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital output subdevice */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -321,7 +321,6 @@ static void adq12b_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		release_region(dev->iobase, ADQ12B_SIZE);
-	kfree(devpriv);
 }
 
 static const struct adq12b_board adq12b_boards[] = {
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 6b4d0d68e637..def37bcc2a66 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -45,6 +45,7 @@ Configuration options:
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "8253.h"
 #include "amcc_s5933.h"
 
@@ -52,17 +53,6 @@ Configuration options:
 				 * correct channel number on every 12 bit
 				 * sample */
 
-#undef PCI171X_EXTDEBUG
-
-#define DRV_NAME "adv_pci1710"
-
-#undef DPRINTK
-#ifdef PCI171X_EXTDEBUG
-#define DPRINTK(fmt, args...) printk(fmt, ## args)
-#else
-#define DPRINTK(fmt, args...)
-#endif
-
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe
 
 /* hardware types of the cards */
@@ -211,44 +201,101 @@ struct boardtype {
 };
 
 static const struct boardtype boardtypes[] = {
-	{"pci1710", 0x1710,
-	 IORANGE_171x, 1, TYPE_PCI171X,
-	 16, 8, 2, 16, 16, 1, 0x0fff, 0x0fff,
-	 &range_pci1710_3, range_codes_pci1710_3,
-	 &range_pci171x_da,
-	 10000, 2048},
-	{"pci1710hg", 0x1710,
-	 IORANGE_171x, 1, TYPE_PCI171X,
-	 16, 8, 2, 16, 16, 1, 0x0fff, 0x0fff,
-	 &range_pci1710hg, range_codes_pci1710hg,
-	 &range_pci171x_da,
-	 10000, 2048},
-	{"pci1711", 0x1711,
-	 IORANGE_171x, 1, TYPE_PCI171X,
-	 16, 0, 2, 16, 16, 1, 0x0fff, 0x0fff,
-	 &range_pci17x1, range_codes_pci17x1, &range_pci171x_da,
-	 10000, 512},
-	{"pci1713", 0x1713,
-	 IORANGE_171x, 1, TYPE_PCI1713,
-	 32, 16, 0, 0, 0, 0, 0x0fff, 0x0000,
-	 &range_pci1710_3, range_codes_pci1710_3, NULL,
-	 10000, 2048},
-	{"pci1720", 0x1720,
-	 IORANGE_1720, 0, TYPE_PCI1720,
-	 0, 0, 4, 0, 0, 0, 0x0000, 0x0fff,
-	 NULL, NULL, &range_pci1720,
-	 0, 0},
-	{"pci1731", 0x1731,
-	 IORANGE_171x, 1, TYPE_PCI171X,
-	 16, 0, 0, 16, 16, 0, 0x0fff, 0x0000,
-	 &range_pci17x1, range_codes_pci17x1, NULL,
-	 10000, 512},
-	/*  dummy entry corresponding to driver name */
-	{.name = DRV_NAME},
+	{
+		.name		= "pci1710",
+		.device_id	= 0x1710,
+		.iorange	= IORANGE_171x,
+		.have_irq	= 1,
+		.cardtype	= TYPE_PCI171X,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.n_aochan	= 2,
+		.n_dichan	= 16,
+		.n_dochan	= 16,
+		.n_counter	= 1,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci1710_3,
+		.rangecode_ai	= range_codes_pci1710_3,
+		.rangelist_ao	= &range_pci171x_da,
+		.ai_ns_min	= 10000,
+		.fifo_half_size	= 2048,
+	}, {
+		.name		= "pci1710hg",
+		.device_id	= 0x1710,
+		.iorange	= IORANGE_171x,
+		.have_irq	= 1,
+		.cardtype	= TYPE_PCI171X,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.n_aochan	= 2,
+		.n_dichan	= 16,
+		.n_dochan	= 16,
+		.n_counter	= 1,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci1710hg,
+		.rangecode_ai	= range_codes_pci1710hg,
+		.rangelist_ao	= &range_pci171x_da,
+		.ai_ns_min	= 10000,
+		.fifo_half_size	= 2048,
+	}, {
+		.name		= "pci1711",
+		.device_id	= 0x1711,
+		.iorange	= IORANGE_171x,
+		.have_irq	= 1,
+		.cardtype	= TYPE_PCI171X,
+		.n_aichan	= 16,
+		.n_aochan	= 2,
+		.n_dichan	= 16,
+		.n_dochan	= 16,
+		.n_counter	= 1,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci17x1,
+		.rangecode_ai	= range_codes_pci17x1,
+		.rangelist_ao	= &range_pci171x_da,
+		.ai_ns_min	= 10000,
+		.fifo_half_size	= 512,
+	}, {
+		.name		= "pci1713",
+		.device_id	= 0x1713,
+		.iorange	= IORANGE_171x,
+		.have_irq	= 1,
+		.cardtype	= TYPE_PCI1713,
+		.n_aichan	= 32,
+		.n_aichand	= 16,
+		.ai_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci1710_3,
+		.rangecode_ai	= range_codes_pci1710_3,
+		.ai_ns_min	= 10000,
+		.fifo_half_size	= 2048,
+	}, {
+		.name		= "pci1720",
+		.device_id	= 0x1720,
+		.iorange	= IORANGE_1720,
+		.cardtype	= TYPE_PCI1720,
+		.n_aochan	= 4,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ao	= &range_pci1720,
+	}, {
+		.name		= "pci1731",
+		.device_id	= 0x1731,
+		.iorange	= IORANGE_171x,
+		.have_irq	= 1,
+		.cardtype	= TYPE_PCI171X,
+		.n_aichan	= 16,
+		.n_dichan	= 16,
+		.n_dochan	= 16,
+		.ai_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci17x1,
+		.rangecode_ai	= range_codes_pci17x1,
+		.ai_ns_min	= 10000,
+		.fifo_half_size	= 512,
+	},
 };
 
 struct pci1710_private {
-	char valid;		/*  card is usable */
 	char neverending_ai;	/*  we do unlimited AI */
 	unsigned int CntrlReg;	/*  Control register */
 	unsigned int i8254_osc_base;	/*  frequence of onboard oscilator */
@@ -278,33 +325,108 @@ struct pci1710_private {
 					 * internal state */
 };
 
-#define devpriv ((struct pci1710_private *)dev->private)
-#define this_board ((const struct boardtype *)dev->board_ptr)
+/*  used for gain list programming */
+static const unsigned int muxonechan[] = {
+	0x0000, 0x0101, 0x0202, 0x0303, 0x0404, 0x0505, 0x0606, 0x0707,
+	0x0808, 0x0909, 0x0a0a, 0x0b0b, 0x0c0c, 0x0d0d, 0x0e0e, 0x0f0f,
+	0x1010, 0x1111, 0x1212, 0x1313, 0x1414, 0x1515, 0x1616, 0x1717,
+	0x1818, 0x1919, 0x1a1a, 0x1b1b, 0x1c1c, 0x1d1d, 0x1e1e, 0x1f1f
+};
 
 /*
 ==============================================================================
+ Check if channel list from user is builded correctly
+ If it's ok, then program scan/gain logic.
+ This works for all cards.
 */
-
 static int check_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
-			      unsigned int *chanlist, unsigned int n_chan);
+			      unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int chansegment[32];
+	unsigned int i, nowmustbechan, seglen, segpos;
+
+	/* correct channel and range number check itself comedi/range.c */
+	if (n_chan < 1) {
+		comedi_error(dev, "range/channel list is empty!");
+		return 0;
+	}
+
+	if (n_chan == 1)
+		return 1; /* seglen=1 */
+
+	chansegment[0] = chanlist[0]; /*  first channel is every time ok */
+	for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {
+		if (chanlist[0] == chanlist[i])
+			break;	/*  we detected a loop, stop */
+		if ((CR_CHAN(chanlist[i]) & 1) &&
+		    (CR_AREF(chanlist[i]) == AREF_DIFF)) {
+			comedi_error(dev, "Odd channel cannot be differential input!\n");
+			return 0;
+		}
+		nowmustbechan = (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
+		if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
+			nowmustbechan = (nowmustbechan + 1) % s->n_chan;
+		if (nowmustbechan != CR_CHAN(chanlist[i])) {
+			printk("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
+			       i, CR_CHAN(chanlist[i]), nowmustbechan,
+			       CR_CHAN(chanlist[0]));
+			return 0;
+		}
+		chansegment[i] = chanlist[i]; /* next correct channel in list */
+	}
+
+	for (i = 0, segpos = 0; i < n_chan; i++) {
+		if (chanlist[i] != chansegment[i % seglen]) {
+			printk("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
+			       i, CR_CHAN(chansegment[i]),
+			       CR_RANGE(chansegment[i]),
+			       CR_AREF(chansegment[i]),
+			       CR_CHAN(chanlist[i % seglen]),
+			       CR_RANGE(chanlist[i % seglen]),
+			       CR_AREF(chansegment[i % seglen]));
+			return 0;
+		}
+	}
+	return seglen;
+}
+
 static void setup_channel_list(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       unsigned int *chanlist, unsigned int n_chan,
-			       unsigned int seglen);
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2);
-static int pci1710_reset(struct comedi_device *dev);
-static int pci171x_ai_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
+			       unsigned int seglen)
+{
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+	unsigned int i, range, chanprog;
 
-/*  used for gain list programming */
-static const unsigned int muxonechan[] = {
-	0x0000, 0x0101, 0x0202, 0x0303, 0x0404, 0x0505, 0x0606, 0x0707,
-	0x0808, 0x0909, 0x0a0a, 0x0b0b, 0x0c0c, 0x0d0d, 0x0e0e, 0x0f0f,
-	0x1010, 0x1111, 0x1212, 0x1313, 0x1414, 0x1515, 0x1616, 0x1717,
-	0x1818, 0x1919, 0x1a1a, 0x1b1b, 0x1c1c, 0x1d1d, 0x1e1e, 0x1f1f
-};
+	devpriv->act_chanlist_len = seglen;
+	devpriv->act_chanlist_pos = 0;
+
+	for (i = 0; i < seglen; i++) {	/*  store range list to card */
+		chanprog = muxonechan[CR_CHAN(chanlist[i])];
+		outw(chanprog, dev->iobase + PCI171x_MUX); /* select channel */
+		range = this_board->rangecode_ai[CR_RANGE(chanlist[i])];
+		if (CR_AREF(chanlist[i]) == AREF_DIFF)
+			range |= 0x0020;
+		outw(range, dev->iobase + PCI171x_RANGE); /* select gain */
+#ifdef PCI171x_PARANOIDCHECK
+		devpriv->act_chanlist[i] =
+			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
+#endif
+	}
+#ifdef PCI171x_PARANOIDCHECK
+	for ( ; i < n_chan; i++) { /* store remainder of channel list */
+		devpriv->act_chanlist[i] =
+			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
+	}
+#endif
+
+	devpriv->ai_et_MuxVal =
+		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
+	/* select channel interval to scan */
+	outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
+}
 
 /*
 ==============================================================================
@@ -313,12 +435,13 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1710_private *devpriv = dev->private;
 	int n, timeout;
 #ifdef PCI171x_PARANOIDCHECK
+	const struct boardtype *this_board = comedi_board(dev);
 	unsigned int idata;
 #endif
 
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_insn_read_ai(...)\n");
 	devpriv->CntrlReg &= Control_CNT0;
 	devpriv->CntrlReg |= Control_SW;	/*  set software trigger */
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
@@ -327,32 +450,18 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 
 	setup_channel_list(dev, s, &insn->chanspec, 1, 1);
 
-	DPRINTK("adv_pci1710 A ST=%4x IO=%x\n",
-		inw(dev->iobase + PCI171x_STATUS),
-		dev->iobase + PCI171x_STATUS);
 	for (n = 0; n < insn->n; n++) {
 		outw(0, dev->iobase + PCI171x_SOFTTRG);	/* start conversion */
-		DPRINTK("adv_pci1710 B n=%d ST=%4x\n", n,
-			inw(dev->iobase + PCI171x_STATUS));
 		/* udelay(1); */
-		DPRINTK("adv_pci1710 C n=%d ST=%4x\n", n,
-			inw(dev->iobase + PCI171x_STATUS));
 		timeout = 100;
 		while (timeout--) {
 			if (!(inw(dev->iobase + PCI171x_STATUS) & Status_FE))
 				goto conv_finish;
-			if (!(timeout % 10))
-				DPRINTK("adv_pci1710 D n=%d tm=%d ST=%4x\n", n,
-					timeout,
-					inw(dev->iobase + PCI171x_STATUS));
 		}
 		comedi_error(dev, "A/D insn timeout");
 		outb(0, dev->iobase + PCI171x_CLRFIFO);
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		data[n] = 0;
-		DPRINTK
-		    ("adv_pci1710 EDBG: END: pci171x_insn_read_ai(...) n=%d\n",
-		     n);
 		return -ETIME;
 
 conv_finish:
@@ -373,7 +482,6 @@ conv_finish:
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	DPRINTK("adv_pci1710 EDBG: END: pci171x_insn_read_ai(...) n=%d\n", n);
 	return n;
 }
 
@@ -384,6 +492,7 @@ static int pci171x_insn_write_ao(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1710_private *devpriv = dev->private;
 	int n, chan, range, ofs;
 
 	chan = CR_CHAN(insn->chanspec);
@@ -416,6 +525,7 @@ static int pci171x_insn_read_ao(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1710_private *devpriv = dev->private;
 	int n, chan;
 
 	chan = CR_CHAN(insn->chanspec);
@@ -457,6 +567,23 @@ static int pci171x_insn_bits_do(struct comedi_device *dev,
 /*
 ==============================================================================
 */
+static void start_pacer(struct comedi_device *dev, int mode,
+			unsigned int divisor1, unsigned int divisor2)
+{
+	outw(0xb4, dev->iobase + PCI171x_CNTCTRL);
+	outw(0x74, dev->iobase + PCI171x_CNTCTRL);
+
+	if (mode == 1) {
+		outw(divisor2 & 0xff, dev->iobase + PCI171x_CNT2);
+		outw((divisor2 >> 8) & 0xff, dev->iobase + PCI171x_CNT2);
+		outw(divisor1 & 0xff, dev->iobase + PCI171x_CNT1);
+		outw((divisor1 >> 8) & 0xff, dev->iobase + PCI171x_CNT1);
+	}
+}
+
+/*
+==============================================================================
+*/
 static int pci171x_insn_counter_read(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     struct comedi_insn *insn,
@@ -486,6 +613,7 @@ static int pci171x_insn_counter_write(struct comedi_device *dev,
 				      struct comedi_insn *insn,
 				      unsigned int *data)
 {
+	struct pci1710_private *devpriv = dev->private;
 	uint msb, lsb, ccntrl, status;
 
 	lsb = data[0] & 0x00FF;
@@ -517,6 +645,7 @@ static int pci171x_insn_counter_config(struct comedi_device *dev,
 {
 #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;
@@ -552,6 +681,7 @@ static int pci1720_insn_write_ao(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1710_private *devpriv = dev->private;
 	int n, rangereg, chan;
 
 	chan = CR_CHAN(insn->chanspec);
@@ -575,16 +705,47 @@ static int pci1720_insn_write_ao(struct comedi_device *dev,
 /*
 ==============================================================================
 */
+static int pci171x_ai_cancel(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+
+	switch (this_board->cardtype) {
+	default:
+		devpriv->CntrlReg &= Control_CNT0;
+		devpriv->CntrlReg |= Control_SW;
+
+		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
+		start_pacer(dev, -1, 0, 0);
+		outb(0, dev->iobase + PCI171x_CLRFIFO);
+		outb(0, dev->iobase + PCI171x_CLRINT);
+		break;
+	}
+
+	devpriv->ai_do = 0;
+	devpriv->ai_act_scan = 0;
+	s->async->cur_chan = 0;
+	devpriv->ai_buf_ptr = 0;
+	devpriv->neverending_ai = 0;
+
+	return 0;
+}
+
+/*
+==============================================================================
+*/
 static void interrupt_pci1710_every_sample(void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct pci1710_private *devpriv = dev->private;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int m;
 #ifdef PCI171x_PARANOIDCHECK
+	const struct boardtype *this_board = comedi_board(dev);
 	short sampl;
 #endif
 
-	DPRINTK("adv_pci1710 EDBG: BGN: interrupt_pci1710_every_sample(...)\n");
 	m = inw(dev->iobase + PCI171x_STATUS);
 	if (m & Status_FE) {
 		printk("comedi%d: A/D FIFO empty (%4x)\n", dev->minor, m);
@@ -605,11 +766,9 @@ static void interrupt_pci1710_every_sample(void *d)
 
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
-	DPRINTK("FOR ");
 	for (; !(inw(dev->iobase + PCI171x_STATUS) & Status_FE);) {
 #ifdef PCI171x_PARANOIDCHECK
 		sampl = inw(dev->iobase + PCI171x_AD_DATA);
-		DPRINTK("%04x:", sampl);
 		if (this_board->cardtype != TYPE_PCI1713)
 			if ((sampl & 0xf000) !=
 			    devpriv->act_chanlist[s->async->cur_chan]) {
@@ -626,8 +785,6 @@ static void interrupt_pci1710_every_sample(void *d)
 				comedi_event(dev, s);
 				return;
 			}
-		DPRINTK("%8d %2d %8d~", s->async->buf_int_ptr,
-			s->async->cur_chan, s->async->buf_int_count);
 		comedi_buf_put(s->async, sampl & 0x0fff);
 #else
 		comedi_buf_put(s->async,
@@ -641,11 +798,6 @@ static void interrupt_pci1710_every_sample(void *d)
 
 		if (s->async->cur_chan == 0) {	/*  one scan done */
 			devpriv->ai_act_scan++;
-			DPRINTK
-			    ("adv_pci1710 EDBG: EOS1 bic %d bip %d buc %d bup %d\n",
-			     s->async->buf_int_count, s->async->buf_int_ptr,
-			     s->async->buf_user_count, s->async->buf_user_ptr);
-			DPRINTK("adv_pci1710 EDBG: EOS2\n");
 			if ((!devpriv->neverending_ai) &&
 			    (devpriv->ai_act_scan >= devpriv->ai_scans)) {
 				/*  all data sampled */
@@ -658,7 +810,6 @@ static void interrupt_pci1710_every_sample(void *d)
 	}
 
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
-	DPRINTK("adv_pci1710 EDBG: END: interrupt_pci1710_every_sample(...)\n");
 
 	comedi_event(dev, s);
 }
@@ -669,12 +820,13 @@ static void interrupt_pci1710_every_sample(void *d)
 static int move_block_from_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s, int n, int turn)
 {
+	struct pci1710_private *devpriv = dev->private;
 	int i, j;
 #ifdef PCI171x_PARANOIDCHECK
+	const struct boardtype *this_board = comedi_board(dev);
 	int sampl;
 #endif
-	DPRINTK("adv_pci1710 EDBG: BGN: move_block_from_fifo(...,%d,%d)\n", n,
-		turn);
+
 	j = s->async->cur_chan;
 	for (i = 0; i < n; i++) {
 #ifdef PCI171x_PARANOIDCHECK
@@ -705,7 +857,6 @@ static int move_block_from_fifo(struct comedi_device *dev,
 		}
 	}
 	s->async->cur_chan = j;
-	DPRINTK("adv_pci1710 EDBG: END: move_block_from_fifo(...)\n");
 	return 0;
 }
 
@@ -715,10 +866,11 @@ static int move_block_from_fifo(struct comedi_device *dev,
 static void interrupt_pci1710_half_fifo(void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int m, samplesinbuf;
 
-	DPRINTK("adv_pci1710 EDBG: BGN: interrupt_pci1710_half_fifo(...)\n");
 	m = inw(dev->iobase + PCI171x_STATUS);
 	if (!(m & Status_FH)) {
 		printk("comedi%d: A/D FIFO not half full! (%4x)\n",
@@ -760,7 +912,6 @@ static void interrupt_pci1710_half_fifo(void *d)
 			return;
 		}
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
-	DPRINTK("adv_pci1710 EDBG: END: interrupt_pci1710_half_fifo(...)\n");
 
 	comedi_event(dev, s);
 }
@@ -771,18 +922,14 @@ static void interrupt_pci1710_half_fifo(void *d)
 static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct pci1710_private *devpriv = dev->private;
 
-	DPRINTK("adv_pci1710 EDBG: BGN: interrupt_service_pci1710(%d,...)\n",
-		irq);
 	if (!dev->attached)	/*  is device attached? */
 		return IRQ_NONE;	/*  no, exit */
 	/*  is this interrupt from our board? */
 	if (!(inw(dev->iobase + PCI171x_STATUS) & Status_IRQ))
 		return IRQ_NONE;	/*  no, exit */
 
-	DPRINTK("adv_pci1710 EDBG: interrupt_service_pci1710() ST: %4x\n",
-		inw(dev->iobase + PCI171x_STATUS));
-
 	if (devpriv->ai_et) {	/*  Switch from initial TRIG_EXT to TRIG_xxx. */
 		devpriv->ai_et = 0;
 		devpriv->CntrlReg &= Control_CNT0;
@@ -802,7 +949,6 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 	} else {
 		interrupt_pci1710_half_fifo(d);
 	}
-	DPRINTK("adv_pci1710 EDBG: END: interrupt_service_pci1710(...)\n");
 	return IRQ_HANDLED;
 }
 
@@ -812,11 +958,11 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
 				     struct comedi_subdevice *s)
 {
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
 	unsigned int divisor1 = 0, divisor2 = 0;
 	unsigned int seglen;
 
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_ai_docmd_and_mode(%d,...)\n",
-		mode);
 	start_pacer(dev, -1, 0, 0);	/*  stop pacer */
 
 	seglen = check_channel_list(dev, s, devpriv->ai_chanlist,
@@ -869,10 +1015,6 @@ static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
 		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base, &divisor1,
 					  &divisor2, &devpriv->ai_timer1,
 					  devpriv->ai_flags & TRIG_ROUND_MASK);
-		DPRINTK
-		    ("adv_pci1710 EDBG: OSC base=%u div1=%u div2=%u timer=%u\n",
-		     devpriv->i8254_osc_base, divisor1, divisor2,
-		     devpriv->ai_timer1);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
 		if (mode != 2) {
 			/*  start pacer */
@@ -888,27 +1030,9 @@ static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
 		break;
 	}
 
-	DPRINTK("adv_pci1710 EDBG: END: pci171x_ai_docmd_and_mode(...)\n");
 	return 0;
 }
 
-#ifdef PCI171X_EXTDEBUG
-/*
-==============================================================================
-*/
-static void pci171x_cmdtest_out(int e, struct comedi_cmd *cmd)
-{
-	printk("adv_pci1710 e=%d startsrc=%x scansrc=%x convsrc=%x\n", e,
-	       cmd->start_src, cmd->scan_begin_src, cmd->convert_src);
-	printk("adv_pci1710 e=%d startarg=%d scanarg=%d convarg=%d\n", e,
-	       cmd->start_arg, cmd->scan_begin_arg, cmd->convert_arg);
-	printk("adv_pci1710 e=%d stopsrc=%x scanend=%x\n", e, cmd->stop_src,
-	       cmd->scan_end_src);
-	printk("adv_pci1710 e=%d stoparg=%d scanendarg=%d chanlistlen=%d\n",
-	       e, cmd->stop_arg, cmd->scan_end_arg, cmd->chanlist_len);
-}
-#endif
-
 /*
 ==============================================================================
 */
@@ -916,83 +1040,33 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
 	int err = 0;
 	int tmp;
 	unsigned int divisor1 = 0, divisor2 = 0;
 
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...)\n");
-#ifdef PCI171X_EXTDEBUG
-	pci171x_cmdtest_out(-1, cmd);
-#endif
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	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);
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
-
-	if (err) {
-#ifdef PCI171X_EXTDEBUG
-		pci171x_cmdtest_out(1, cmd);
-#endif
-		DPRINTK(
-		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=1\n",
-		err);
+	if (err)
 		return 1;
-	}
-
-	/* step2: make sure trigger srcs are unique and mutually compatible */
-
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
 
-	if (cmd->scan_begin_src != TRIG_FOLLOW) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
+	/* step 2a: make sure trigger sources are unique */
 
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
+	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);
 
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	/* step 2b: and mutually compatible */
 
-	if (err) {
-#ifdef PCI171X_EXTDEBUG
-		pci171x_cmdtest_out(2, cmd);
-#endif
-		DPRINTK(
-		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=2\n",
-		err);
+	if (err)
 		return 2;
-	}
 
 	/* step 3: make sure arguments are trivially compatible */
 
@@ -1034,15 +1108,8 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 		}
 	}
 
-	if (err) {
-#ifdef PCI171X_EXTDEBUG
-		pci171x_cmdtest_out(3, cmd);
-#endif
-		DPRINTK(
-		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=3\n",
-		err);
+	if (err)
 		return 3;
-	}
 
 	/* step 4: fix up any arguments */
 
@@ -1057,12 +1124,8 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 			err++;
 	}
 
-	if (err) {
-		DPRINTK
-		    ("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=4\n",
-		     err);
+	if (err)
 		return 4;
-	}
 
 	/* step 5: complain about special chanlist considerations */
 
@@ -1072,7 +1135,6 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 			return 5;	/*  incorrect channels list */
 	}
 
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) ret=0\n");
 	return 0;
 }
 
@@ -1081,9 +1143,9 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 */
 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;
 
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_ai_cmd(...)\n");
 	devpriv->ai_n_chan = cmd->chanlist_len;
 	devpriv->ai_chanlist = cmd->chanlist;
 	devpriv->ai_flags = cmd->flags;
@@ -1115,162 +1177,12 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 /*
 ==============================================================================
- Check if channel list from user is builded correctly
- If it's ok, then program scan/gain logic.
- This works for all cards.
-*/
-static int check_channel_list(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      unsigned int *chanlist, unsigned int n_chan)
-{
-	unsigned int chansegment[32];
-	unsigned int i, nowmustbechan, seglen, segpos;
-
-	DPRINTK("adv_pci1710 EDBG:  check_channel_list(...,%d)\n", n_chan);
-	/* correct channel and range number check itself comedi/range.c */
-	if (n_chan < 1) {
-		comedi_error(dev, "range/channel list is empty!");
-		return 0;
-	}
-
-	if (n_chan == 1)
-		return 1; /* seglen=1 */
-
-	chansegment[0] = chanlist[0]; /*  first channel is every time ok */
-	for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {
-		if (chanlist[0] == chanlist[i])
-			break;	/*  we detected a loop, stop */
-		if ((CR_CHAN(chanlist[i]) & 1) &&
-		    (CR_AREF(chanlist[i]) == AREF_DIFF)) {
-			comedi_error(dev, "Odd channel cannot be differential input!\n");
-			return 0;
-		}
-		nowmustbechan = (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
-		if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
-			nowmustbechan = (nowmustbechan + 1) % s->n_chan;
-		if (nowmustbechan != CR_CHAN(chanlist[i])) {
-			printk("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
-			       i, CR_CHAN(chanlist[i]), nowmustbechan,
-			       CR_CHAN(chanlist[0]));
-			return 0;
-		}
-		chansegment[i] = chanlist[i]; /* next correct channel in list */
-	}
-
-	for (i = 0, segpos = 0; i < n_chan; i++) {
-		if (chanlist[i] != chansegment[i % seglen]) {
-			printk("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
-			       i, CR_CHAN(chansegment[i]),
-			       CR_RANGE(chansegment[i]),
-			       CR_AREF(chansegment[i]),
-			       CR_CHAN(chanlist[i % seglen]),
-			       CR_RANGE(chanlist[i % seglen]),
-			       CR_AREF(chansegment[i % seglen]));
-			return 0;
-		}
-	}
-	return seglen;
-}
-
-static void setup_channel_list(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       unsigned int *chanlist, unsigned int n_chan,
-			       unsigned int seglen)
-{
-	unsigned int i, range, chanprog;
-
-	DPRINTK("adv_pci1710 EDBG:  setup_channel_list(...,%d,%d)\n", n_chan,
-		seglen);
-	devpriv->act_chanlist_len = seglen;
-	devpriv->act_chanlist_pos = 0;
-
-	DPRINTK("SegLen: %d\n", seglen);
-	for (i = 0; i < seglen; i++) {	/*  store range list to card */
-		chanprog = muxonechan[CR_CHAN(chanlist[i])];
-		outw(chanprog, dev->iobase + PCI171x_MUX); /* select channel */
-		range = this_board->rangecode_ai[CR_RANGE(chanlist[i])];
-		if (CR_AREF(chanlist[i]) == AREF_DIFF)
-			range |= 0x0020;
-		outw(range, dev->iobase + PCI171x_RANGE); /* select gain */
-#ifdef PCI171x_PARANOIDCHECK
-		devpriv->act_chanlist[i] =
-			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
-#endif
-		DPRINTK("GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
-			devpriv->act_chanlist[i]);
-	}
-#ifdef PCI171x_PARANOIDCHECK
-	for ( ; i < n_chan; i++) { /* store remainder of channel list */
-		devpriv->act_chanlist[i] =
-			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
-	}
-#endif
-
-	devpriv->ai_et_MuxVal =
-		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
-	/* select channel interval to scan */
-	outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
-	DPRINTK("MUX: %4x L%4x.H%4x\n",
-		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8),
-		CR_CHAN(chanlist[0]), CR_CHAN(chanlist[seglen - 1]));
-}
-
-/*
-==============================================================================
-*/
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
-{
-	DPRINTK("adv_pci1710 EDBG: BGN: start_pacer(%d,%u,%u)\n", mode,
-		divisor1, divisor2);
-	outw(0xb4, dev->iobase + PCI171x_CNTCTRL);
-	outw(0x74, dev->iobase + PCI171x_CNTCTRL);
-
-	if (mode == 1) {
-		outw(divisor2 & 0xff, dev->iobase + PCI171x_CNT2);
-		outw((divisor2 >> 8) & 0xff, dev->iobase + PCI171x_CNT2);
-		outw(divisor1 & 0xff, dev->iobase + PCI171x_CNT1);
-		outw((divisor1 >> 8) & 0xff, dev->iobase + PCI171x_CNT1);
-	}
-	DPRINTK("adv_pci1710 EDBG: END: start_pacer(...)\n");
-}
-
-/*
-==============================================================================
-*/
-static int pci171x_ai_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_ai_cancel(...)\n");
-
-	switch (this_board->cardtype) {
-	default:
-		devpriv->CntrlReg &= Control_CNT0;
-		devpriv->CntrlReg |= Control_SW;
-
-		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
-		start_pacer(dev, -1, 0, 0);
-		outb(0, dev->iobase + PCI171x_CLRFIFO);
-		outb(0, dev->iobase + PCI171x_CLRINT);
-		break;
-	}
-
-	devpriv->ai_do = 0;
-	devpriv->ai_act_scan = 0;
-	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->neverending_ai = 0;
-
-	DPRINTK("adv_pci1710 EDBG: END: pci171x_ai_cancel(...)\n");
-	return 0;
-}
-
-/*
-==============================================================================
 */
 static int pci171x_reset(struct comedi_device *dev)
 {
-	DPRINTK("adv_pci1710 EDBG: BGN: pci171x_reset(...)\n");
+	const struct boardtype *this_board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+
 	outw(0x30, dev->iobase + PCI171x_CNTCTRL);
 	devpriv->CntrlReg = Control_SW | Control_CNT0;	/*  Software trigger, CNT0=external */
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
@@ -1291,7 +1203,6 @@ static int pci171x_reset(struct comedi_device *dev)
 	outb(0, dev->iobase + PCI171x_CLRFIFO);	/*  clear FIFO */
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear INT request */
 
-	DPRINTK("adv_pci1710 EDBG: END: pci171x_reset(...)\n");
 	return 0;
 }
 
@@ -1300,7 +1211,8 @@ static int pci171x_reset(struct comedi_device *dev)
 */
 static int pci1720_reset(struct comedi_device *dev)
 {
-	DPRINTK("adv_pci1710 EDBG: BGN: pci1720_reset(...)\n");
+	struct pci1710_private *devpriv = dev->private;
+
 	outb(Syncont_SC0, dev->iobase + PCI1720_SYNCONT);	/*  set synchronous output mode */
 	devpriv->da_ranges = 0xAA;
 	outb(devpriv->da_ranges, dev->iobase + PCI1720_RANGE);	/*  set all ranges to +/-5V */
@@ -1313,7 +1225,6 @@ static int pci1720_reset(struct comedi_device *dev)
 	devpriv->ao_data[1] = 0x0800;
 	devpriv->ao_data[2] = 0x0800;
 	devpriv->ao_data[3] = 0x0800;
-	DPRINTK("adv_pci1710 EDBG: END: pci1720_reset(...)\n");
 	return 0;
 }
 
@@ -1322,82 +1233,55 @@ static int pci1720_reset(struct comedi_device *dev)
 */
 static int pci1710_reset(struct comedi_device *dev)
 {
-	DPRINTK("adv_pci1710 EDBG: BGN: pci1710_reset(...)\n");
+	const struct boardtype *this_board = comedi_board(dev);
+
 	switch (this_board->cardtype) {
 	case TYPE_PCI1720:
 		return pci1720_reset(dev);
 	default:
 		return pci171x_reset(dev);
 	}
-	DPRINTK("adv_pci1710 EDBG: END: pci1710_reset(...)\n");
 }
 
-static struct pci_dev *pci1710_find_pci_dev(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
+static const void *pci1710_find_boardinfo(struct comedi_device *dev,
+					  struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int board_index = this_board - boardtypes;
+	const struct boardtype *this_board;
 	int i;
 
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_ADVANTECH)
-			continue;
-		if (strcmp(this_board->name, DRV_NAME) == 0) {
-			for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-				if (pcidev->device == boardtypes[i].device_id) {
-					board_index = i;
-					break;
-				}
-			}
-			if (i == ARRAY_SIZE(boardtypes))
-				continue;
-		} else {
-			if (pcidev->device != boardtypes[board_index].device_id)
-				continue;
-		}
-		dev->board_ptr = &boardtypes[board_index];
-		return pcidev;
+	for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
+		this_board = &boardtypes[i];
+		if (pcidev->device == this_board->device_id)
+			return this_board;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int pci1710_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int pci1710_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev;
+	const struct boardtype *this_board;
+	struct pci1710_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, subdev, n_subdevices;
-	unsigned int irq;
 
-	dev_info(dev->class_dev, DRV_NAME ": attach\n");
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	ret = alloc_private(dev, sizeof(struct pci1710_private));
-	if (ret < 0)
-		return -ENOMEM;
+	this_board = pci1710_find_boardinfo(dev, pcidev);
+	if (!this_board)
+		return -ENODEV;
+	dev->board_ptr = this_board;
+	dev->board_name = this_board->name;
 
-	pcidev = pci1710_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
 
-	ret = comedi_pci_enable(pcidev, DRV_NAME);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
 	if (ret)
 		return ret;
-
 	dev->iobase = pci_resource_start(pcidev, 2);
-	irq = pcidev->irq;
-
-	dev->board_name = this_board->name;
 
 	n_subdevices = 0;
 	if (this_board->n_aichan)
@@ -1417,30 +1301,17 @@ static int pci1710_attach(struct comedi_device *dev,
 
 	pci1710_reset(dev);
 
-	if (this_board->have_irq) {
-		if (irq) {
-			if (request_irq(irq, interrupt_service_pci1710,
-					IRQF_SHARED, "Advantech PCI-1710",
-					dev)) {
-				dev_dbg(dev->class_dev,
-					"unable to allocate IRQ %d, DISABLING IT",
-					irq);
-				irq = 0;	/* Can't use IRQ */
-			} else {
-				dev_dbg(dev->class_dev, "irq=%u", irq);
-			}
-		} else {
-			dev_dbg(dev->class_dev, "IRQ disabled");
-		}
-	} else {
-		irq = 0;
+	if (this_board->have_irq && pcidev->irq) {
+		ret = request_irq(pcidev->irq, interrupt_service_pci1710,
+				  IRQF_SHARED, dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = pcidev->irq;
 	}
 
-	dev->irq = irq;
 	subdev = 0;
 
 	if (this_board->n_aichan) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		dev->read_subdev = s;
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND;
@@ -1452,7 +1323,7 @@ static int pci1710_attach(struct comedi_device *dev,
 		s->range_table = this_board->rangelist_ai;
 		s->cancel = pci171x_ai_cancel;
 		s->insn_read = pci171x_insn_read_ai;
-		if (irq) {
+		if (dev->irq) {
 			s->subdev_flags |= SDF_CMD_READ;
 			s->do_cmdtest = pci171x_ai_cmdtest;
 			s->do_cmd = pci171x_ai_cmd;
@@ -1462,7 +1333,7 @@ static int pci1710_attach(struct comedi_device *dev,
 	}
 
 	if (this_board->n_aochan) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan = this_board->n_aochan;
@@ -1482,7 +1353,7 @@ static int pci1710_attach(struct comedi_device *dev,
 	}
 
 	if (this_board->n_dichan) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan = this_board->n_dichan;
@@ -1495,7 +1366,7 @@ static int pci1710_attach(struct comedi_device *dev,
 	}
 
 	if (this_board->n_dochan) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan = this_board->n_dochan;
@@ -1510,7 +1381,7 @@ static int pci1710_attach(struct comedi_device *dev,
 	}
 
 	if (this_board->n_counter) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_COUNTER;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 		s->n_chan = this_board->n_counter;
@@ -1523,7 +1394,8 @@ static int pci1710_attach(struct comedi_device *dev,
 		subdev++;
 	}
 
-	devpriv->valid = 1;
+	dev_info(dev->class_dev, "%s attached, irq %sabled\n",
+		dev->board_name, dev->irq ? "en" : "dis");
 
 	return 0;
 }
@@ -1532,27 +1404,21 @@ static void pci1710_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	if (dev->private) {
-		if (devpriv->valid)
-			pci1710_reset(dev);
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-	}
+	if (dev->iobase)
+		pci1710_reset(dev);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver adv_pci1710_driver = {
 	.driver_name	= "adv_pci1710",
 	.module		= THIS_MODULE,
-	.attach		= pci1710_attach,
+	.attach_pci	= pci1710_attach_pci,
 	.detach		= pci1710_detach,
-	.num_names	= ARRAY_SIZE(boardtypes),
-	.board_name	= &boardtypes[0].name,
-	.offset		= sizeof(struct boardtype),
 };
 
 static int __devinit adv_pci1710_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index dfde0f6328dd..df4efc0606de 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -52,11 +52,6 @@ TODO:
 
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe	/* Advantech PCI vendor ID */
 
-/* hardware types of the cards */
-#define TYPE_PCI1723 0
-
-#define IORANGE_1723  0x2A
-
 /* all the registers for the pci1723 board */
 #define PCI1723_DA(N)   ((N)<<1)	/* W: D/A register N (0 to 7) */
 
@@ -112,63 +107,18 @@ TODO:
 
 #define PCI1723_SELECT_CALIBRATION 0x28	/* Select the calibration Ref_V */
 
-/* static unsigned short pci_list_builded=0;      =1 list of card is know */
-
-static const struct comedi_lrange range_pci1723 = { 1, {
-							BIP_RANGE(10)
-							}
-};
-
-/*
- * Board descriptions for pci1723 boards.
- */
-struct pci1723_board {
-	const char *name;
-	int vendor_id;		/* PCI vendor a device ID of card */
-	int device_id;
-	int iorange;
-	char cardtype;
-	int n_aochan;		/* num of D/A chans */
-	int n_diochan;		/* num of DIO chans */
-	int ao_maxdata;		/* resolution of D/A */
-	const struct comedi_lrange *rangelist_ao;	/* rangelist for D/A */
-};
-
-static const struct pci1723_board boardtypes[] = {
-	{
-	 .name = "pci1723",
-	 .vendor_id = PCI_VENDOR_ID_ADVANTECH,
-	 .device_id = 0x1723,
-	 .iorange = IORANGE_1723,
-	 .cardtype = TYPE_PCI1723,
-	 .n_aochan = 8,
-	 .n_diochan = 16,
-	 .ao_maxdata = 0xffff,
-	 .rangelist_ao = &range_pci1723,
-	 },
-};
-
-/* This structure is for data unique to this hardware driver. */
 struct pci1723_private {
-	int valid;		/* card is usable; */
-
 	unsigned char da_range[8];	/* D/A output range for each channel */
-
 	short ao_data[8];	/* data output buffer */
 };
 
-/* The following macro to make it easy to access the private structure. */
-#define devpriv ((struct pci1723_private *)dev->private)
-
-#define this_board boardtypes
-
 /*
  * The pci1723 card reset;
  */
 static int pci1723_reset(struct comedi_device *dev)
 {
+	struct pci1723_private *devpriv = dev->private;
 	int i;
-	DPRINTK("adv_pci1723 EDBG: BGN: pci1723_reset(...)\n");
 
 	outw(0x01, dev->iobase + PCI1723_SYN_SET);
 					       /* set synchronous output mode */
@@ -190,7 +140,6 @@ static int pci1723_reset(struct comedi_device *dev)
 	/* set asynchronous output mode */
 	outw(0, dev->iobase + PCI1723_SYN_SET);
 
-	DPRINTK("adv_pci1723 EDBG: END: pci1723_reset(...)\n");
 	return 0;
 }
 
@@ -198,10 +147,10 @@ static int pci1723_insn_read_ao(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1723_private *devpriv = dev->private;
 	int n, chan;
 
 	chan = CR_CHAN(insn->chanspec);
-	DPRINTK(" adv_PCI1723 DEBUG: pci1723_insn_read_ao() -----\n");
 	for (n = 0; n < insn->n; n++)
 		data[n] = devpriv->ao_data[chan];
 
@@ -215,11 +164,10 @@ static int pci1723_ao_write_winsn(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci1723_private *devpriv = dev->private;
 	int n, chan;
 	chan = CR_CHAN(insn->chanspec);
 
-	DPRINTK("PCI1723: the pci1723_ao_write_winsn() ------\n");
-
 	for (n = 0; n < insn->n; n++) {
 
 		devpriv->ao_data[chan] = data[n];
@@ -286,124 +234,73 @@ static int pci1723_dio_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static struct pci_dev *pci1723_find_pci_dev(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
+static int pci1723_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_ADVANTECH)
-			continue;
-		return pcidev;
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-static int pci1723_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
+	struct pci1723_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, subdev, n_subdevices;
+	int ret;
 
-	printk(KERN_ERR "comedi%d: adv_pci1723: board=%s",
-						dev->minor, this_board->name);
-
-	ret = alloc_private(dev, sizeof(struct pci1723_private));
-	if (ret < 0) {
-		printk(" - Allocation failed!\n");
-		return -ENOMEM;
-	}
-
-	pcidev = pci1723_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
+	dev->board_name = dev->driver->driver_name;
 
-	ret = comedi_pci_enable(pcidev, "adv_pci1723");
-	if (ret)
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
 		return ret;
+	devpriv = dev->private;
 
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
 
-	dev->board_name = this_board->name;
-
-	n_subdevices = 0;
-
-	if (this_board->n_aochan)
-		n_subdevices++;
-	if (this_board->n_diochan)
-		n_subdevices++;
-
-	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
 		return ret;
 
-	pci1723_reset(dev);
-	subdev = 0;
-	if (this_board->n_aochan) {
-		s = dev->subdevices + subdev;
-		dev->write_subdev = s;
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = this_board->n_aochan;
-		s->maxdata = this_board->ao_maxdata;
-		s->len_chanlist = this_board->n_aochan;
-		s->range_table = this_board->rangelist_ao;
-
-		s->insn_write = pci1723_ao_write_winsn;
-		s->insn_read = pci1723_insn_read_ao;
-
-		/* read DIO config */
-		switch (inw(dev->iobase + PCI1723_DIGITAL_IO_PORT_MODE)
-								       & 0x03) {
-		case 0x00:	/* low byte output, high byte output */
-			s->io_bits = 0xFFFF;
-			break;
-		case 0x01:	/* low byte input, high byte output */
-			s->io_bits = 0xFF00;
-			break;
-		case 0x02:	/* low byte output, high byte input */
-			s->io_bits = 0x00FF;
-			break;
-		case 0x03:	/* low byte input, high byte input */
-			s->io_bits = 0x0000;
-			break;
-		}
-		/* read DIO port state */
-		s->state = inw(dev->iobase + PCI1723_READ_DIGITAL_INPUT_DATA);
-
-		subdev++;
-	}
-
-	if (this_board->n_diochan) {
-		s = dev->subdevices + subdev;
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags =
-		    SDF_READABLE | SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = this_board->n_diochan;
-		s->maxdata = 1;
-		s->len_chanlist = this_board->n_diochan;
-		s->range_table = &range_digital;
-		s->insn_config = pci1723_dio_insn_config;
-		s->insn_bits = pci1723_dio_insn_bits;
-		subdev++;
+	s = &dev->subdevices[0];
+	dev->write_subdev = s;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan	= 8;
+	s->maxdata	= 0xffff;
+	s->len_chanlist	= 8;
+	s->range_table	= &range_bipolar10;
+	s->insn_write	= pci1723_ao_write_winsn;
+	s->insn_read	= pci1723_insn_read_ao;
+
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->len_chanlist	= 16;
+	s->range_table	= &range_digital;
+	s->insn_config	= pci1723_dio_insn_config;
+	s->insn_bits	= pci1723_dio_insn_bits;
+
+	/* read DIO config */
+	switch (inw(dev->iobase + PCI1723_DIGITAL_IO_PORT_MODE) & 0x03) {
+	case 0x00:	/* low byte output, high byte output */
+		s->io_bits = 0xFFFF;
+		break;
+	case 0x01:	/* low byte input, high byte output */
+		s->io_bits = 0xFF00;
+		break;
+	case 0x02:	/* low byte output, high byte input */
+		s->io_bits = 0x00FF;
+		break;
+	case 0x03:	/* low byte input, high byte input */
+		s->io_bits = 0x0000;
+		break;
 	}
-
-	devpriv->valid = 1;
+	/* read DIO port state */
+	s->state = inw(dev->iobase + PCI1723_READ_DIGITAL_INPUT_DATA);
 
 	pci1723_reset(dev);
 
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+
 	return 0;
 }
 
@@ -411,21 +308,18 @@ static void pci1723_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	if (dev->private) {
-		if (devpriv->valid)
-			pci1723_reset(dev);
-	}
 	if (pcidev) {
-		if (dev->iobase)
+		if (dev->iobase) {
+			pci1723_reset(dev);
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
+		}
 	}
 }
 
 static struct comedi_driver adv_pci1723_driver = {
 	.driver_name	= "adv_pci1723",
 	.module		= THIS_MODULE,
-	.attach		= pci1723_attach,
+	.attach_pci	= pci1723_attach_pci,
 	.detach		= pci1723_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 2d4cb7f638b2..a3c22419cd5f 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -36,15 +36,6 @@ Configuration options:
 #include "8255.h"
 #include "8253.h"
 
-#undef PCI_DIO_EXTDEBUG		/* if defined, enable extensive debug logging */
-
-#undef DPRINTK
-#ifdef PCI_DIO_EXTDEBUG
-#define DPRINTK(fmt, args...) printk(fmt, ## args)
-#else
-#define DPRINTK(fmt, args...)
-#endif
-
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe
 
 /* hardware types of the cards */
@@ -250,6 +241,7 @@ struct dio_boardtype {
 	int device_id;
 	int main_pci_region;	/*  main I/O PCI region */
 	enum hw_cards_id cardtype;
+	int nsubdevs;
 	struct diosubd_data sdi[MAX_DI_SUBDEVS];	/*  DI chans */
 	struct diosubd_data sdo[MAX_DO_SUBDEVS];	/*  DO chans */
 	struct diosubd_data sdio[MAX_DIO_SUBDEVG];	/*  DIO 8255 chans */
@@ -259,126 +251,164 @@ struct dio_boardtype {
 };
 
 static const struct dio_boardtype boardtypes[] = {
-	{"pci1730", PCI_VENDOR_ID_ADVANTECH, 0x1730, PCIDIO_MAINREG,
-	 TYPE_PCI1730,
-	 { {16, PCI1730_DI, 2, 0}, {16, PCI1730_IDI, 2, 0} },
-	 { {16, PCI1730_DO, 2, 0}, {16, PCI1730_IDO, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1733", PCI_VENDOR_ID_ADVANTECH, 0x1733, PCIDIO_MAINREG,
-	 TYPE_PCI1733,
-	 { {0, 0, 0, 0}, {32, PCI1733_IDI, 4, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1734", PCI_VENDOR_ID_ADVANTECH, 0x1734, PCIDIO_MAINREG,
-	 TYPE_PCI1734,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {32, PCI1734_IDO, 4, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1735", PCI_VENDOR_ID_ADVANTECH, 0x1735, PCIDIO_MAINREG,
-	 TYPE_PCI1735,
-	 { {32, PCI1735_DI, 4, 0}, {0, 0, 0, 0} },
-	 { {32, PCI1735_DO, 4, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { 4, PCI1735_BOARDID, 1, SDF_INTERNAL},
-	 { {3, PCI1735_C8254, 1, 0} },
-	 IO_8b},
-	{"pci1736", PCI_VENDOR_ID_ADVANTECH, 0x1736, PCI1736_MAINREG,
-	 TYPE_PCI1736,
-	 { {0, 0, 0, 0}, {16, PCI1736_IDI, 2, 0} },
-	 { {0, 0, 0, 0}, {16, PCI1736_IDO, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI1736_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1739", PCI_VENDOR_ID_ADVANTECH, 0x1739, PCIDIO_MAINREG,
-	 TYPE_PCI1739,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {48, PCI1739_DIO, 2, 0}, {0, 0, 0, 0} },
-	 {0, 0, 0, 0},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1750", PCI_VENDOR_ID_ADVANTECH, 0x1750, PCIDIO_MAINREG,
-	 TYPE_PCI1750,
-	 { {0, 0, 0, 0}, {16, PCI1750_IDI, 2, 0} },
-	 { {0, 0, 0, 0}, {16, PCI1750_IDO, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {0, 0, 0, 0},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1751", PCI_VENDOR_ID_ADVANTECH, 0x1751, PCIDIO_MAINREG,
-	 TYPE_PCI1751,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {48, PCI1751_DIO, 2, 0}, {0, 0, 0, 0} },
-	 {0, 0, 0, 0},
-	 { {3, PCI1751_CNT, 1, 0} },
-	 IO_8b},
-	{"pci1752", PCI_VENDOR_ID_ADVANTECH, 0x1752, PCIDIO_MAINREG,
-	 TYPE_PCI1752,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {32, PCI1752_IDO, 2, 0}, {32, PCI1752_IDO2, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_16b},
-	{"pci1753", PCI_VENDOR_ID_ADVANTECH, 0x1753, PCIDIO_MAINREG,
-	 TYPE_PCI1753,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {96, PCI1753_DIO, 4, 0}, {0, 0, 0, 0} },
-	 {0, 0, 0, 0},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1753e", PCI_VENDOR_ID_ADVANTECH, 0x1753, PCIDIO_MAINREG,
-	 TYPE_PCI1753E,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {96, PCI1753_DIO, 4, 0}, {96, PCI1753E_DIO, 4, 0} },
-	 {0, 0, 0, 0},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1754", PCI_VENDOR_ID_ADVANTECH, 0x1754, PCIDIO_MAINREG,
-	 TYPE_PCI1754,
-	 { {32, PCI1754_IDI, 2, 0}, {32, PCI1754_IDI2, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_16b},
-	{"pci1756", PCI_VENDOR_ID_ADVANTECH, 0x1756, PCIDIO_MAINREG,
-	 TYPE_PCI1756,
-	 { {0, 0, 0, 0}, {32, PCI1756_IDI, 2, 0} },
-	 { {0, 0, 0, 0}, {32, PCI1756_IDO, 2, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_16b},
-	{"pci1760", PCI_VENDOR_ID_ADVANTECH, 0x1760, 0,
-	 TYPE_PCI1760,
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} }, /* This card have own setup work */
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {0, 0, 0, 0},
-	 { {0, 0, 0, 0} },
-	 IO_8b},
-	{"pci1762", PCI_VENDOR_ID_ADVANTECH, 0x1762, PCIDIO_MAINREG,
-	 TYPE_PCI1762,
-	 { {0, 0, 0, 0}, {16, PCI1762_IDI, 1, 0} },
-	 { {0, 0, 0, 0}, {16, PCI1762_RO, 1, 0} },
-	 { {0, 0, 0, 0}, {0, 0, 0, 0} },
-	 {4, PCI1762_BOARDID, 1, SDF_INTERNAL},
-	 { {0, 0, 0, 0} },
-	 IO_16b}
+	{
+		.name		= "pci1730",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1730,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1730,
+		.nsubdevs	= 5,
+		.sdi[0]		= { 16, PCI1730_DI, 2, 0, },
+		.sdi[1]		= { 16, PCI1730_IDI, 2, 0, },
+		.sdo[0]		= { 16, PCI1730_DO, 2, 0, },
+		.sdo[1]		= { 16, PCI1730_IDO, 2, 0, },
+		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1733",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1733,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1733,
+		.nsubdevs	= 2,
+		.sdi[1]		= { 32, PCI1733_IDI, 4, 0, },
+		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1734",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1734,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1734,
+		.nsubdevs	= 2,
+		.sdo[1]		= { 32, PCI1734_IDO, 4, 0, },
+		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1735",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1735,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1735,
+		.nsubdevs	= 4,
+		.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, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1736",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1736,
+		.main_pci_region = PCI1736_MAINREG,
+		.cardtype	= TYPE_PCI1736,
+		.nsubdevs	= 3,
+		.sdi[1]		= { 16, PCI1736_IDI, 2, 0, },
+		.sdo[1]		= { 16, PCI1736_IDO, 2, 0, },
+		.boardid	= { 4, PCI1736_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1739",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1739,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1739,
+		.nsubdevs	= 2,
+		.sdio[0]	= { 48, PCI1739_DIO, 2, 0, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1750",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1750,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1750,
+		.nsubdevs	= 2,
+		.sdi[1]		= { 16, PCI1750_IDI, 2, 0, },
+		.sdo[1]		= { 16, PCI1750_IDO, 2, 0, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1751",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1751,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1751,
+		.nsubdevs	= 3,
+		.sdio[0]	= { 48, PCI1751_DIO, 2, 0, },
+		.s8254[0]	= { 3, PCI1751_CNT, 1, 0, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1752",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1752,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1752,
+		.nsubdevs	= 3,
+		.sdo[0]		= { 32, PCI1752_IDO, 2, 0, },
+		.sdo[1]		= { 32, PCI1752_IDO2, 2, 0, },
+		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_16b,
+	}, {
+		.name		= "pci1753",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1753,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1753,
+		.nsubdevs	= 4,
+		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1753e",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1753,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1753E,
+		.nsubdevs	= 8,
+		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
+		.sdio[1]	= { 96, PCI1753E_DIO, 4, 0, },
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1754",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1754,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1754,
+		.nsubdevs	= 3,
+		.sdi[0]		= { 32, PCI1754_IDI, 2, 0, },
+		.sdi[1]		= { 32, PCI1754_IDI2, 2, 0, },
+		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_16b,
+	}, {
+		.name		= "pci1756",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1756,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1756,
+		.nsubdevs	= 3,
+		.sdi[1]		= { 32, PCI1756_IDI, 2, 0, },
+		.sdo[1]		= { 32, PCI1756_IDO, 2, 0, },
+		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_16b,
+	}, {
+		/* This card has its own 'attach' */
+		.name		= "pci1760",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1760,
+		.main_pci_region = 0,
+		.cardtype	= TYPE_PCI1760,
+		.nsubdevs	= 4,
+		.io_access	= IO_8b,
+	}, {
+		.name		= "pci1762",
+		.vendor_id	= PCI_VENDOR_ID_ADVANTECH,
+		.device_id	= 0x1762,
+		.main_pci_region = PCIDIO_MAINREG,
+		.cardtype	= TYPE_PCI1762,
+		.nsubdevs	= 3,
+		.sdi[1]		= { 16, PCI1762_IDI, 1, 0, },
+		.sdo[1]		= { 16, PCI1762_RO, 1, 0, },
+		.boardid	= { 4, PCI1762_BOARDID, 1, SDF_INTERNAL, },
+		.io_access	= IO_16b,
+	},
 };
 
 struct pci_dio_private {
@@ -401,9 +431,6 @@ struct pci_dio_private {
 	unsigned short IDIFiltrHigh[8];	/*  IDI's filter value high signal */
 };
 
-#define devpriv ((struct pci_dio_private *)dev->private)
-#define this_board ((const struct dio_boardtype *)dev->board_ptr)
-
 /*
 ==============================================================================
 */
@@ -694,6 +721,7 @@ static int pci1760_insn_cnt_write(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci_dio_private *devpriv = dev->private;
 	int ret;
 	unsigned char chan = CR_CHAN(insn->chanspec) & 0x07;
 	unsigned char bitmask = 1 << chan;
@@ -736,6 +764,7 @@ static int pci1760_insn_cnt_write(struct comedi_device *dev,
 */
 static int pci1760_reset(struct comedi_device *dev)
 {
+	struct pci_dio_private *devpriv = dev->private;
 	int i;
 	unsigned char omb[4] = { 0x00, 0x00, 0x00, 0x00 };
 	unsigned char imb[4];
@@ -816,7 +845,7 @@ static int pci1760_reset(struct comedi_device *dev)
 */
 static int pci_dio_reset(struct comedi_device *dev)
 {
-	DPRINTK("adv_pci_dio EDBG: BGN: pci171x_reset(...)\n");
+	const struct dio_boardtype *this_board = comedi_board(dev);
 
 	switch (this_board->cardtype) {
 	case TYPE_PCI1730:
@@ -917,21 +946,17 @@ static int pci_dio_reset(struct comedi_device *dev)
 		break;
 	}
 
-	DPRINTK("adv_pci_dio EDBG: END: pci171x_reset(...)\n");
-
 	return 0;
 }
 
 /*
 ==============================================================================
 */
-static int pci1760_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int pci1760_attach(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
-	int subdev = 0;
 
-	s = dev->subdevices + subdev;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan = 8;
@@ -939,9 +964,8 @@ static int pci1760_attach(struct comedi_device *dev,
 	s->len_chanlist = 8;
 	s->range_table = &range_digital;
 	s->insn_bits = pci1760_insn_bits_di;
-	subdev++;
 
-	s = dev->subdevices + subdev;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan = 8;
@@ -950,18 +974,16 @@ static int pci1760_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->state = 0;
 	s->insn_bits = pci1760_insn_bits_do;
-	subdev++;
 
-	s = dev->subdevices + subdev;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_TIMER;
 	s->subdev_flags = SDF_WRITABLE | SDF_LSAMPL;
 	s->n_chan = 2;
 	s->maxdata = 0xffffffff;
 	s->len_chanlist = 2;
 /*       s->insn_config=pci1760_insn_pwm_cfg; */
-	subdev++;
 
-	s = dev->subdevices + subdev;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 8;
@@ -970,7 +992,6 @@ static int pci1760_attach(struct comedi_device *dev,
 	s->insn_read = pci1760_insn_cnt_read;
 	s->insn_write = pci1760_insn_cnt_write;
 /*       s->insn_config=pci1760_insn_cnt_cfg; */
-	subdev++;
 
 	return 0;
 }
@@ -978,9 +999,12 @@ static int pci1760_attach(struct comedi_device *dev,
 /*
 ==============================================================================
 */
-static int pci_dio_add_di(struct comedi_device *dev, struct comedi_subdevice *s,
-			  const struct diosubd_data *d, int subdev)
+static int pci_dio_add_di(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  const struct diosubd_data *d)
 {
+	const struct dio_boardtype *this_board = comedi_board(dev);
+
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON | d->specflags;
 	if (d->chans > 16)
@@ -1005,9 +1029,12 @@ static int pci_dio_add_di(struct comedi_device *dev, struct comedi_subdevice *s,
 /*
 ==============================================================================
 */
-static int pci_dio_add_do(struct comedi_device *dev, struct comedi_subdevice *s,
-			  const struct diosubd_data *d, int subdev)
+static int pci_dio_add_do(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  const struct diosubd_data *d)
 {
+	const struct dio_boardtype *this_board = comedi_board(dev);
+
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 	if (d->chans > 16)
@@ -1035,7 +1062,7 @@ static int pci_dio_add_do(struct comedi_device *dev, struct comedi_subdevice *s,
 */
 static int pci_dio_add_8254(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
-			    const struct diosubd_data *d, int subdev)
+			    const struct diosubd_data *d)
 {
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
@@ -1050,101 +1077,69 @@ static int pci_dio_add_8254(struct comedi_device *dev,
 	return 0;
 }
 
-static struct pci_dev *pci_dio_find_pci_dev(struct comedi_device *dev,
-					    struct comedi_devconfig *it)
+static const void *pci_dio_find_boardinfo(struct comedi_device *dev,
+					  struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
+	const struct dio_boardtype *this_board;
 	int i;
 
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-			if (boardtypes[i].vendor_id != pcidev->vendor)
-				continue;
-			if (boardtypes[i].device_id != pcidev->device)
-				continue;
-			dev->board_ptr = boardtypes + i;
-			return pcidev;
-		}
+	for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
+		this_board = &boardtypes[i];
+		if (this_board->vendor_id == pcidev->vendor &&
+		    this_board->device_id == pcidev->device)
+			return this_board;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int pci_dio_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int pci_dio_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev;
+	const struct dio_boardtype *this_board;
+	struct pci_dio_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, subdev, n_subdevices, i, j;
+	int ret, subdev, i, j;
 
-	ret = alloc_private(dev, sizeof(struct pci_dio_private));
-	if (ret < 0)
-		return -ENOMEM;
-
-	pcidev = pci_dio_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
-		dev_err(dev->class_dev,
-			"Error: Can't enable PCI device and request regions!\n");
-		return -EIO;
-	}
-
-	dev->iobase = pci_resource_start(pcidev, this_board->main_pci_region);
+	this_board = pci_dio_find_boardinfo(dev, pcidev);
+	if (!this_board)
+		return -ENODEV;
+	dev->board_ptr = this_board;
 	dev->board_name = this_board->name;
 
-	if (this_board->cardtype == TYPE_PCI1760) {
-		n_subdevices = 4;	/*  8 IDI, 8 IDO, 2 PWM, 8 CNT */
-	} else {
-		n_subdevices = 0;
-		for (i = 0; i < MAX_DI_SUBDEVS; i++)
-			if (this_board->sdi[i].chans)
-				n_subdevices++;
-		for (i = 0; i < MAX_DO_SUBDEVS; i++)
-			if (this_board->sdo[i].chans)
-				n_subdevices++;
-		for (i = 0; i < MAX_DIO_SUBDEVG; i++)
-			n_subdevices += this_board->sdio[i].regs;
-		if (this_board->boardid.chans)
-			n_subdevices++;
-		for (i = 0; i < MAX_8254_SUBDEVS; i++)
-			if (this_board->s8254[i].chans)
-				n_subdevices++;
-	}
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
 
-	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, this_board->main_pci_region);
+
+	ret = comedi_alloc_subdevices(dev, this_board->nsubdevs);
 	if (ret)
 		return ret;
 
 	subdev = 0;
 	for (i = 0; i < MAX_DI_SUBDEVS; i++)
 		if (this_board->sdi[i].chans) {
-			s = dev->subdevices + subdev;
-			pci_dio_add_di(dev, s, &this_board->sdi[i], subdev);
+			s = &dev->subdevices[subdev];
+			pci_dio_add_di(dev, s, &this_board->sdi[i]);
 			subdev++;
 		}
 
 	for (i = 0; i < MAX_DO_SUBDEVS; i++)
 		if (this_board->sdo[i].chans) {
-			s = dev->subdevices + subdev;
-			pci_dio_add_do(dev, s, &this_board->sdo[i], subdev);
+			s = &dev->subdevices[subdev];
+			pci_dio_add_do(dev, s, &this_board->sdo[i]);
 			subdev++;
 		}
 
 	for (i = 0; i < MAX_DIO_SUBDEVG; i++)
 		for (j = 0; j < this_board->sdio[i].regs; j++) {
-			s = dev->subdevices + subdev;
+			s = &dev->subdevices[subdev];
 			subdev_8255_init(dev, s, NULL,
 					 dev->iobase +
 					 this_board->sdio[i].addr +
@@ -1153,21 +1148,21 @@ static int pci_dio_attach(struct comedi_device *dev,
 		}
 
 	if (this_board->boardid.chans) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DI;
-		pci_dio_add_di(dev, s, &this_board->boardid, subdev);
+		pci_dio_add_di(dev, s, &this_board->boardid);
 		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);
+			s = &dev->subdevices[subdev];
+			pci_dio_add_8254(dev, s, &this_board->s8254[i]);
 			subdev++;
 		}
 
 	if (this_board->cardtype == TYPE_PCI1760)
-		pci1760_attach(dev, it);
+		pci1760_attach(dev);
 
 	devpriv->valid = 1;
 
@@ -1178,52 +1173,34 @@ static int pci_dio_attach(struct comedi_device *dev,
 
 static void pci_dio_detach(struct comedi_device *dev)
 {
+	struct pci_dio_private *devpriv = dev->private;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	int i, j;
 	struct comedi_subdevice *s;
-	int subdev;
+	int i;
 
-	if (dev->private) {
+	if (devpriv) {
 		if (devpriv->valid)
 			pci_dio_reset(dev);
-		subdev = 0;
-		for (i = 0; i < MAX_DI_SUBDEVS; i++) {
-			if (this_board->sdi[i].chans)
-				subdev++;
-		}
-		for (i = 0; i < MAX_DO_SUBDEVS; i++) {
-			if (this_board->sdo[i].chans)
-				subdev++;
-		}
-		for (i = 0; i < MAX_DIO_SUBDEVG; i++) {
-			for (j = 0; j < this_board->sdio[i].regs; j++) {
-				s = dev->subdevices + subdev;
-				subdev_8255_cleanup(dev, s);
-				subdev++;
-			}
-		}
-		if (this_board->boardid.chans)
-			subdev++;
-		for (i = 0; i < MAX_8254_SUBDEVS; i++)
-			if (this_board->s8254[i].chans)
-				subdev++;
+	}
+	if (dev->subdevices) {
 		for (i = 0; i < dev->n_subdevices; i++) {
-			s = dev->subdevices + i;
+			s = &dev->subdevices[i];
+			if (s->type == COMEDI_SUBD_DIO)
+				subdev_8255_cleanup(dev, s);
 			s->private = NULL;
 		}
 	}
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver adv_pci_dio_driver = {
 	.driver_name	= "adv_pci_dio",
 	.module		= THIS_MODULE,
-	.attach		= pci_dio_attach,
-	.detach		= pci_dio_detach
+	.attach_pci	= pci_dio_attach_pci,
+	.detach		= pci_dio_detach,
 };
 
 static int __devinit adv_pci_dio_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index f7d453f8fe33..8acf60d0f20d 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -25,8 +25,9 @@
 Driver: aio_aio12_8
 Description: Access I/O Products PC-104 AIO12-8 Analog I/O Board
 Author: Pablo Mejia <pablo.mejia@cctechnol.com>
-Devices:
- [Access I/O] PC-104 AIO12-8
+Devices: [Access I/O] PC-104 AIO12-8 (aio_aio12_8)
+	 [Access I/O] PC-104 AI12-8 (aio_ai12_8)
+	 [Access I/O] PC-104 AO12-8 (aio_ao12_8)
 Status: experimental
 
 Configuration Options:
@@ -42,91 +43,118 @@ Notes:
 #include <linux/ioport.h>
 #include "8255.h"
 
-#define AIO12_8_STATUS			0x00
-#define AIO12_8_INTERRUPT		0x01
-#define AIO12_8_ADC			0x02
-#define AIO12_8_DAC_0			0x04
-#define AIO12_8_DAC_1			0x06
-#define AIO12_8_DAC_2			0x08
-#define AIO12_8_DAC_3			0x0A
-#define AIO12_8_COUNTER_0		0x0C
-#define AIO12_8_COUNTER_1		0x0D
-#define AIO12_8_COUNTER_2		0x0E
-#define AIO12_8_COUNTER_CONTROL		0x0F
-#define AIO12_8_DIO_0			0x10
-#define AIO12_8_DIO_1			0x11
-#define AIO12_8_DIO_2			0x12
-#define AIO12_8_DIO_STATUS		0x13
-#define AIO12_8_DIO_CONTROL		0x14
-#define AIO12_8_ADC_TRIGGER_CONTROL	0x15
-#define AIO12_8_TRIGGER			0x16
-#define AIO12_8_POWER			0x17
-
-#define STATUS_ADC_EOC			0x80
-
-#define ADC_MODE_NORMAL			0x00
-#define ADC_MODE_INTERNAL_CLOCK		0x40
-#define ADC_MODE_STANDBY		0x80
-#define ADC_MODE_POWERDOWN		0xC0
-
-#define DAC_ENABLE			0x18
+/*
+ * Register map
+ */
+#define AIO12_8_STATUS_REG		0x00
+#define AIO12_8_STATUS_ADC_EOC		(1 << 7)
+#define AIO12_8_STATUS_PORT_C_COS	(1 << 6)
+#define AIO12_8_STATUS_IRQ_ENA		(1 << 2)
+#define AIO12_8_INTERRUPT_REG		0x01
+#define AIO12_8_INTERRUPT_ADC		(1 << 7)
+#define AIO12_8_INTERRUPT_COS		(1 << 6)
+#define AIO12_8_INTERRUPT_COUNTER1	(1 << 5)
+#define AIO12_8_INTERRUPT_PORT_C3	(1 << 4)
+#define AIO12_8_INTERRUPT_PORT_C0	(1 << 3)
+#define AIO12_8_INTERRUPT_ENA		(1 << 2)
+#define AIO12_8_ADC_REG			0x02
+#define AIO12_8_ADC_MODE_NORMAL		(0 << 6)
+#define AIO12_8_ADC_MODE_INT_CLK	(1 << 6)
+#define AIO12_8_ADC_MODE_STANDBY	(2 << 6)
+#define AIO12_8_ADC_MODE_POWERDOWN	(3 << 6)
+#define AIO12_8_ADC_ACQ_3USEC		(0 << 5)
+#define AIO12_8_ADC_ACQ_PROGRAM		(1 << 5)
+#define AIO12_8_ADC_RANGE(x)		((x) << 3)
+#define AIO12_8_ADC_CHAN(x)		((x) << 0)
+#define AIO12_8_DAC_REG(x)		(0x04 + (x) * 2)
+#define AIO12_8_8254_BASE_REG		0x0c
+#define AIO12_8_8255_BASE_REG		0x10
+#define AIO12_8_DIO_CONTROL_REG		0x14
+#define AIO12_8_DIO_CONTROL_TST		(1 << 0)
+#define AIO12_8_ADC_TRIGGER_REG		0x15
+#define AIO12_8_ADC_TRIGGER_RANGE(x)	((x) << 3)
+#define AIO12_8_ADC_TRIGGER_CHAN(x)	((x) << 0)
+#define AIO12_8_TRIGGER_REG		0x16
+#define AIO12_8_TRIGGER_ADTRIG		(1 << 1)
+#define AIO12_8_TRIGGER_DACTRIG		(1 << 0)
+#define AIO12_8_COS_REG			0x17
+#define AIO12_8_DAC_ENABLE_REG		0x18
+#define AIO12_8_DAC_ENABLE_REF_ENA	(1 << 0)
 
 struct aio12_8_boardtype {
 	const char *name;
+	int ai_nchan;
+	int ao_nchan;
 };
 
 static const struct aio12_8_boardtype board_types[] = {
 	{
-	 .name = "aio_aio12_8"},
+		.name		= "aio_aio12_8",
+		.ai_nchan	= 8,
+		.ao_nchan	= 4,
+	}, {
+		.name		= "aio_ai12_8",
+		.ai_nchan	= 8,
+	}, {
+		.name		= "aio_ao12_8",
+		.ao_nchan	= 4,
+	},
 };
 
 struct aio12_8_private {
 	unsigned int ao_readback[4];
 };
 
-#define devpriv	((struct aio12_8_private *) dev->private)
-
 static int aio_aio12_8_ai_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int val;
+	unsigned char control;
 	int n;
-	unsigned char control =
-	    ADC_MODE_NORMAL |
-	    (CR_RANGE(insn->chanspec) << 3) | CR_CHAN(insn->chanspec);
 
-	/* read status to clear EOC latch */
-	inb(dev->iobase + AIO12_8_STATUS);
+	/*
+	 * Setup the control byte for internal 2MHz clock, 3uS conversion,
+	 * at the desired range of the requested channel.
+	 */
+	control = AIO12_8_ADC_MODE_NORMAL | AIO12_8_ADC_ACQ_3USEC |
+		  AIO12_8_ADC_RANGE(range) | AIO12_8_ADC_CHAN(chan);
+
+	/* Read status to clear EOC latch */
+	inb(dev->iobase + AIO12_8_STATUS_REG);
 
 	for (n = 0; n < insn->n; n++) {
 		int timeout = 5;
 
 		/*  Setup and start conversion */
-		outb(control, dev->iobase + AIO12_8_ADC);
+		outb(control, dev->iobase + AIO12_8_ADC_REG);
 
 		/*  Wait for conversion to complete */
-		while (timeout &&
-		       !(inb(dev->iobase + AIO12_8_STATUS) & STATUS_ADC_EOC)) {
+		do {
+			val = inb(dev->iobase + AIO12_8_STATUS_REG);
 			timeout--;
-			printk(KERN_ERR "timeout %d\n", timeout);
-			udelay(1);
-		}
-		if (timeout == 0) {
-			comedi_error(dev, "ADC timeout");
-			return -EIO;
-		}
-
-		data[n] = inw(dev->iobase + AIO12_8_ADC) & 0x0FFF;
+			if (timeout == 0) {
+				dev_err(dev->class_dev, "ADC timeout\n");
+				return -ETIMEDOUT;
+			}
+		} while (!(val & AIO12_8_STATUS_ADC_EOC));
+
+		data[n] = inw(dev->iobase + AIO12_8_ADC_REG) & s->maxdata;
 	}
-	return n;
+
+	return insn->n;
 }
 
 static int aio_aio12_8_ao_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct aio12_8_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int val = devpriv->ao_readback[chan];
 	int i;
-	int val = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = val;
@@ -137,18 +165,22 @@ static int aio_aio12_8_ao_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct aio12_8_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long port = dev->iobase + AIO12_8_DAC_REG(chan);
+	unsigned int val = 0;
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned long port = dev->iobase + AIO12_8_DAC_0 + (2 * chan);
 
 	/* enable DACs */
-	outb(0x01, dev->iobase + DAC_ENABLE);
+	outb(AIO12_8_DAC_ENABLE_REF_ENA, dev->iobase + AIO12_8_DAC_ENABLE_REG);
 
 	for (i = 0; i < insn->n; i++) {
-		outb(data[i] & 0xFF, port);	/*  LSB */
-		outb((data[i] >> 8) & 0x0F, port + 1);	/*  MSB */
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		outw(val, port);
 	}
+
+	devpriv->ao_readback[chan] = val;
+
 	return insn->n;
 }
 
@@ -166,53 +198,77 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
 	const struct aio12_8_boardtype *board = comedi_board(dev);
-	int iobase;
+	struct aio12_8_private *devpriv;
 	struct comedi_subdevice *s;
+	int iobase;
 	int ret;
 
+	dev->board_name = board->name;
+
 	iobase = it->options[0];
-	if (!request_region(iobase, 24, "aio_aio12_8")) {
+	if (!request_region(iobase, 32, dev->board_name)) {
 		printk(KERN_ERR "I/O port conflict");
 		return -EIO;
 	}
-
-	dev->board_name = board->name;
-
 	dev->iobase = iobase;
 
-	if (alloc_private(dev, sizeof(struct aio12_8_private)) < 0)
-		return -ENOMEM;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
-	ret = comedi_alloc_subdevices(dev, 3);
+	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan = 8;
-	s->maxdata = (1 << 12) - 1;
-	s->range_table = &range_aio_aio12_8;
-	s->insn_read = aio_aio12_8_ai_read;
+	if (board->ai_nchan) {
+		/* Analog input subdevice */
+		s->type		= COMEDI_SUBD_AI;
+		s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_DIFF;
+		s->n_chan	= board->ai_nchan;
+		s->maxdata	= 0x0fff;
+		s->range_table	= &range_aio_aio12_8;
+		s->insn_read	= aio_aio12_8_ai_read;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan = 4;
-	s->maxdata = (1 << 12) - 1;
-	s->range_table = &range_aio_aio12_8;
-	s->insn_read = aio_aio12_8_ao_read;
-	s->insn_write = aio_aio12_8_ao_write;
+	if (board->ao_nchan) {
+		/* Analog output subdevice */
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_DIFF;
+		s->n_chan	= 4;
+		s->maxdata	= 0x0fff;
+		s->range_table	= &range_aio_aio12_8;
+		s->insn_read	= aio_aio12_8_ao_read;
+		s->insn_write	= aio_aio12_8_ao_write;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
 
 	s = &dev->subdevices[2];
-	subdev_8255_init(dev, s, NULL, dev->iobase + AIO12_8_DIO_0);
+	/* 8255 Digital i/o subdevice */
+	iobase = dev->iobase + AIO12_8_8255_BASE_REG;
+	ret = subdev_8255_init(dev, s, NULL, iobase);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[3];
+	/* 8254 counter/timer subdevice */
+	s->type		= COMEDI_SUBD_UNUSED;
+
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
 
 	return 0;
 }
 
 static void aio_aio12_8_detach(struct comedi_device *dev)
 {
-	subdev_8255_cleanup(dev, &dev->subdevices[2]);
+	if (dev->subdevices)
+		subdev_8255_cleanup(dev, &dev->subdevices[2]);
 	if (dev->iobase)
 		release_region(dev->iobase, 24);
 }
diff --git a/drivers/staging/comedi/drivers/aio_iiro_16.c b/drivers/staging/comedi/drivers/aio_iiro_16.c
index ba1e3bbf2df8..b2cb8b02b2a1 100644
--- a/drivers/staging/comedi/drivers/aio_iiro_16.c
+++ b/drivers/staging/comedi/drivers/aio_iiro_16.c
@@ -112,7 +112,7 @@ static int aio_iiro_16_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 16;
@@ -120,7 +120,7 @@ static int aio_iiro_16_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = aio_iiro_16_dio_insn_bits_write;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 16;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index cc8931fde839..08f305210a69 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -210,11 +210,15 @@ order they appear in the channel list.
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "8255.h"
 #include "8253.h"
 
 #define DIO200_DRIVER_NAME	"amplc_dio200"
 
+#define DO_ISA	IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#define DO_PCI	IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
+
 /* PCI IDs */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI272 0x000a
@@ -272,12 +276,12 @@ enum dio200_model {
 };
 
 enum dio200_layout {
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#if DO_ISA
 	pc212_layout,
 	pc214_layout,
 #endif
 	pc215_layout,
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#if DO_ISA
 	pc218_layout,
 #endif
 	pc272_layout
@@ -292,7 +296,7 @@ struct dio200_board {
 };
 
 static const struct dio200_board dio200_boards[] = {
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#if DO_ISA
 	{
 	 .name = "pc212e",
 	 .bustype = isa_bustype,
@@ -324,7 +328,7 @@ static const struct dio200_board dio200_boards[] = {
 	 .layout = pc272_layout,
 	 },
 #endif
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
+#if DO_PCI
 	{
 	 .name = "pci215",
 	 .devid = PCI_DEVICE_ID_AMPLICON_PCI215,
@@ -367,7 +371,7 @@ struct dio200_layout_struct {
 };
 
 static const struct dio200_layout_struct dio200_layouts[] = {
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#if DO_ISA
 	[pc212_layout] = {
 			  .n_subdevs = 6,
 			  .sdtype = {sd_8255, sd_8254, sd_8254, sd_8254,
@@ -396,7 +400,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  .has_int_sce = 1,
 			  .has_clk_gat_sce = 1,
 			  },
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
+#if DO_ISA
 	[pc218_layout] = {
 			  .n_subdevs = 7,
 			  .sdtype = {sd_8254, sd_8254, sd_8255, sd_8254,
@@ -449,6 +453,16 @@ struct dio200_subdev_intr {
 	int continuous;
 };
 
+static inline bool is_pci_board(const struct dio200_board *board)
+{
+	return DO_PCI && board->bustype == pci_bustype;
+}
+
+static inline bool is_isa_board(const struct dio200_board *board)
+{
+	return DO_ISA && board->bustype == isa_bustype;
+}
+
 /*
  * This function looks for a board matching the supplied PCI device.
  */
@@ -458,7 +472,7 @@ dio200_find_pci_board(struct pci_dev *pci_dev)
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(dio200_boards); i++)
-		if (dio200_boards[i].bustype == pci_bustype &&
+		if (is_pci_board(&dio200_boards[i]) &&
 		    pci_dev->device == dio200_boards[i].devid)
 			return &dio200_boards[i];
 	return NULL;
@@ -758,52 +772,24 @@ dio200_subdev_intr_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	unsigned int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= (TRIG_NOW | TRIG_INT);
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= (TRIG_COUNT | TRIG_NONE);
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-		   compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* these tests are true if more than one _src bit is set */
-	if ((cmd->start_src & (cmd->start_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_begin_src & (cmd->scan_begin_src - 1)) != 0)
-		err++;
-	if ((cmd->convert_src & (cmd->convert_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_end_src & (cmd->scan_end_src - 1)) != 0)
-		err++;
-	if ((cmd->stop_src & (cmd->stop_src - 1)) != 0)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -965,15 +951,15 @@ static irqreturn_t dio200_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct dio200_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
 	int handled;
 
 	if (!dev->attached)
 		return IRQ_NONE;
 
 	if (devpriv->intr_sd >= 0) {
-		handled = dio200_handle_read_intr(dev,
-						  dev->subdevices +
-						  devpriv->intr_sd);
+		s = &dev->subdevices[devpriv->intr_sd];
+		handled = dio200_handle_read_intr(dev, s);
 	} else {
 		handled = 0;
 	}
@@ -1228,12 +1214,10 @@ static void dio200_report_attach(struct comedi_device *dev, unsigned int irq)
 	char tmpbuf[60];
 	int tmplen;
 
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA) &&
-	    thisboard->bustype == isa_bustype)
+	if (is_isa_board(thisboard))
 		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
 				   "(base %#lx) ", dev->iobase);
-	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI) &&
-		 thisboard->bustype == pci_bustype)
+	else if (is_pci_board(thisboard))
 		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
 				   "(pci %s) ", pci_name(pcidev));
 	else
@@ -1361,8 +1345,7 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* Process options and reserve resources according to bus type. */
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA) &&
-	    thisboard->bustype == isa_bustype) {
+	if (is_isa_board(thisboard)) {
 		unsigned long iobase;
 		unsigned int irq;
 
@@ -1372,8 +1355,7 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		if (ret < 0)
 			return ret;
 		return dio200_common_attach(dev, iobase, irq, 0);
-	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI) &&
-		   thisboard->bustype == pci_bustype) {
+	} else if (is_pci_board(thisboard)) {
 		struct pci_dev *pci_dev;
 
 		pci_dev = dio200_find_pci_dev(dev, it);
@@ -1397,7 +1379,7 @@ static int __devinit dio200_attach_pci(struct comedi_device *dev,
 {
 	int ret;
 
-	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI))
+	if (!DO_PCI)
 		return -EINVAL;
 
 	dev_info(dev->class_dev, DIO200_DRIVER_NAME ": attach pci %s\n",
@@ -1425,7 +1407,6 @@ static int __devinit dio200_attach_pci(struct comedi_device *dev,
 static void dio200_detach(struct comedi_device *dev)
 {
 	const struct dio200_board *thisboard = comedi_board(dev);
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct dio200_layout_struct *layout;
 	unsigned n;
 
@@ -1450,13 +1431,16 @@ static void dio200_detach(struct comedi_device *dev)
 			}
 		}
 	}
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	} else {
+	if (is_isa_board(thisboard)) {
 		if (dev->iobase)
 			release_region(dev->iobase, DIO200_IO_SIZE);
+	} else if (is_pci_board(thisboard)) {
+		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+		if (pcidev) {
+			if (dev->iobase)
+				comedi_pci_disable(pcidev);
+			pci_dev_put(pcidev);
+		}
 	}
 }
 
@@ -1477,7 +1461,7 @@ static struct comedi_driver amplc_dio200_driver = {
 	.num_names = ARRAY_SIZE(dio200_boards),
 };
 
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
+#if DO_PCI
 static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index f50287903038..eacb5e4735d7 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -56,11 +56,15 @@ unused.
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "8255.h"
 #include "plx9052.h"
 
 #define PC236_DRIVER_NAME	"amplc_pc236"
 
+#define DO_ISA	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA)
+#define DO_PCI	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI)
+
 /* PCI236 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI236 0x0009
@@ -103,14 +107,14 @@ struct pc236_board {
 	enum pc236_model model;
 };
 static const struct pc236_board pc236_boards[] = {
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA)
+#if DO_ISA
 	{
 		.name = "pc36at",
 		.bustype = isa_bustype,
 		.model = pc36at_model,
 	},
 #endif
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI)
+#if DO_PCI
 	{
 		.name = "pci236",
 		.devid = PCI_DEVICE_ID_AMPLICON_PCI236,
@@ -135,6 +139,18 @@ struct pc236_private {
 	int enable_irq;
 };
 
+/* test if ISA supported and this is an ISA board */
+static inline bool is_isa_board(const struct pc236_board *board)
+{
+	return DO_ISA && board->bustype == isa_bustype;
+}
+
+/* test if PCI supported and this is a PCI board */
+static inline bool is_pci_board(const struct pc236_board *board)
+{
+	return DO_PCI && board->bustype == pci_bustype;
+}
+
 /*
  * This function looks for a board matching the supplied PCI device.
  */
@@ -143,7 +159,7 @@ static const struct pc236_board *pc236_find_pci_board(struct pci_dev *pci_dev)
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(pc236_boards); i++)
-		if (pc236_boards[i].bustype == pci_bustype &&
+		if (is_pci_board(&pc236_boards[i]) &&
 		    pci_dev->device == pc236_boards[i].devid)
 			return &pc236_boards[i];
 	return NULL;
@@ -214,12 +230,13 @@ static int pc236_request_region(struct comedi_device *dev, unsigned long from,
  */
 static void pc236_intr_disable(struct comedi_device *dev)
 {
+	const struct pc236_board *thisboard = comedi_board(dev);
 	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 0;
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) && devpriv->lcr_iobase)
+	if (is_pci_board(thisboard))
 		outl(PCI236_INTR_DISABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
@@ -231,12 +248,13 @@ static void pc236_intr_disable(struct comedi_device *dev)
  */
 static void pc236_intr_enable(struct comedi_device *dev)
 {
+	const struct pc236_board *thisboard = comedi_board(dev);
 	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 1;
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) && devpriv->lcr_iobase)
+	if (is_pci_board(thisboard))
 		outl(PCI236_INTR_ENABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
@@ -250,6 +268,7 @@ static void pc236_intr_enable(struct comedi_device *dev)
  */
 static int pc236_intr_check(struct comedi_device *dev)
 {
+	const struct pc236_board *thisboard = comedi_board(dev);
 	struct pc236_private *devpriv = dev->private;
 	int retval = 0;
 	unsigned long flags;
@@ -257,8 +276,7 @@ static int pc236_intr_check(struct comedi_device *dev)
 	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->enable_irq) {
 		retval = 1;
-		if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
-		    devpriv->lcr_iobase) {
+		if (is_pci_board(thisboard)) {
 			if ((inl(devpriv->lcr_iobase + PLX9052_INTCSR)
 			     & PLX9052_INTCSR_LI1STAT_MASK)
 			    == PLX9052_INTCSR_LI1STAT_INACTIVE) {
@@ -296,39 +314,20 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 			      struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
 
-	/* step 1 */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: ignored */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -395,7 +394,7 @@ static int pc236_intr_cancel(struct comedi_device *dev,
 static irqreturn_t pc236_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 1;
+	struct comedi_subdevice *s = &dev->subdevices[1];
 	int handled;
 
 	handled = pc236_intr_check(dev);
@@ -414,15 +413,13 @@ static void pc236_report_attach(struct comedi_device *dev, unsigned int irq)
 	char tmpbuf[60];
 	int tmplen;
 
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA) &&
-	    thisboard->bustype == isa_bustype)
+	if (is_isa_board(thisboard))
 		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
 				   "(base %#lx) ", dev->iobase);
-	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
-		 thisboard->bustype == pci_bustype) {
+	else if (is_pci_board(thisboard))
 		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
 				   "(pci %s) ", pci_name(pcidev));
-	} else
+	else
 		tmplen = 0;
 	if (irq)
 		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
@@ -449,14 +446,14 @@ static int pc236_common_attach(struct comedi_device *dev, unsigned long iobase,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* digital i/o subdevice (8255) */
 	ret = subdev_8255_init(dev, s, NULL, iobase);
 	if (ret < 0) {
 		dev_err(dev->class_dev, "error! out of memory!\n");
 		return ret;
 	}
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_UNUSED;
 	pc236_intr_disable(dev);
@@ -517,16 +514,14 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 	}
 	/* Process options according to bus type. */
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA) &&
-	    thisboard->bustype == isa_bustype) {
+	if (is_isa_board(thisboard)) {
 		unsigned long iobase = it->options[0];
 		unsigned int irq = it->options[1];
 		ret = pc236_request_region(dev, iobase, PC236_IO_SIZE);
 		if (ret < 0)
 			return ret;
 		return pc236_common_attach(dev, iobase, irq, 0);
-	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
-		   thisboard->bustype == pci_bustype) {
+	} else if (is_pci_board(thisboard)) {
 		struct pci_dev *pci_dev;
 
 		pci_dev = pc236_find_pci_dev(dev, it);
@@ -550,7 +545,7 @@ static int __devinit pc236_attach_pci(struct comedi_device *dev,
 {
 	int ret;
 
-	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI))
+	if (!DO_PCI)
 		return -EINVAL;
 
 	dev_info(dev->class_dev, PC236_DRIVER_NAME ": attach pci %s\n",
@@ -577,22 +572,25 @@ static int __devinit pc236_attach_pci(struct comedi_device *dev,
 
 static void pc236_detach(struct comedi_device *dev)
 {
+	const struct pc236_board *thisboard = comedi_board(dev);
 	struct pc236_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
 	if (devpriv)
 		pc236_intr_disable(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 0);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	} else {
+		subdev_8255_cleanup(dev, &dev->subdevices[0]);
+	if (is_isa_board(thisboard)) {
 		if (dev->iobase)
 			release_region(dev->iobase, PC236_IO_SIZE);
+	} else if (is_pci_board(thisboard)) {
+		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+		if (pcidev) {
+			if (dev->iobase)
+				comedi_pci_disable(pcidev);
+			pci_dev_put(pcidev);
+		}
 	}
 }
 
@@ -613,7 +611,7 @@ static struct comedi_driver amplc_pc236_driver = {
 	.num_names = ARRAY_SIZE(pc236_boards),
 };
 
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI)
+#if DO_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236) },
 	{0}
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 8191c4e28e0a..60830ccfb903 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -48,6 +48,9 @@ The state of the outputs can be read.
 
 #define PC263_DRIVER_NAME	"amplc_pc263"
 
+#define DO_ISA	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA)
+#define DO_PCI	IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
+
 /* PCI263 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI263 0x000c
@@ -70,14 +73,14 @@ struct pc263_board {
 	enum pc263_model model;
 };
 static const struct pc263_board pc263_boards[] = {
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA)
+#if DO_ISA
 	{
 		.name = "pc263",
 		.bustype = isa_bustype,
 		.model = pc263_model,
 	},
 #endif
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
+#if DO_PCI
 	{
 		.name = "pci263",
 		.devid = PCI_DEVICE_ID_AMPLICON_PCI263,
@@ -93,6 +96,18 @@ static const struct pc263_board pc263_boards[] = {
 #endif
 };
 
+/* test if ISA supported and this is an ISA board */
+static inline bool is_isa_board(const struct pc263_board *board)
+{
+	return DO_ISA && board->bustype == isa_bustype;
+}
+
+/* test if PCI supported and this is a PCI board */
+static inline bool is_pci_board(const struct pc263_board *board)
+{
+	return DO_PCI && board->bustype == pci_bustype;
+}
+
 /*
  * This function looks for a board matching the supplied PCI device.
  */
@@ -101,7 +116,7 @@ static const struct pc263_board *pc263_find_pci_board(struct pci_dev *pci_dev)
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(pc263_boards); i++)
-		if (pc263_boards[i].bustype == pci_bustype &&
+		if (is_pci_board(&pc263_boards[i]) &&
 		    pci_dev->device == pc263_boards[i].devid)
 			return &pc263_boards[i];
 	return NULL;
@@ -187,11 +202,9 @@ static void pc263_report_attach(struct comedi_device *dev)
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	char tmpbuf[40];
 
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA) &&
-	    thisboard->bustype == isa_bustype)
+	if (is_isa_board(thisboard))
 		snprintf(tmpbuf, sizeof(tmpbuf), "(base %#lx) ", dev->iobase);
-	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI) &&
-		 thisboard->bustype == pci_bustype)
+	else if (is_pci_board(thisboard))
 		snprintf(tmpbuf, sizeof(tmpbuf), "(pci %s) ",
 			 pci_name(pcidev));
 	else
@@ -212,7 +225,7 @@ static int pc263_common_attach(struct comedi_device *dev, unsigned long iobase)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* digital output subdevice */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -259,15 +272,13 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach\n");
 
 	/* Process options and reserve resources according to bus type. */
-	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA) &&
-	    thisboard->bustype == isa_bustype) {
+	if (is_isa_board(thisboard)) {
 		unsigned long iobase = it->options[0];
 		ret = pc263_request_region(dev, iobase, PC263_IO_SIZE);
 		if (ret < 0)
 			return ret;
 		return pc263_common_attach(dev, iobase);
-	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI) &&
-		   thisboard->bustype == pci_bustype) {
+	} else if (is_pci_board(thisboard)) {
 		struct pci_dev *pci_dev;
 
 		pci_dev = pc263_find_pci_dev(dev, it);
@@ -288,7 +299,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 static int __devinit pc263_attach_pci(struct comedi_device *dev,
 				      struct pci_dev *pci_dev)
 {
-	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI))
+	if (!DO_PCI)
 		return -EINVAL;
 
 	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach pci %s\n",
@@ -310,15 +321,18 @@ static int __devinit pc263_attach_pci(struct comedi_device *dev,
 
 static void pc263_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct pc263_board *thisboard = comedi_board(dev);
 
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	} else {
+	if (is_isa_board(thisboard)) {
 		if (dev->iobase)
 			release_region(dev->iobase, PC263_IO_SIZE);
+	} else if (is_pci_board(thisboard)) {
+		struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+		if (pcidev) {
+			if (dev->iobase)
+				comedi_pci_disable(pcidev);
+			pci_dev_put(pcidev);
+		}
 	}
 }
 
@@ -339,7 +353,7 @@ static struct comedi_driver amplc_pc263_driver = {
 	.num_names = ARRAY_SIZE(pc263_boards),
 };
 
-#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
+#if DO_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
 	{0}
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 8bf109e7bb05..1f65ec4d261e 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -720,53 +720,31 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	int err = 0;
 	unsigned int tmp;
 
-	/* Step 1: make sure trigger sources are trivially valid. */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT | TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_EXT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* Step 2: make sure trigger sources are unique and mutually
-	 * compatible. */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* these tests are true if more than one _src bit is set */
-	if ((cmd->start_src & (cmd->start_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_begin_src & (cmd->scan_begin_src - 1)) != 0)
-		err++;
-	if ((cmd->convert_src & (cmd->convert_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_end_src & (cmd->scan_end_src - 1)) != 0)
-		err++;
-	if ((cmd->stop_src & (cmd->stop_src - 1)) != 0)
-		err++;
+	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);
 
-	/* There's only one external trigger signal (which makes these
-	 * tests easier).  Only one thing can use it. */
+	/* Step 2b : and mutually compatible */
+
+	/*
+	 * There's only one external trigger signal (which makes these
+	 * tests easier).  Only one thing can use it.
+	 */
 	tmp = 0;
 	if (cmd->start_src & TRIG_EXT)
 		tmp++;
@@ -775,7 +753,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (cmd->stop_src & TRIG_EXT)
 		tmp++;
 	if (tmp > 1)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -1375,7 +1353,7 @@ static int pci224_attach_common(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* Analog output subdevice. */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
@@ -1523,7 +1501,7 @@ static void pci224_detach(struct comedi_device *dev)
 	if (dev->subdevices) {
 		struct comedi_subdevice *s;
 
-		s = dev->subdevices + 0;
+		s = &dev->subdevices[0];
 		/* AO subdevice */
 		kfree(s->range_table_list);
 	}
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 66e74bd12267..bd8fb876ce2e 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -193,6 +193,7 @@ for (or detection of) various hardware problems added by Ian Abbott.
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 #include "8255.h"
 
@@ -958,23 +959,11 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	unsigned int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
+	/* Step 1 : check if triggers are trivially valid */
 
-	/* Step 1: make sure trigger sources are trivially valid.
-	 * "invalid source" returned by comedilib to user mode process
-	 * if this fails. */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
 
-	tmp = cmd->scan_begin_src;
+	tmp = TRIG_TIMER | TRIG_INT;
 	if ((thisboard->min_hwver > 0) && (devpriv->hwver >= 2)) {
 		/*
 		 * For PCI230+ hardware version 2 onwards, allow external
@@ -990,46 +979,23 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 		 * scan_begin_src==TRIG_EXT support to be a bonus rather than a
 		 * guarantee!
 		 */
-		cmd->scan_begin_src &= TRIG_TIMER | TRIG_INT | TRIG_EXT;
-	} else {
-		cmd->scan_begin_src &= TRIG_TIMER | TRIG_INT;
+		tmp |= TRIG_EXT;
 	}
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, tmp);
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* Step 2: make sure trigger sources are unique and mutually compatible
-	 * "source conflict" returned by comedilib to user mode process
-	 * if this fails. */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* these tests are true if more than one _src bit is set */
-	if ((cmd->start_src & (cmd->start_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_begin_src & (cmd->scan_begin_src - 1)) != 0)
-		err++;
-	if ((cmd->convert_src & (cmd->convert_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_end_src & (cmd->scan_end_src - 1)) != 0)
-		err++;
-	if ((cmd->stop_src & (cmd->stop_src - 1)) != 0)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1610,75 +1576,45 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	unsigned int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4,5 or 0, depending on which tests
-	 * the command passes. */
+	/* Step 1 : check if triggers are trivially valid */
 
-	/* Step 1: make sure trigger sources are trivially valid.
-	 * "invalid source" returned by comedilib to user mode process
-	 * if this fails. */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
-	tmp = cmd->scan_begin_src;
-	/* Unfortunately, we cannot trigger a scan off an external source
-	 * on the PCI260 board, since it uses the PPIC0 (DIO) input, which
-	 * isn't present on the PCI260.  For PCI260+ we can use the
-	 * EXTTRIG/EXTCONVCLK input on pin 17 instead. */
+	tmp = TRIG_FOLLOW | TRIG_TIMER | TRIG_INT;
 	if ((thisboard->have_dio) || (thisboard->min_hwver > 0)) {
-		cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_INT
-		    | TRIG_EXT;
-	} else {
-		cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_INT;
+		/*
+		 * Unfortunately, we cannot trigger a scan off an external
+		 * source on the PCI260 board, since it uses the PPIC0 (DIO)
+		 * input, which isn't present on the PCI260.  For PCI260+
+		 * we can use the EXTTRIG/EXTCONVCLK input on pin 17 instead.
+		 */
+		tmp |= TRIG_EXT;
 	}
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_INT | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, tmp);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/* Step 2: make sure trigger sources are unique and mutually compatible
-	 * "source conflict" returned by comedilib to user mode process
-	 * if this fails. */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* these tests are true if more than one _src bit is set */
-	if ((cmd->start_src & (cmd->start_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_begin_src & (cmd->scan_begin_src - 1)) != 0)
-		err++;
-	if ((cmd->convert_src & (cmd->convert_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_end_src & (cmd->scan_end_src - 1)) != 0)
-		err++;
-	if ((cmd->stop_src & (cmd->stop_src - 1)) != 0)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
-	/* If scan_begin_src is not TRIG_FOLLOW, then a monostable will be
-	 * set up to generate a fixed number of timed conversion pulses. */
+	/*
+	 * If scan_begin_src is not TRIG_FOLLOW, then a monostable will be
+	 * set up to generate a fixed number of timed conversion pulses.
+	 */
 	if ((cmd->scan_begin_src != TRIG_FOLLOW)
 	    && (cmd->convert_src != TRIG_TIMER))
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -2838,7 +2774,7 @@ static int pci230_attach_common(struct comedi_device *dev,
 	if (rc)
 		return rc;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
@@ -2855,7 +2791,7 @@ static int pci230_attach_common(struct comedi_device *dev,
 		s->do_cmdtest = &pci230_ai_cmdtest;
 		s->cancel = pci230_ai_cancel;
 	}
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
 	if (thisboard->ao_chans > 0) {
 		s->type = COMEDI_SUBD_AO;
@@ -2878,7 +2814,7 @@ static int pci230_attach_common(struct comedi_device *dev,
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 	if (thisboard->have_dio) {
 		rc = subdev_8255_init(dev, s, NULL,
@@ -2941,7 +2877,7 @@ static void pci230_detach(struct comedi_device *dev)
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
 	if (dev->subdevices && thisboard->have_dio)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
+		subdev_8255_cleanup(dev, &dev->subdevices[2]);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (pcidev) {
diff --git a/drivers/staging/comedi/drivers/c6xdigio.c b/drivers/staging/comedi/drivers/c6xdigio.c
index 41ed8576f301..070037c22db7 100644
--- a/drivers/staging/comedi/drivers/c6xdigio.c
+++ b/drivers/staging/comedi/drivers/c6xdigio.c
@@ -447,7 +447,7 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	else if (irq == 0)
 		printk(KERN_DEBUG "comedi%d: no irq\n", dev->minor);
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* pwm output subdevice */
 	s->type = COMEDI_SUBD_AO;	/*  Not sure what to put here */
 	s->subdev_flags = SDF_WRITEABLE;
@@ -458,7 +458,7 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	s->maxdata = 500;
 	s->range_table = &range_bipolar10;	/*  A suitable lie */
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* encoder (counter) subdevice */
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
@@ -468,7 +468,7 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	s->maxdata = 0xffffff;
 	s->range_table = &range_unknown;
 
-	/*	s = dev->subdevices + 2; */
+	/*	s = &dev->subdevices[2]; */
 	/* pwm output subdevice */
 	/*	s->type = COMEDI_SUBD_COUNTER;  // Not sure what to put here */
 	/*	s->subdev_flags = SDF_WRITEABLE; */
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 58d45299bf85..6d81d8b40ccc 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -46,6 +46,7 @@ Status: experimental
 #include <pcmcia/cistpl.h>
 #include <pcmcia/ds.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 
 #define DAS16CS_SIZE			18
@@ -169,47 +170,26 @@ static int das16cs_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and
-	 * mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -478,7 +458,7 @@ static int das16cs_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* analog input subdevice */
 	s->type		= COMEDI_SUBD_AI;
@@ -491,7 +471,7 @@ static int das16cs_attach(struct comedi_device *dev,
 	s->do_cmd	= das16cs_ai_cmd;
 	s->do_cmdtest	= das16cs_ai_cmdtest;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
 	if (thisboard->n_ao_chans) {
 		s->type		= COMEDI_SUBD_AO;
@@ -505,7 +485,7 @@ static int das16cs_attach(struct comedi_device *dev,
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 	s->type		= COMEDI_SUBD_DIO;
 	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 2b6a637c3499..de21a261ff45 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -45,11 +45,7 @@ Status:
   The boards may be autocalibrated using the comedi_calibrate
   utility.
 
-Configuration options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+Configuration options: not applicable, uses PCI auto config
 
 For commands, the scanned channels must be consecutive
 (i.e. 4-5-6-7, 2-3-4,...), and must all have the same
@@ -807,58 +803,35 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	int tmp;
 	int i, gain, start_chan;
 
-	/* step 1: trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_NOW | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/* step 2: trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
-	/*  make sure trigger sources are compatible with each other */
 	if (cmd->scan_begin_src == TRIG_FOLLOW && cmd->convert_src == TRIG_NOW)
-		err++;
+		err |= -EINVAL;
 	if (cmd->scan_begin_src != TRIG_FOLLOW && cmd->convert_src != TRIG_NOW)
-		err++;
+		err |= -EINVAL;
 	if (cmd->start_src == TRIG_EXT &&
 	    (cmd->convert_src == TRIG_EXT || cmd->scan_begin_src == TRIG_EXT))
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -1083,43 +1056,24 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
+	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_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1501,69 +1455,45 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static struct pci_dev *cb_pcidas_find_pci_device(struct comedi_device *dev,
-						 struct comedi_devconfig *it)
+static const void *cb_pcidas_find_boardinfo(struct comedi_device *dev,
+					    struct pci_dev *pcidev)
 {
 	const struct cb_pcidas_board *thisboard;
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
 	int i;
 
-	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
-		if (pcidev->vendor != PCI_VENDOR_ID_CB)
-			continue;
-		/*  loop through cards supported by this driver */
-		for (i = 0; i < ARRAY_SIZE(cb_pcidas_boards); i++) {
-			thisboard = &cb_pcidas_boards[i];
-			if (thisboard->device_id != pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (bus || slot) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != bus ||
-				    PCI_SLOT(pcidev->devfn) != slot) {
-					continue;
-				}
-			}
-			dev_dbg(dev->class_dev,
-				"Found %s on bus %i, slot %i\n",
-				thisboard->name,
-				pcidev->bus->number, PCI_SLOT(pcidev->devfn));
-			dev->board_ptr = thisboard;
-			return pcidev;
-		}
+	for (i = 0; i < ARRAY_SIZE(cb_pcidas_boards); i++) {
+		thisboard = &cb_pcidas_boards[i];
+		if (thisboard->device_id == pcidev->device)
+			return thisboard;
 	}
-	dev_err(dev->class_dev, "No supported card found\n");
 	return NULL;
 }
 
-static int cb_pcidas_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it)
+static int cb_pcidas_attach_pci(struct comedi_device *dev,
+				struct pci_dev *pcidev)
 {
 	const struct cb_pcidas_board *thisboard;
 	struct cb_pcidas_private *devpriv;
-	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int i;
 	int ret;
 
-	if (alloc_private(dev, sizeof(struct cb_pcidas_private)) < 0)
-		return -ENOMEM;
-	devpriv = dev->private;
-
-	pcidev = cb_pcidas_find_pci_device(dev, it);
-	if (!pcidev)
-		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
-	thisboard = comedi_board(dev);
 
-	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
-		dev_err(dev->class_dev,
-			"Failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
+	thisboard = cb_pcidas_find_boardinfo(dev, pcidev);
+	if (!thisboard)
+		return -ENODEV;
+	dev->board_ptr  = thisboard;
+	dev->board_name = thisboard->name;
+
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
 
 	devpriv->s5933_config = pci_resource_start(pcidev, 0);
 	devpriv->control_status = pci_resource_start(pcidev, 1);
@@ -1584,13 +1514,11 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	}
 	dev->irq = pcidev->irq;
 
-	dev->board_name = thisboard->name;
-
 	ret = comedi_alloc_subdevices(dev, 7);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
@@ -1607,7 +1535,7 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	s->cancel = cb_pcidas_cancel;
 
 	/* analog output subdevice */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (thisboard->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
@@ -1634,14 +1562,14 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	}
 
 	/* 8255 */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	ret = subdev_8255_init(dev, s, NULL,
 			       devpriv->pacer_counter_dio + DIO_8255);
 	if (ret)
 		return ret;
 
 	/*  serial EEPROM, */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_MEMORY;
 	s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
 	s->n_chan = 256;
@@ -1649,7 +1577,7 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	s->insn_read = eeprom_read_insn;
 
 	/*  8800 caldac */
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	s->type = COMEDI_SUBD_CALIB;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan = NUM_CHANNELS_8800;
@@ -1660,7 +1588,7 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 		caldac_8800_write(dev, i, s->maxdata / 2);
 
 	/*  trim potentiometer */
-	s = dev->subdevices + 5;
+	s = &dev->subdevices[5];
 	s->type = COMEDI_SUBD_CALIB;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	if (thisboard->trimpot == AD7376) {
@@ -1676,7 +1604,7 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 		cb_pcidas_trimpot_write(dev, i, s->maxdata / 2);
 
 	/*  dac08 caldac */
-	s = dev->subdevices + 6;
+	s = &dev->subdevices[6];
 	if (thisboard->has_dac08) {
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
@@ -1698,7 +1626,10 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	outl(devpriv->s5933_intcsr_bits | INTCSR_INBOX_INTR_STATUS,
 	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
-	return 1;
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
+
+	return 0;
 }
 
 static void cb_pcidas_detach(struct comedi_device *dev)
@@ -1715,18 +1646,17 @@ static void cb_pcidas_detach(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
+		subdev_8255_cleanup(dev, &dev->subdevices[2]);
 	if (pcidev) {
 		if (devpriv->s5933_config)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver cb_pcidas_driver = {
 	.driver_name	= "cb_pcidas",
 	.module		= THIS_MODULE,
-	.attach		= cb_pcidas_attach,
+	.attach_pci	= cb_pcidas_attach_pci,
 	.detach		= cb_pcidas_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 65cbaabf6456..0472a9088abe 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1348,7 +1348,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
@@ -1379,7 +1379,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	}
 
 	/* analog output subdevice */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (board(dev)->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags =
@@ -1401,7 +1401,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	}
 
 	/*  digital input */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	if (board(dev)->layout == LAYOUT_64XX) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
@@ -1414,7 +1414,7 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/*  digital output */
 	if (board(dev)->layout == LAYOUT_64XX) {
-		s = dev->subdevices + 3;
+		s = &dev->subdevices[3];
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 		s->n_chan = 4;
@@ -1425,7 +1425,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/* 8255 */
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	if (board(dev)->has_8255) {
 		if (board(dev)->layout == LAYOUT_4020) {
 			dio_8255_iobase =
@@ -1442,7 +1442,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/*  8 channel dio for 60xx */
-	s = dev->subdevices + 5;
+	s = &dev->subdevices[5];
 	if (board(dev)->layout == LAYOUT_60XX) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
@@ -1455,7 +1455,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/*  caldac */
-	s = dev->subdevices + 6;
+	s = &dev->subdevices[6];
 	s->type = COMEDI_SUBD_CALIB;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan = 8;
@@ -1469,7 +1469,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		caldac_write(dev, i, s->maxdata / 2);
 
 	/*  2 channel ad8402 potentiometer */
-	s = dev->subdevices + 7;
+	s = &dev->subdevices[7];
 	if (board(dev)->layout == LAYOUT_64XX) {
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
@@ -1483,7 +1483,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/* serial EEPROM, if present */
-	s = dev->subdevices + 8;
+	s = &dev->subdevices[8];
 	if (readl(priv(dev)->plx9080_iobase + PLX_CONTROL_REG) & CTL_EECHK) {
 		s->type = COMEDI_SUBD_MEMORY;
 		s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
@@ -1494,7 +1494,7 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/*  user counter subd XXX */
-	s = dev->subdevices + 9;
+	s = &dev->subdevices[9];
 	s->type = COMEDI_SUBD_UNUSED;
 
 	return 0;
@@ -1847,7 +1847,7 @@ static void detach(struct comedi_device *dev)
 		}
 	}
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 4);
+		subdev_8255_cleanup(dev, &dev->subdevices[4]);
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
@@ -2108,74 +2108,50 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
 	unsigned int tmp_arg, tmp_arg2;
 	int i;
 	int aref;
 	unsigned int triggers;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
 
-	tmp = cmd->scan_begin_src;
 	triggers = TRIG_TIMER;
 	if (board(dev)->layout == LAYOUT_4020)
 		triggers |= TRIG_OTHER;
 	else
 		triggers |= TRIG_FOLLOW;
-	cmd->scan_begin_src &= triggers;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, triggers);
 
-	tmp = cmd->convert_src;
 	triggers = TRIG_TIMER;
 	if (board(dev)->layout == LAYOUT_4020)
 		triggers |= TRIG_NOW;
 	else
 		triggers |= TRIG_EXT;
-	cmd->convert_src &= triggers;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->convert_src, triggers);
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_EXT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/*  uniqueness check */
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_OTHER &&
-	    cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
-	/*  compatibility check */
 	if (cmd->convert_src == TRIG_EXT && cmd->scan_begin_src == TRIG_TIMER)
-		err++;
+		err |= -EINVAL;
 	if (cmd->stop_src != TRIG_COUNT &&
 	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -3466,55 +3442,33 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
 	unsigned int tmp_arg;
 	int i;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/*  uniqueness check */
-	if (cmd->start_src != TRIG_INT && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+
+	/* Step 2b : and mutually compatible */
 
-	/*  compatibility check */
 	if (cmd->convert_src == TRIG_EXT && cmd->scan_begin_src == TRIG_TIMER)
-		err++;
+		err |= -EINVAL;
 	if (cmd->stop_src != TRIG_COUNT &&
 	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 12660a384e59..aef946df27e2 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -48,6 +48,7 @@ Please report success/failure with other different cards to
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "8255.h"
 
 /* PCI vendor number of ComputerBoards */
@@ -56,14 +57,6 @@ Please report success/failure with other different cards to
 /* maximum number of ao channels for supported boards */
 #define MAX_AO_CHANNELS 8
 
-/* PCI-DDA base addresses */
-#define DIGITALIO_BADRINDEX	2
-	/*  DIGITAL I/O is pci_dev->resource[2] */
-#define DIGITALIO_SIZE 8
-	/*  DIGITAL I/O uses 8 I/O port addresses */
-#define DAC_BADRINDEX	3
-	/*  DAC is pci_dev->resource[3] */
-
 /* Digital I/O registers */
 #define PORT1A 0		/*  PORT 1A DATA */
 
@@ -203,11 +196,6 @@ static const struct cb_pcidda_board cb_pcidda_boards[] = {
 };
 
 /*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct cb_pcidda_board *)dev->board_ptr)
-
-/*
  * this structure is for data unique to this hardware driver.  If
  * several hardware drivers keep similar information in this structure,
  * feel free to suggest moving the variable to the struct comedi_device
@@ -230,164 +218,6 @@ struct cb_pcidda_private {
 };
 
 /*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct cb_pcidda_private *)dev->private)
-
-/* static int cb_pcidda_ai_rinsn(struct comedi_device *dev,struct comedi_subdevice *s,struct comedi_insn *insn,unsigned int *data); */
-static int cb_pcidda_ao_winsn(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-
-/* static int cb_pcidda_ai_cmd(struct comedi_device *dev, struct *comedi_subdevice *s);*/
-/* static int cb_pcidda_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s, struct comedi_cmd *cmd); */
-/* static int cb_pcidda_ns_to_timer(unsigned int *ns,int *round); */
-
-static unsigned int cb_pcidda_serial_in(struct comedi_device *dev);
-static void cb_pcidda_serial_out(struct comedi_device *dev, unsigned int value,
-				 unsigned int num_bits);
-static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
-					  unsigned int address);
-static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
-				unsigned int range);
-
-static struct pci_dev *cb_pcidda_find_pci_dev(struct comedi_device *dev,
-					      struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_CB)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(cb_pcidda_boards); i++) {
-			if (cb_pcidda_boards[i].device_id != pcidev->device)
-				continue;
-			dev->board_ptr = cb_pcidda_boards + i;
-			return pcidev;
-		}
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.
- */
-static int cb_pcidda_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *s;
-	int index;
-	int ret;
-
-/*
- * Allocate the private structure area.
- */
-	if (alloc_private(dev, sizeof(struct cb_pcidda_private)) < 0)
-		return -ENOMEM;
-
-	pcidev = cb_pcidda_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	/*
-	 * Enable PCI device and request regions.
-	 */
-	if (comedi_pci_enable(pcidev, thisboard->name)) {
-		dev_err(dev->class_dev,
-			"cb_pcidda: failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
-
-/*
- * Allocate the I/O ports.
- */
-	devpriv->digitalio = pci_resource_start(pcidev, DIGITALIO_BADRINDEX);
-	devpriv->dac = pci_resource_start(pcidev, DAC_BADRINDEX);
-	dev->iobase = devpriv->dac;
-
-/*
- * Warn about the status of the driver.
- */
-	if (thisboard->status == 2)
-		printk
-		    ("WARNING: DRIVER FOR THIS BOARD NOT CHECKED WITH MANUAL. "
-		     "WORKS ASSUMING FULL COMPATIBILITY WITH PCI-DDA08/12. "
-		     "PLEASE REPORT USAGE TO <ivanmr@altavista.com>.\n");
-
-/*
- * Initialize dev->board_name.
- */
-	dev->board_name = thisboard->name;
-
-	ret = comedi_alloc_subdevices(dev, 3);
-	if (ret)
-		return ret;
-
-	s = dev->subdevices + 0;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->range_table = thisboard->ranges;
-	s->insn_write = cb_pcidda_ao_winsn;
-
-	/* s->subdev_flags |= SDF_CMD_READ; */
-	/* s->do_cmd = cb_pcidda_ai_cmd; */
-	/* s->do_cmdtest = cb_pcidda_ai_cmdtest; */
-
-	/*  two 8255 digital io subdevices */
-	s = dev->subdevices + 1;
-	subdev_8255_init(dev, s, NULL, devpriv->digitalio);
-	s = dev->subdevices + 2;
-	subdev_8255_init(dev, s, NULL, devpriv->digitalio + PORT2A);
-
-	dev_dbg(dev->class_dev, "eeprom:\n");
-	for (index = 0; index < EEPROM_SIZE; index++) {
-		devpriv->eeprom_data[index] = cb_pcidda_read_eeprom(dev, index);
-		dev_dbg(dev->class_dev, "%i:0x%x\n", index,
-			devpriv->eeprom_data[index]);
-	}
-
-	/*  set calibrations dacs */
-	for (index = 0; index < thisboard->ao_chans; index++)
-		cb_pcidda_calibrate(dev, index, devpriv->ao_range[index]);
-
-	return 1;
-}
-
-static void cb_pcidda_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-	if (dev->subdevices) {
-		subdev_8255_cleanup(dev, dev->subdevices + 1);
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
-	}
-}
-
-/*
  * I will program this later... ;-)
  */
 #if 0
@@ -418,56 +248,26 @@ static int cb_pcidda_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually
-	 * compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER
-	    && cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_TIMER && cmd->stop_src != TRIG_EXT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -581,59 +381,10 @@ static int cb_pcidda_ns_to_timer(unsigned int *ns, int round)
 }
 #endif
 
-static int cb_pcidda_ao_winsn(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
-{
-	unsigned int command;
-	unsigned int channel, range;
-
-	channel = CR_CHAN(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
-
-	/*  adjust calibration dacs if range has changed */
-	if (range != devpriv->ao_range[channel])
-		cb_pcidda_calibrate(dev, channel, range);
-
-	/* output channel configuration */
-	command = NOSU | ENABLEDAC;
-
-	/* output channel range */
-	switch (range) {
-	case 0:
-		command |= BIP | RANGE10V;
-		break;
-	case 1:
-		command |= BIP | RANGE5V;
-		break;
-	case 2:
-		command |= BIP | RANGE2V5;
-		break;
-	case 3:
-		command |= UNIP | RANGE10V;
-		break;
-	case 4:
-		command |= UNIP | RANGE5V;
-		break;
-	case 5:
-		command |= UNIP | RANGE2V5;
-		break;
-	}
-
-	/* output channel specification */
-	command |= channel << 2;
-	outw(command, devpriv->dac + DACONTROL);
-
-	/* write data */
-	outw(data[0], devpriv->dac + DADATA + channel * 2);
-
-	/* return the number of samples read/written */
-	return 1;
-}
-
 /* lowlevel read from eeprom */
 static unsigned int cb_pcidda_serial_in(struct comedi_device *dev)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int value = 0;
 	int i;
 	const int value_width = 16;	/*  number of bits wide values are */
@@ -651,6 +402,7 @@ static unsigned int cb_pcidda_serial_in(struct comedi_device *dev)
 static void cb_pcidda_serial_out(struct comedi_device *dev, unsigned int value,
 				 unsigned int num_bits)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	int i;
 
 	for (i = 1; i <= num_bits; i++) {
@@ -667,6 +419,7 @@ static void cb_pcidda_serial_out(struct comedi_device *dev, unsigned int value,
 static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 					  unsigned int address)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int i;
 	unsigned int cal2_bits;
 	unsigned int value;
@@ -703,6 +456,7 @@ static void cb_pcidda_write_caldac(struct comedi_device *dev,
 				   unsigned int caldac, unsigned int channel,
 				   unsigned int value)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int cal2_bits;
 	unsigned int i;
 	/* caldacs use 3 bit channel specification */
@@ -797,6 +551,7 @@ static unsigned int eeprom_fine_byte(unsigned int word)
 static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 				unsigned int range)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int coarse_offset, fine_offset, coarse_gain, fine_gain;
 
 	/* remember range so we can tell when we need to readjust calibration */
@@ -827,10 +582,164 @@ static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 			       fine_gain_channel(channel), fine_gain);
 }
 
+static int cb_pcidda_ao_winsn(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn, unsigned int *data)
+{
+	struct cb_pcidda_private *devpriv = dev->private;
+	unsigned int command;
+	unsigned int channel, range;
+
+	channel = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+
+	/*  adjust calibration dacs if range has changed */
+	if (range != devpriv->ao_range[channel])
+		cb_pcidda_calibrate(dev, channel, range);
+
+	/* output channel configuration */
+	command = NOSU | ENABLEDAC;
+
+	/* output channel range */
+	switch (range) {
+	case 0:
+		command |= BIP | RANGE10V;
+		break;
+	case 1:
+		command |= BIP | RANGE5V;
+		break;
+	case 2:
+		command |= BIP | RANGE2V5;
+		break;
+	case 3:
+		command |= UNIP | RANGE10V;
+		break;
+	case 4:
+		command |= UNIP | RANGE5V;
+		break;
+	case 5:
+		command |= UNIP | RANGE2V5;
+		break;
+	}
+
+	/* output channel specification */
+	command |= channel << 2;
+	outw(command, devpriv->dac + DACONTROL);
+
+	/* write data */
+	outw(data[0], devpriv->dac + DADATA + channel * 2);
+
+	/* return the number of samples read/written */
+	return 1;
+}
+
+static const void *cb_pcidda_find_boardinfo(struct comedi_device *dev,
+					    struct pci_dev *pcidev)
+{
+	const struct cb_pcidda_board *thisboard;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cb_pcidda_boards); i++) {
+		thisboard = &cb_pcidda_boards[i];
+		if (thisboard->device_id != pcidev->device)
+			return thisboard;
+	}
+	return NULL;
+}
+
+static int cb_pcidda_attach_pci(struct comedi_device *dev,
+				struct pci_dev *pcidev)
+{
+	const struct cb_pcidda_board *thisboard;
+	struct cb_pcidda_private *devpriv;
+	struct comedi_subdevice *s;
+	int index;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	thisboard = cb_pcidda_find_boardinfo(dev, pcidev);
+	if (!pcidev)
+		return -ENODEV;
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+
+	devpriv->digitalio = pci_resource_start(pcidev, 2);
+	devpriv->dac = pci_resource_start(pcidev, 3);
+	dev->iobase = devpriv->dac;
+
+	if (thisboard->status == 2)
+		printk
+		    ("WARNING: DRIVER FOR THIS BOARD NOT CHECKED WITH MANUAL. "
+		     "WORKS ASSUMING FULL COMPATIBILITY WITH PCI-DDA08/12. "
+		     "PLEASE REPORT USAGE TO <ivanmr@altavista.com>.\n");
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[0];
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = (1 << thisboard->ao_bits) - 1;
+	s->range_table = thisboard->ranges;
+	s->insn_write = cb_pcidda_ao_winsn;
+
+	/* s->subdev_flags |= SDF_CMD_READ; */
+	/* s->do_cmd = cb_pcidda_ai_cmd; */
+	/* s->do_cmdtest = cb_pcidda_ai_cmdtest; */
+
+	/*  two 8255 digital io subdevices */
+	s = &dev->subdevices[1];
+	subdev_8255_init(dev, s, NULL, devpriv->digitalio);
+	s = &dev->subdevices[2];
+	subdev_8255_init(dev, s, NULL, devpriv->digitalio + PORT2A);
+
+	dev_dbg(dev->class_dev, "eeprom:\n");
+	for (index = 0; index < EEPROM_SIZE; index++) {
+		devpriv->eeprom_data[index] = cb_pcidda_read_eeprom(dev, index);
+		dev_dbg(dev->class_dev, "%i:0x%x\n", index,
+			devpriv->eeprom_data[index]);
+	}
+
+	/*  set calibrations dacs */
+	for (index = 0; index < thisboard->ao_chans; index++)
+		cb_pcidda_calibrate(dev, index, devpriv->ao_range[index]);
+
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+
+	return 0;
+}
+
+static void cb_pcidda_detach(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (dev->subdevices) {
+		subdev_8255_cleanup(dev, &dev->subdevices[1]);
+		subdev_8255_cleanup(dev, &dev->subdevices[2]);
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+	}
+}
+
 static struct comedi_driver cb_pcidda_driver = {
 	.driver_name	= "cb_pcidda",
 	.module		= THIS_MODULE,
-	.attach		= cb_pcidda_attach,
+	.attach_pci	= cb_pcidda_attach_pci,
 	.detach		= cb_pcidda_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidio.c b/drivers/staging/comedi/drivers/cb_pcidio.c
deleted file mode 100644
index e370d0d81bbd..000000000000
--- a/drivers/staging/comedi/drivers/cb_pcidio.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
-    comedi/drivers/cb_pcidio.c
-    A Comedi driver for PCI-DIO24H & PCI-DIO48H of ComputerBoards (currently MeasurementComputing)
-
-    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.
-
-    You 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.
-
-*/
-/*
-Driver: cb_pcidio
-Description: ComputerBoards' DIO boards with PCI interface
-Devices: [Measurement Computing] PCI-DIO24 (cb_pcidio), PCI-DIO24H, PCI-DIO48H
-Author: Yoshiya Matsuzaka
-Updated: Mon, 29 Oct 2007 15:40:47 +0000
-Status: experimental
-
-This driver has been modified from skel.c of comedi-0.7.70.
-
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first available PCI device will
-  be used.
-
-Passing a zero for an option is the same as leaving it unspecified.
-*/
-
-/*------------------------------ HEADER FILES ---------------------------------*/
-#include "../comedidev.h"
-#include "8255.h"
-
-/*-------------------------- MACROS and DATATYPES -----------------------------*/
-#define PCI_VENDOR_ID_CB	0x1307
-
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
-struct pcidio_board {
-	const char *name;	/*  name of the board */
-	int dev_id;
-	int n_8255;		/*  number of 8255 chips on board */
-
-	/*  indices of base address regions */
-	int pcicontroler_badrindex;
-	int dioregs_badrindex;
-};
-
-static const struct pcidio_board pcidio_boards[] = {
-	{
-	 .name = "pci-dio24",
-	 .dev_id = 0x0028,
-	 .n_8255 = 1,
-	 .pcicontroler_badrindex = 1,
-	 .dioregs_badrindex = 2,
-	 },
-	{
-	 .name = "pci-dio24h",
-	 .dev_id = 0x0014,
-	 .n_8255 = 1,
-	 .pcicontroler_badrindex = 1,
-	 .dioregs_badrindex = 2,
-	 },
-	{
-	 .name = "pci-dio48h",
-	 .dev_id = 0x000b,
-	 .n_8255 = 2,
-	 .pcicontroler_badrindex = 0,
-	 .dioregs_badrindex = 1,
-	 },
-};
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct pcidio_board *)dev->board_ptr)
-
-static struct pci_dev *pcidio_find_pci_dev(struct comedi_device *dev,
-					   struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-				slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_CB)
-			continue;
-		for (i = 0; i < ARRAY_SIZE(pcidio_boards); i++) {
-			if (pcidio_boards[i].dev_id != pcidev->device)
-				continue;
-
-			dev->board_ptr = pcidio_boards + i;
-			return pcidev;
-		}
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	int i;
-	int ret;
-
-	pcidev = pcidio_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-	if (comedi_pci_enable(pcidev, thisboard->name))
-		return -EIO;
-
-	dev->iobase = pci_resource_start(pcidev, thisboard->dioregs_badrindex);
-
-	ret = comedi_alloc_subdevices(dev, thisboard->n_8255);
-	if (ret)
-		return ret;
-
-	for (i = 0; i < thisboard->n_8255; i++) {
-		subdev_8255_init(dev, dev->subdevices + i,
-				 NULL, dev->iobase + i * 4);
-		dev_dbg(dev->class_dev, "subdev %d: base = 0x%lx\n", i,
-			dev->iobase + i * 4);
-	}
-
-	return 1;
-}
-
-static void pcidio_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-	if (dev->subdevices) {
-		int i;
-		for (i = 0; i < thisboard->n_8255; i++)
-			subdev_8255_cleanup(dev, dev->subdevices + i);
-	}
-}
-
-static struct comedi_driver cb_pcidio_driver = {
-	.driver_name	= "cb_pcidio",
-	.module		= THIS_MODULE,
-	.attach		= pcidio_attach,
-	.detach		= pcidio_detach,
-};
-
-static int __devinit cb_pcidio_pci_probe(struct pci_dev *dev,
-						const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &cb_pcidio_driver);
-}
-
-static void __devexit cb_pcidio_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static DEFINE_PCI_DEVICE_TABLE(cb_pcidio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0028) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0014) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000b) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, cb_pcidio_pci_table);
-
-static struct pci_driver cb_pcidio_pci_driver = {
-	.name		= "cb_pcidio",
-	.id_table	= cb_pcidio_pci_table,
-	.probe		= cb_pcidio_pci_probe,
-	.remove		= __devexit_p(cb_pcidio_pci_remove),
-};
-module_comedi_pci_driver(cb_pcidio_driver, cb_pcidio_pci_driver);
-
-MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index c632a89f3ae9..9515b6926662 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -119,11 +119,6 @@ static const struct cb_pcimdas_board cb_pcimdas_boards[] = {
 };
 
 /*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct cb_pcimdas_board *)dev->board_ptr)
-
-/*
  * this structure is for data unique to this hardware driver.  If
  * several hardware drivers keep similar information in this structure,
  * feel free to suggest moving the variable to the struct comedi_device
@@ -138,160 +133,6 @@ struct cb_pcimdas_private {
 };
 
 /*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct cb_pcimdas_private *)dev->private)
-
-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_ao_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
-static struct pci_dev *cb_pcimdas_find_pci_dev(struct comedi_device *dev,
-					       struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-				slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(cb_pcimdas_boards); i++) {
-			if (cb_pcimdas_boards[i].device_id != pcidev->device)
-				continue;
-
-			dev->board_ptr = cb_pcimdas_boards + i;
-			return pcidev;
-		}
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int cb_pcimdas_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *s;
-	unsigned long iobase_8255;
-	int ret;
-
-/*
- * Allocate the private structure area.
- */
-	if (alloc_private(dev, sizeof(struct cb_pcimdas_private)) < 0)
-		return -ENOMEM;
-
-	pcidev = cb_pcimdas_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	/*  Warn about non-tested features */
-	switch (thisboard->device_id) {
-	case 0x56:
-		break;
-	default:
-		dev_dbg(dev->class_dev, "THIS CARD IS UNSUPPORTED.\n");
-		dev_dbg(dev->class_dev,
-			"PLEASE REPORT USAGE TO <mocelet@sucs.org>\n");
-	}
-
-	if (comedi_pci_enable(pcidev, "cb_pcimdas")) {
-		dev_err(dev->class_dev,
-			"Failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
-
-	dev->iobase = pci_resource_start(pcidev, 2);
-	devpriv->BADR3 = pci_resource_start(pcidev, 3);
-	iobase_8255 = pci_resource_start(pcidev, 4);
-
-/* Dont support IRQ yet */
-/*  get irq */
-/* if(request_irq(pcidev->irq, cb_pcimdas_interrupt, IRQF_SHARED, "cb_pcimdas", dev )) */
-/* { */
-/* printk(" unable to allocate irq %u\n", pcidev->irq); */
-/* return -EINVAL; */
-/* } */
-/* dev->irq = pcidev->irq; */
-
-	/* Initialize dev->board_name */
-	dev->board_name = thisboard->name;
-
-	ret = comedi_alloc_subdevices(dev, 3);
-	if (ret)
-		return ret;
-
-	s = dev->subdevices + 0;
-	/* dev->read_subdev=s; */
-	/*  analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = thisboard->ai_se_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	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 = dev->subdevices + 1;
-	/*  analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_nchan;
-	s->maxdata = 1 << thisboard->ao_bits;
-	/* ranges are hardware settable, but not software readable. */
-	s->range_table = &range_unknown;
-	s->insn_write = &cb_pcimdas_ao_winsn;
-	s->insn_read = &cb_pcimdas_ao_rinsn;
-
-	s = dev->subdevices + 2;
-	/* digital i/o subdevice */
-	if (thisboard->has_dio)
-		subdev_8255_init(dev, s, NULL, iobase_8255);
-	else
-		s->type = COMEDI_SUBD_UNUSED;
-
-	return 1;
-}
-
-static void cb_pcimdas_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (pcidev) {
-		if (dev->iobase)
-			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
-	}
-}
-
-/*
  * "instructions" read/write data in "one-shot" or "software-triggered"
  * mode.
  */
@@ -299,6 +140,8 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	const struct cb_pcimdas_board *thisboard = comedi_board(dev);
+	struct cb_pcimdas_private *devpriv = dev->private;
 	int n, i;
 	unsigned int d;
 	unsigned int busy;
@@ -368,6 +211,7 @@ static int cb_pcimdas_ao_winsn(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct cb_pcimdas_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -397,6 +241,7 @@ static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct cb_pcimdas_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -406,10 +251,124 @@ static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
+static const void *cb_pcimdas_find_boardinfo(struct comedi_device *dev,
+					     struct pci_dev *pcidev)
+{
+	const struct cb_pcimdas_board *thisboard;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cb_pcimdas_boards); i++) {
+		thisboard = &cb_pcimdas_boards[i];
+		if (thisboard->device_id == pcidev->device)
+			return thisboard;
+	}
+	return NULL;
+}
+
+static int cb_pcimdas_attach_pci(struct comedi_device *dev,
+				 struct pci_dev *pcidev)
+{
+	const struct cb_pcimdas_board *thisboard;
+	struct cb_pcimdas_private *devpriv;
+	struct comedi_subdevice *s;
+	unsigned long iobase_8255;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	thisboard = cb_pcimdas_find_boardinfo(dev, pcidev);
+	if (!thisboard)
+		return -ENODEV;
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
+
+	/*  Warn about non-tested features */
+	switch (thisboard->device_id) {
+	case 0x56:
+		break;
+	default:
+		dev_dbg(dev->class_dev, "THIS CARD IS UNSUPPORTED.\n");
+		dev_dbg(dev->class_dev,
+			"PLEASE REPORT USAGE TO <mocelet@sucs.org>\n");
+	}
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pcidev, 2);
+	devpriv->BADR3 = pci_resource_start(pcidev, 3);
+	iobase_8255 = pci_resource_start(pcidev, 4);
+
+/* Dont support IRQ yet */
+/*  get irq */
+/* if(request_irq(pcidev->irq, cb_pcimdas_interrupt, IRQF_SHARED, "cb_pcimdas", dev )) */
+/* { */
+/* printk(" unable to allocate irq %u\n", pcidev->irq); */
+/* return -EINVAL; */
+/* } */
+/* dev->irq = pcidev->irq; */
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	s = &dev->subdevices[0];
+	/* dev->read_subdev=s; */
+	/*  analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND;
+	s->n_chan = thisboard->ai_se_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	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 = &dev->subdevices[1];
+	/*  analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = thisboard->ao_nchan;
+	s->maxdata = 1 << thisboard->ao_bits;
+	/* ranges are hardware settable, but not software readable. */
+	s->range_table = &range_unknown;
+	s->insn_write = &cb_pcimdas_ao_winsn;
+	s->insn_read = &cb_pcimdas_ao_rinsn;
+
+	s = &dev->subdevices[2];
+	/* digital i/o subdevice */
+	if (thisboard->has_dio)
+		subdev_8255_init(dev, s, NULL, iobase_8255);
+	else
+		s->type = COMEDI_SUBD_UNUSED;
+
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+
+	return 0;
+}
+
+static void cb_pcimdas_detach(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+	}
+}
+
 static struct comedi_driver cb_pcimdas_driver = {
 	.driver_name	= "cb_pcimdas",
 	.module		= THIS_MODULE,
-	.attach		= cb_pcimdas_attach,
+	.attach_pci	= cb_pcimdas_attach_pci,
 	.detach		= cb_pcimdas_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index a80146133c04..ba9f0599be28 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -57,12 +57,7 @@ output modes on the board:
     then issue one comedi_data_read() on any channel on the AO subdevice
     to initiate the simultaneous XFER.
 
-Configuration Options:
-  [0] PCI bus (optional)
-  [1] PCI slot (optional)
-  [2] analog output range jumper setting
-      0 == +/- 5 V
-      1 == +/- 10 V
+Configuration Options: not applicable, uses PCI auto config
 */
 
 /*
@@ -93,337 +88,133 @@ Configuration Options:
 #define PCI_ID_PCIM_DDA06_16		0x0053
 
 /*
- * This is straight from skel.c -- I did this in case this source file
- * will someday support more than 1 board...
+ * Register map, 8-bit access only
  */
-struct board_struct {
-	const char *name;
-	unsigned short device_id;
-	int ao_chans;
-	int ao_bits;
-	int dio_chans;
-	int dio_method;
-	/* how many bytes into the BADR are the DIO ports */
-	int dio_offset;
-	int regs_badrindex;	/* IO Region for the control, analog output,
-				   and DIO registers */
-	int reg_sz;		/* number of bytes of registers in io region */
-};
+#define PCIMDDA_DA_CHAN(x)		(0x00 + (x) * 2)
+#define PCIMDDA_8255_BASE_REG		0x0c
 
-enum DIO_METHODS {
-	DIO_NONE = 0,
-	DIO_8255,
-	DIO_INTERNAL		/* unimplemented */
-};
+#define MAX_AO_READBACK_CHANNELS	6
 
-static const struct board_struct boards[] = {
-	{
-	 .name = "cb_pcimdda06-16",
-	 .device_id = PCI_ID_PCIM_DDA06_16,
-	 .ao_chans = 6,
-	 .ao_bits = 16,
-	 .dio_chans = 24,
-	 .dio_method = DIO_8255,
-	 .dio_offset = 12,
-	 .regs_badrindex = 3,
-	 .reg_sz = 16,
-	 }
+struct cb_pcimdda_private {
+	unsigned int ao_readback[MAX_AO_READBACK_CHANNELS];
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard    ((const struct board_struct *)dev->board_ptr)
-
-#define REG_SZ (thisboard->reg_sz)
-#define REGS_BADRINDEX (thisboard->regs_badrindex)
-
-/*
- * this structure is for data unique to this hardware driver.  If
- * several hardware drivers keep similar information in this structure,
- * feel free to suggest moving the variable to the struct comedi_device
- * struct.
- */
-struct board_private_struct {
-	unsigned long registers;	/* set by probe */
-	unsigned long dio_registers;
-	char attached_to_8255;	/* boolean */
-	/* would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-
-#define MAX_AO_READBACK_CHANNELS 6
-	/* Used for AO readback */
-	unsigned int ao_readback[MAX_AO_READBACK_CHANNELS];
+static int cb_pcimdda_ao_winsn(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
+{
+	struct cb_pcimdda_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long offset = dev->iobase + PCIMDDA_DA_CHAN(chan);
+	unsigned int val = 0;
+	int i;
 
-};
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
 
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct board_private_struct *)dev->private)
+		/*
+		 * Write the LSB then MSB.
+		 *
+		 * If the simultaneous xfer mode is selected by the
+		 * jumper on the card, a read instruction is needed
+		 * in order to initiate the simultaneous transfer.
+		 * Otherwise, the DAC will be updated when the MSB
+		 * is written.
+		 */
+		outb(val & 0x00ff, offset);
+		outb((val >> 8) & 0x00ff, offset + 1);
+	}
 
-static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data);
-static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data);
+	/* Cache the last value for readback */
+	devpriv->ao_readback[chan] = val;
 
-/*---------------------------------------------------------------------------
-  HELPER FUNCTION DECLARATIONS
------------------------------------------------------------------------------*/
+	return insn->n;
+}
 
-/* returns a maxdata value for a given n_bits */
-static inline unsigned int figure_out_maxdata(int bits)
+static int cb_pcimdda_ao_rinsn(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	return ((unsigned int)1 << bits) - 1;
-}
+	struct cb_pcimdda_private *devpriv = dev->private;
+	int chan = CR_CHAN(insn->chanspec);
+	unsigned long offset = dev->iobase + PCIMDDA_DA_CHAN(chan);
+	int i;
 
-/*
- *  Probes for a supported device.
- *
- *  Prerequisite: private be allocated already inside dev
- *
- *  If the device is found, it returns 0 and has the following side effects:
- *
- *  o  assigns a struct pci_dev * to dev->private->pci_dev
- *  o  assigns a struct board * to dev->board_ptr
- *  o  sets dev->private->registers
- *  o  sets dev->private->dio_registers
- *
- *  Otherwise, returns a -errno on error
- */
-static int probe(struct comedi_device *dev, const struct comedi_devconfig *it);
+	for (i = 0; i < insn->n; i++) {
+		/* Initiate the simultaneous transfer */
+		inw(offset);
+
+		data[i] = devpriv->ao_readback[chan];
+	}
 
-/*---------------------------------------------------------------------------
-  FUNCTION DEFINITIONS
------------------------------------------------------------------------------*/
+	return insn->n;
+}
 
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int cb_pcimdda_attach_pci(struct comedi_device *dev,
+				 struct pci_dev *pcidev)
 {
+	struct cb_pcimdda_private *devpriv;
 	struct comedi_subdevice *s;
-	int err;
+	int ret;
 
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- * if this function fails (returns negative) then the private area is
- * kfree'd by comedi
- */
-	if (alloc_private(dev, sizeof(struct board_private_struct)) < 0)
-		return -ENOMEM;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	dev->board_name = dev->driver->driver_name;
 
-/*
- * If you can probe the device to determine what device in a series
- * it is, this is the place to do it.  Otherwise, dev->board_ptr
- * should already be initialized.
- */
-	err = probe(dev, it);
-	if (err)
-		return err;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
-/* Output some info */
-	printk("comedi%d: %s: ", dev->minor, thisboard->name);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, 3);
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-	err = comedi_alloc_subdevices(dev, 2);
-	if (err)
-		return err;
-
-	s = dev->subdevices + 0;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
+	s = &dev->subdevices[0];
 	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = figure_out_maxdata(thisboard->ao_bits);
-	/* this is hard-coded here */
-	if (it->options[2])
-		s->range_table = &range_bipolar10;
-	else
-		s->range_table = &range_bipolar5;
-	s->insn_write = &ao_winsn;
-	s->insn_read = &ao_rinsn;
-
-	s = dev->subdevices + 1;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->n_chan	= 6;
+	s->maxdata	= 0xffff;
+	s->range_table	= &range_bipolar5;
+	s->insn_write	= cb_pcimdda_ao_winsn;
+	s->insn_read	= cb_pcimdda_ao_rinsn;
+
+	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
-	if (thisboard->dio_chans) {
-		switch (thisboard->dio_method) {
-		case DIO_8255:
-			/*
-			 * this is a straight 8255, so register us with
-			 * the 8255 driver
-			 */
-			subdev_8255_init(dev, s, NULL, devpriv->dio_registers);
-			devpriv->attached_to_8255 = 1;
-			break;
-		case DIO_INTERNAL:
-		default:
-			printk("DIO_INTERNAL not implemented yet!\n");
-			return -ENXIO;
-			break;
-		}
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
+	ret = subdev_8255_init(dev, s, NULL,
+			dev->iobase + PCIMDDA_8255_BASE_REG);
+	if (ret)
+		return ret;
 
-	printk("attached\n");
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
 	return 1;
 }
 
-static void detach(struct comedi_device *dev)
-{
-	if (devpriv) {
-		if (dev->subdevices && devpriv->attached_to_8255) {
-			subdev_8255_cleanup(dev, dev->subdevices + 2);
-			devpriv->attached_to_8255 = 0;
-		}
-		if (devpriv->pci_dev) {
-			if (devpriv->registers)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
-	}
-}
-
-static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data)
+static void cb_pcimdda_detach(struct comedi_device *dev)
 {
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned long offset = devpriv->registers + chan * 2;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
-	for (i = 0; i < insn->n; i++) {
-		/*  first, load the low byte */
-		outb((char)(data[i] & 0x00ff), offset);
-		/*  next, write the high byte -- only after this is written is
-		   the channel voltage updated in the DAC, unless
-		   we're in simultaneous xfer mode (jumper on card)
-		   then a rinsn is necessary to actually update the DAC --
-		   see ao_rinsn() below... */
-		outb((char)(data[i] >> 8 & 0x00ff), offset + 1);
-
-		/* for testing only.. the actual rinsn SHOULD do an inw!
-		   (see the stuff about simultaneous XFER mode on this board) */
-		devpriv->ao_readback[chan] = data[i];
+	if (dev->subdevices)
+		subdev_8255_cleanup(dev, &dev->subdevices[1]);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
 	}
-
-	/* return the number of samples read/written */
-	return i;
-}
-
-/* AO subdevices should have a read insn as well as a write insn.
-
-   Usually this means copying a value stored in devpriv->ao_readback.
-   However, since this board has this jumper setting called "Simultaneous
-   Xfer mode" (off by default), we will support it.  Simultaneaous xfer
-   mode is accomplished by loading ALL the values you want for AO in all the
-   channels, then READing off one of the AO registers to initiate the
-   instantaneous simultaneous update of all DAC outputs, which makes
-   all AO channels update simultaneously.  This is useful for some control
-   applications, I would imagine.
-*/
-static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data)
-{
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++) {
-		inw(devpriv->registers + chan * 2);
-		/*
-		 * should I set data[i] to the result of the actual read
-		 * on the register or the cached unsigned int in
-		 * devpriv->ao_readback[]?
-		 */
-		data[i] = devpriv->ao_readback[chan];
-	}
-
-	return i;
-}
-
-/*---------------------------------------------------------------------------
-  HELPER FUNCTION DEFINITIONS
------------------------------------------------------------------------------*/
-
-/*
- *  Probes for a supported device.
- *
- *  Prerequisite: private be allocated already inside dev
- *
- *  If the device is found, it returns 0 and has the following side effects:
- *
- *  o  assigns a struct pci_dev * to dev->private->pci_dev
- *  o  assigns a struct board * to dev->board_ptr
- *  o  sets dev->private->registers
- *  o  sets dev->private->dio_registers
- *
- *  Otherwise, returns a -errno on error
- */
-static int probe(struct comedi_device *dev, const struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int index;
-	unsigned long registers;
-
-	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
-		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
-			continue;
-		/*  loop through cards supported by this driver */
-		for (index = 0; index < ARRAY_SIZE(boards); index++) {
-			if (boards[index].device_id != pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			/* found ! */
-
-			devpriv->pci_dev = pcidev;
-			dev->board_ptr = boards + index;
-			if (comedi_pci_enable(pcidev, thisboard->name)) {
-				printk
-				    ("cb_pcimdda: Failed to enable PCI device and request regions\n");
-				return -EIO;
-			}
-			registers =
-			    pci_resource_start(devpriv->pci_dev,
-					       REGS_BADRINDEX);
-			devpriv->registers = registers;
-			devpriv->dio_registers
-			    = devpriv->registers + thisboard->dio_offset;
-			return 0;
-		}
-	}
-
-	printk("cb_pcimdda: No supported ComputerBoards/MeasurementComputing "
-	       "card found at the requested position\n");
-	return -ENODEV;
 }
 
 static struct comedi_driver cb_pcimdda_driver = {
 	.driver_name	= "cb_pcimdda",
 	.module		= THIS_MODULE,
-	.attach		= attach,
-	.detach		= detach,
+	.attach_pci	= cb_pcimdda_attach_pci,
+	.detach		= cb_pcimdda_detach,
 };
 
 static int __devinit cb_pcimdda_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index 5ed324c4f620..5c768bc76eb1 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -59,40 +59,6 @@ Configuration Options:
 /* The maxiumum number of channels per subdevice. */
 #define MAX_CHANS 256
 
-#define MODULE_NAME "comedi_bond"
-#ifndef STR
-#  define STR1(x) #x
-#  define STR(x) STR1(x)
-#endif
-
-static int debug;
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug, "If true, print extra cryptic debugging output useful"
-		 "only to developers.");
-
-#define LOG_MSG(x...) printk(KERN_INFO MODULE_NAME": "x)
-#define DEBUG(x...)							\
-	do {								\
-		if (debug)						\
-			printk(KERN_DEBUG MODULE_NAME": DEBUG: "x);	\
-	} while (0)
-#define WARNING(x...)  printk(KERN_WARNING MODULE_NAME ": WARNING: "x)
-#define ERROR(x...)  printk(KERN_ERR MODULE_NAME ": INTERNAL ERROR: "x)
-
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
-struct BondingBoard {
-	const char *name;
-};
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct BondingBoard *)dev->board_ptr)
-
 struct BondedDevice {
 	struct comedi_device *dev;
 	unsigned minor;
@@ -107,7 +73,7 @@ struct BondedDevice {
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.  */
-struct Private {
+struct comedi_bond_private {
 # define MAX_BOARD_NAME 256
 	char name[MAX_BOARD_NAME];
 	struct BondedDevice **devs;
@@ -116,12 +82,6 @@ struct Private {
 	unsigned nchans;
 };
 
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct Private *)dev->private)
-
 /* DIO devices are slightly special.  Although it is possible to
  * implement the insn_read/insn_write interface, it is much more
  * useful to applications if you implement the insn_bits interface.
@@ -131,6 +91,7 @@ static int bonding_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct comedi_bond_private *devpriv = dev->private;
 #define LSAMPL_BITS (sizeof(unsigned int)*8)
 	unsigned nchans = LSAMPL_BITS, num_done = 0, i;
 
@@ -177,6 +138,7 @@ static int bonding_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	struct comedi_bond_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec), ret, io_bits = s->io_bits;
 	unsigned int io;
 	struct BondedDevice *bdev;
@@ -230,6 +192,7 @@ static void *Realloc(const void *oldmem, size_t newlen, size_t oldlen)
 
 static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	struct comedi_bond_private *devpriv = dev->private;
 	int i;
 	struct comedi_device *devs_opened[COMEDI_NUM_BOARD_MINORS];
 
@@ -245,15 +208,18 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 		struct BondedDevice *bdev = NULL;
 
 		if (minor < 0 || minor >= COMEDI_NUM_BOARD_MINORS) {
-			ERROR("Minor %d is invalid!\n", minor);
+			dev_err(dev->class_dev,
+				"Minor %d is invalid!\n", minor);
 			return 0;
 		}
 		if (minor == dev->minor) {
-			ERROR("Cannot bond this driver to itself!\n");
+			dev_err(dev->class_dev,
+				"Cannot bond this driver to itself!\n");
 			return 0;
 		}
 		if (devs_opened[minor]) {
-			ERROR("Minor %d specified more than once!\n", minor);
+			dev_err(dev->class_dev,
+				"Minor %d specified more than once!\n", minor);
 			return 0;
 		}
 
@@ -263,7 +229,8 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 		d = devs_opened[minor] = comedi_open(file);
 
 		if (!d) {
-			ERROR("Minor %u could not be opened\n", minor);
+			dev_err(dev->class_dev,
+				"Minor %u could not be opened\n", minor);
 			return 0;
 		}
 
@@ -272,14 +239,14 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 							     sdev + 1)) > -1) {
 			nchans = comedi_get_n_channels(d, sdev);
 			if (nchans <= 0) {
-				ERROR("comedi_get_n_channels() returned %d "
-				      "on minor %u subdev %d!\n",
-				      nchans, minor, sdev);
+				dev_err(dev->class_dev,
+					"comedi_get_n_channels() returned %d on minor %u subdev %d!\n",
+					nchans, minor, sdev);
 				return 0;
 			}
 			bdev = kmalloc(sizeof(*bdev), GFP_KERNEL);
 			if (!bdev) {
-				ERROR("Out of memory.\n");
+				dev_err(dev->class_dev, "Out of memory\n");
 				return 0;
 			}
 			bdev->dev = d;
@@ -302,8 +269,8 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 			    Realloc(devpriv->devs,
 				    ++devpriv->ndevs * sizeof(bdev), tmp);
 			if (!devpriv->devs) {
-				ERROR("Could not allocate memory. "
-				      "Out of memory?");
+				dev_err(dev->class_dev,
+					"Could not allocate memory. Out of memory?\n");
 				return 0;
 			}
 
@@ -323,7 +290,7 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	if (!devpriv->nchans) {
-		ERROR("No channels found!\n");
+		dev_err(dev->class_dev, "No channels found!\n");
 		return 0;
 	}
 
@@ -333,35 +300,28 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 static int bonding_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct comedi_bond_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	LOG_MSG("comedi%d\n", dev->minor);
-
-	/*
-	 * Allocate the private structure area.  alloc_private() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_private(dev, sizeof(struct Private)) < 0)
-		return -ENOMEM;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
 	/*
-	 * Setup our bonding from config params.. sets up our Private struct..
+	 * Setup our bonding from config params.. sets up our private struct..
 	 */
 	if (!doDevConfig(dev, it))
 		return -EINVAL;
 
-	/*
-	 * Initialize dev->board_name.  Note that we can use the "thisboard"
-	 * macro now, since we just initialized it in the last line.
-	 */
 	dev->board_name = devpriv->name;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = devpriv->nchans;
@@ -370,9 +330,9 @@ static int bonding_attach(struct comedi_device *dev,
 	s->insn_bits = bonding_dio_insn_bits;
 	s->insn_config = bonding_dio_insn_config;
 
-	LOG_MSG("attached with %u DIO channels coming from %u different "
-		"subdevices all bonded together.  "
-		"John Lennon would be proud!\n",
+	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);
 
 	return 1;
@@ -380,6 +340,7 @@ static int bonding_attach(struct comedi_device *dev,
 
 static void bonding_detach(struct comedi_device *dev)
 {
+	struct comedi_bond_private *devpriv = dev->private;
 	unsigned long devs_closed = 0;
 
 	if (devpriv) {
@@ -402,25 +363,16 @@ static void bonding_detach(struct comedi_device *dev)
 	}
 }
 
-static const struct BondingBoard bondingBoards[] = {
-	{
-		.name		= "comedi_bond",
-	},
-};
-
 static struct comedi_driver bonding_driver = {
 	.driver_name	= "comedi_bond",
 	.module		= THIS_MODULE,
 	.attach		= bonding_attach,
 	.detach		= bonding_detach,
-	.board_name	= &bondingBoards[0].name,
-	.offset		= sizeof(struct BondingBoard),
-	.num_names	= ARRAY_SIZE(bondingBoards),
 };
 module_comedi_driver(bonding_driver);
 
 MODULE_AUTHOR("Calin A. Culianu");
-MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
+MODULE_DESCRIPTION("comedi_bond: A driver for COMEDI to bond multiple COMEDI "
 		   "devices together as one.  In the words of John Lennon: "
 		   "'And the world will live as one...'");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
index 4b2cfd327995..94481c637a0a 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.h
+++ b/drivers/staging/comedi/drivers/comedi_fc.h
@@ -73,4 +73,36 @@ static inline unsigned int cfc_bytes_per_scan(struct comedi_subdevice *subd)
 	return num_samples * bytes_per_sample(subd);
 }
 
+/**
+ * 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;
+}
+
 #endif /* _COMEDI_FC_H */
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 9a63cac2434a..22ef94242590 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -85,6 +85,8 @@ pin, which can be used to wake up tasks.
 #include <linux/interrupt.h>
 #include <linux/ioport.h>
 
+#include "comedi_fc.h"
+
 #define PARPORT_SIZE 3
 
 #define PARPORT_A 0
@@ -96,11 +98,12 @@ struct parport_private {
 	unsigned int c_data;
 	int enable_irq;
 };
-#define devpriv ((struct parport_private *)(dev->private))
 
 static int parport_insn_a(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	struct parport_private *devpriv = dev->private;
+
 	if (data[0]) {
 		devpriv->a_data &= ~data[0];
 		devpriv->a_data |= (data[0] & data[1]);
@@ -117,6 +120,8 @@ static int parport_insn_config_a(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct parport_private *devpriv = dev->private;
+
 	if (data[0]) {
 		s->io_bits = 0xff;
 		devpriv->c_data &= ~(1 << 5);
@@ -145,6 +150,8 @@ static int parport_insn_b(struct comedi_device *dev, struct comedi_subdevice *s,
 static int parport_insn_c(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	struct parport_private *devpriv = dev->private;
+
 	data[0] &= 0x0f;
 	if (data[0]) {
 		devpriv->c_data &= ~data[0];
@@ -171,39 +178,20 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 				struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-
-	/* step 1 */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: ignored */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -245,6 +233,8 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 static int parport_intr_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	struct parport_private *devpriv = dev->private;
+
 	devpriv->c_data |= 0x10;
 	outb(devpriv->c_data, dev->iobase + PARPORT_C);
 
@@ -256,7 +246,7 @@ static int parport_intr_cmd(struct comedi_device *dev,
 static int parport_intr_cancel(struct comedi_device *dev,
 			       struct comedi_subdevice *s)
 {
-	printk(KERN_DEBUG "parport_intr_cancel()\n");
+	struct parport_private *devpriv = dev->private;
 
 	devpriv->c_data &= ~0x10;
 	outb(devpriv->c_data, dev->iobase + PARPORT_C);
@@ -269,12 +259,11 @@ static int parport_intr_cancel(struct comedi_device *dev,
 static irqreturn_t parport_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 3;
+	struct parport_private *devpriv = dev->private;
+	struct comedi_subdevice *s = &dev->subdevices[3];
 
-	if (!devpriv->enable_irq) {
-		printk(KERN_ERR "comedi_parport: bogus irq, ignored\n");
+	if (!devpriv->enable_irq)
 		return IRQ_NONE;
-	}
 
 	comedi_buf_put(s->async, 0);
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
@@ -286,41 +275,42 @@ static irqreturn_t parport_interrupt(int irq, void *d)
 static int parport_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct parport_private *devpriv;
 	int ret;
 	unsigned int irq;
 	unsigned long iobase;
 	struct comedi_subdevice *s;
 
+	dev->board_name = dev->driver->driver_name;
+
 	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: parport: 0x%04lx ", dev->minor, iobase);
-	if (!request_region(iobase, PARPORT_SIZE, "parport (comedi)")) {
-		printk(KERN_ERR "I/O port conflict\n");
+	if (!request_region(iobase, PARPORT_SIZE, dev->board_name)) {
+		dev_err(dev->class_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
 
 	irq = it->options[1];
 	if (irq) {
-		printk(KERN_INFO " irq=%u", irq);
-		ret = request_irq(irq, parport_interrupt, 0, "comedi_parport",
+		ret = request_irq(irq, parport_interrupt, 0, dev->board_name,
 				  dev);
 		if (ret < 0) {
-			printk(KERN_ERR " irq not available\n");
+			dev_err(dev->class_dev, "irq not available\n");
 			return -EINVAL;
 		}
 		dev->irq = irq;
 	}
-	dev->board_name = "parport";
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
-	ret = alloc_private(dev, sizeof(struct parport_private));
+	ret = alloc_private(dev, sizeof(*devpriv));
 	if (ret < 0)
 		return ret;
+	devpriv = dev->private;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 8;
@@ -329,7 +319,7 @@ static int parport_attach(struct comedi_device *dev,
 	s->insn_bits = parport_insn_a;
 	s->insn_config = parport_insn_config_a;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 5;
@@ -337,7 +327,7 @@ static int parport_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = parport_insn_b;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 4;
@@ -345,7 +335,7 @@ static int parport_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = parport_insn_c;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	if (irq) {
 		dev->read_subdev = s;
 		s->type = COMEDI_SUBD_DI;
@@ -366,8 +356,10 @@ static int parport_attach(struct comedi_device *dev,
 	devpriv->c_data = 0;
 	outb(devpriv->c_data, dev->iobase + PARPORT_C);
 
-	printk(KERN_INFO "\n");
-	return 1;
+	dev_info(dev->class_dev, "%s: iobase=0x%04lx, irq %sabled",
+		dev->board_name, dev->iobase, dev->irq ? "en" : "dis");
+
+	return 0;
 }
 
 static void parport_detach(struct comedi_device *dev)
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 523a809708b7..7817def1556c 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -57,14 +57,6 @@ zero volts).
 #include "comedi_fc.h"
 #include <linux/timer.h>
 
-/* Board descriptions */
-struct waveform_board {
-	const char *name;
-	int ai_chans;
-	int ai_bits;
-	int have_dio;
-};
-
 #define N_CHANS 8
 
 /* Data unique to this driver */
@@ -81,7 +73,6 @@ struct waveform_private {
 	unsigned timer_running:1;
 	unsigned int ao_loopbacks[N_CHANS];
 };
-#define devpriv ((struct waveform_private *)dev->private)
 
 /* 1000 nanosec in a microsec */
 static const int nano_per_micro = 1000;
@@ -98,6 +89,7 @@ static const struct comedi_lrange waveform_ai_ranges = {
 static short fake_sawtooth(struct comedi_device *dev, unsigned int range_index,
 			   unsigned long current_time)
 {
+	struct waveform_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	unsigned int offset = s->maxdata / 2;
 	u64 value;
@@ -122,6 +114,7 @@ static short fake_squarewave(struct comedi_device *dev,
 			     unsigned int range_index,
 			     unsigned long current_time)
 {
+	struct waveform_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	unsigned int offset = s->maxdata / 2;
 	u64 value;
@@ -175,6 +168,7 @@ static short fake_waveform(struct comedi_device *dev, unsigned int channel,
 static void waveform_ai_interrupt(unsigned long arg)
 {
 	struct comedi_device *dev = (struct comedi_device *)arg;
+	struct waveform_private *devpriv = dev->private;
 	struct comedi_async *async = dev->read_subdev->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int i, j;
@@ -237,44 +231,23 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW | TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->convert_src != TRIG_NOW && cmd->convert_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -362,6 +335,7 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 static int waveform_ai_cmd(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
+	struct waveform_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
 	if (cmd->flags & TRIG_RT) {
@@ -395,6 +369,8 @@ static int waveform_ai_cmd(struct comedi_device *dev,
 static int waveform_ai_cancel(struct comedi_device *dev,
 			      struct comedi_subdevice *s)
 {
+	struct waveform_private *devpriv = dev->private;
+
 	devpriv->timer_running = 0;
 	del_timer(&devpriv->timer);
 	return 0;
@@ -404,6 +380,7 @@ static int waveform_ai_insn_read(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct waveform_private *devpriv = dev->private;
 	int i, chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++)
@@ -416,6 +393,7 @@ static int waveform_ao_insn_write(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct waveform_private *devpriv = dev->private;
 	int i, chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++)
@@ -427,17 +405,19 @@ static int waveform_ao_insn_write(struct comedi_device *dev,
 static int waveform_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
-	const struct waveform_board *board = comedi_board(dev);
+	struct waveform_private *devpriv;
 	struct comedi_subdevice *s;
 	int amplitude = it->options[0];
 	int period = it->options[1];
 	int i;
 	int ret;
 
-	dev->board_name = board->name;
+	dev->board_name = dev->driver->driver_name;
 
-	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
-		return -ENOMEM;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
 
 	/* set default amplitude and period */
 	if (amplitude <= 0)
@@ -452,13 +432,13 @@ static int waveform_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	s->n_chan = board->ai_chans;
-	s->maxdata = (1 << board->ai_bits) - 1;
+	s->n_chan = N_CHANS;
+	s->maxdata = 0xffff;
 	s->range_table = &waveform_ai_ranges;
 	s->len_chanlist = s->n_chan * 2;
 	s->insn_read = waveform_ai_insn_read;
@@ -466,13 +446,13 @@ static int waveform_attach(struct comedi_device *dev,
 	s->do_cmdtest = waveform_ai_cmdtest;
 	s->cancel = waveform_ai_cancel;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	dev->write_subdev = s;
 	/* analog output subdevice (loopback) */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
-	s->n_chan = board->ai_chans;
-	s->maxdata = (1 << board->ai_bits) - 1;
+	s->n_chan = N_CHANS;
+	s->maxdata = 0xffff;
 	s->range_table = &waveform_ai_ranges;
 	s->len_chanlist = s->n_chan * 2;
 	s->insn_write = waveform_ao_insn_write;
@@ -488,35 +468,27 @@ static int waveform_attach(struct comedi_device *dev,
 	devpriv->timer.function = waveform_ai_interrupt;
 	devpriv->timer.data = (unsigned long)dev;
 
-	printk(KERN_INFO "comedi%d: comedi_test: "
-	       "%i microvolt, %li microsecond waveform attached\n", dev->minor,
-	       devpriv->uvolt_amplitude, devpriv->usec_period);
-	return 1;
+	dev_info(dev->class_dev,
+		"%s: %i microvolt, %li microsecond waveform attached\n",
+		dev->board_name,
+		devpriv->uvolt_amplitude, devpriv->usec_period);
+
+	return 0;
 }
 
 static void waveform_detach(struct comedi_device *dev)
 {
-	if (dev->private)
+	struct waveform_private *devpriv = dev->private;
+
+	if (devpriv)
 		waveform_ai_cancel(dev, dev->read_subdev);
 }
 
-static const struct waveform_board waveform_boards[] = {
-	{
-		.name		= "comedi_test",
-		.ai_chans	= N_CHANS,
-		.ai_bits	= 16,
-		.have_dio	= 0,
-	},
-};
-
 static struct comedi_driver waveform_driver = {
 	.driver_name	= "comedi_test",
 	.module		= THIS_MODULE,
 	.attach		= waveform_attach,
 	.detach		= waveform_detach,
-	.board_name	= &waveform_boards[0].name,
-	.offset		= sizeof(struct waveform_board),
-	.num_names	= ARRAY_SIZE(waveform_boards),
 };
 module_comedi_driver(waveform_driver);
 
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index 944cfeeb2b2d..178a6a4bb7d5 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -27,51 +27,35 @@ Author: Stefano Rivoir <s.rivoir@gts.it>
 Updated: Wed, 27 Jun 2007 13:00:06 +0100
 Status: works
 
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+Configuration Options: not applicable, uses comedi PCI auto config
 */
 
 #include "../comedidev.h"
 
-enum contec_model {
-	PIO1616L = 0,
-};
-
-struct contec_board {
-	const char *name;
-	int model;
-	int in_ports;
-	int out_ports;
-	int in_offs;
-	int out_offs;
-	int out_boffs;
-};
-static const struct contec_board contec_boards[] = {
-	{"PIO1616L", PIO1616L, 16, 16, 0, 2, 10},
-};
-
 #define PCI_DEVICE_ID_PIO1616L 0x8172
 
-#define thisboard ((const struct contec_board *)dev->board_ptr)
+/*
+ * Register map
+ */
+#define PIO1616L_DI_REG		0x00
+#define PIO1616L_DO_REG		0x02
 
 static int contec_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
 
-	dev_dbg(dev->class_dev, "contec_do_insn_bits called\n");
-	dev_dbg(dev->class_dev, "data: %d %d\n", data[0], data[1]);
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
 
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		dev_dbg(dev->class_dev, "out: %d on %lx\n", s->state,
-			dev->iobase + thisboard->out_offs);
-		outw(s->state, dev->iobase + thisboard->out_offs);
+		outw(s->state, dev->iobase + PIO1616L_DO_REG);
 	}
+
+	data[1] = s->state;
+
 	return insn->n;
 }
 
@@ -79,87 +63,49 @@ static int contec_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-
-	dev_dbg(dev->class_dev, "contec_di_insn_bits called\n");
-	dev_dbg(dev->class_dev, "data: %d %d\n", data[0], data[1]);
-
-	data[1] = inw(dev->iobase + thisboard->in_offs);
+	data[1] = inw(dev->iobase + PIO1616L_DI_REG);
 
 	return insn->n;
 }
 
-static struct pci_dev *contec_find_pci_dev(struct comedi_device *dev,
-					   struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-				slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_CONTEC ||
-		    pcidev->device != PCI_DEVICE_ID_PIO1616L)
-			continue;
-
-		dev->board_ptr = contec_boards + 0;
-		return pcidev;
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-static int contec_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int contec_attach_pci(struct comedi_device *dev,
+			     struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret;
 
-	printk("comedi%d: contec: ", dev->minor);
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	dev->board_name = thisboard->name;
+	dev->board_name = dev->driver->driver_name;
 
-	ret = comedi_alloc_subdevices(dev, 2);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
 	if (ret)
 		return ret;
-
-	pcidev = contec_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	if (comedi_pci_enable(pcidev, "contec_pci_dio")) {
-		printk("error enabling PCI device and request regions!\n");
-		return -EIO;
-	}
 	dev->iobase = pci_resource_start(pcidev, 0);
-	printk(" base addr %lx ", dev->iobase);
 
-	s = dev->subdevices + 0;
-
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = contec_di_insn_bits;
-
-	s = dev->subdevices + 1;
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = contec_do_insn_bits;
-
-	printk("attached\n");
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
-	return 1;
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= contec_di_insn_bits;
+
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 16;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= contec_do_insn_bits;
+
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
+
+	return 0;
 }
 
 static void contec_detach(struct comedi_device *dev)
@@ -169,14 +115,13 @@ static void contec_detach(struct comedi_device *dev)
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver contec_pci_dio_driver = {
 	.driver_name	= "contec_pci_dio",
 	.module		= THIS_MODULE,
-	.attach		= contec_attach,
+	.attach_pci	= contec_attach_pci,
 	.detach		= contec_detach,
 };
 
@@ -192,8 +137,7 @@ static void __devexit contec_pci_dio_pci_remove(struct pci_dev *dev)
 }
 
 static DEFINE_PCI_DEVICE_TABLE(contec_pci_dio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L),
-		.driver_data = PIO1616L },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, contec_pci_dio_pci_table);
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index cad559a1a730..d13c8c5822bf 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -31,16 +31,10 @@ Devices: [IOTech] DAQBoard/2000 (daqboard2000)
 Much of the functionality of this driver was determined from reading
 the source code for the Windows driver.
 
-The FPGA on the board requires initialization code, which can
-be loaded by comedi_config using the -i
-option.  The initialization code is available from http://www.comedi.org
-in the comedi_nonfree_firmware tarball.
-
-Configuration options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+The FPGA on the board requires fimware, which is available from
+http://www.comedi.org in the comedi_nonfree_firmware tarball.
+
+Configuration options: not applicable, uses PCI auto config
 */
 /*
    This card was obviously never intended to leave the Windows world,
@@ -117,17 +111,17 @@ Configuration options:
 
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/firmware.h>
 
 #include "8255.h"
 
+#define DAQBOARD2000_FIRMWARE		"daqboard2000_firmware.bin"
+
 #define PCI_VENDOR_ID_IOTECH		0x1616
 
 #define DAQBOARD2000_SUBSYSTEM_IDS2 	0x0002	/* Daqboard/2000 - 2 Dacs */
 #define DAQBOARD2000_SUBSYSTEM_IDS4 	0x0004	/* Daqboard/2000 - 4 Dacs */
 
-#define DAQBOARD2000_DAQ_SIZE 		0x1002
-#define DAQBOARD2000_PLX_SIZE 		0x100
-
 /* Initialization bits for the Serial EEPROM Control Register */
 #define DAQBOARD2000_SECRProgPinHi      0x8001767e
 #define DAQBOARD2000_SECRProgPinLo      0x8000767e
@@ -143,85 +137,56 @@ Configuration options:
 #define DAQBOARD2000_CPLD_INIT 		0x0002
 #define DAQBOARD2000_CPLD_DONE 		0x0004
 
-/* Available ranges */
-static const struct comedi_lrange range_daqboard2000_ai = { 13, {
-								 RANGE(-10, 10),
-								 RANGE(-5, 5),
-								 RANGE(-2.5,
-								       2.5),
-								 RANGE(-1.25,
-								       1.25),
-								 RANGE(-0.625,
-								       0.625),
-								 RANGE(-0.3125,
-								       0.3125),
-								 RANGE(-0.156,
-								       0.156),
-								 RANGE(0, 10),
-								 RANGE(0, 5),
-								 RANGE(0, 2.5),
-								 RANGE(0, 1.25),
-								 RANGE(0,
-								       0.625),
-								 RANGE(0,
-								       0.3125)
-								 }
-};
-
-static const struct comedi_lrange range_daqboard2000_ao = { 1, {
-								RANGE(-10, 10)
-								}
+static const struct comedi_lrange range_daqboard2000_ai = {
+	13, {
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25),
+		BIP_RANGE(0.625),
+		BIP_RANGE(0.3125),
+		BIP_RANGE(0.156),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2.5),
+		UNI_RANGE(1.25),
+		UNI_RANGE(0.625),
+		UNI_RANGE(0.3125)
+	}
 };
 
-struct daqboard2000_hw {
-	volatile u16 acqControl;	/*  0x00 */
-	volatile u16 acqScanListFIFO;	/*  0x02 */
-	volatile u32 acqPacerClockDivLow;	/*  0x04 */
-
-	volatile u16 acqScanCounter;	/*  0x08 */
-	volatile u16 acqPacerClockDivHigh;	/*  0x0a */
-	volatile u16 acqTriggerCount;	/*  0x0c */
-	volatile u16 fill2;	/*  0x0e */
-	volatile u16 acqResultsFIFO;	/*  0x10 */
-	volatile u16 fill3;	/*  0x12 */
-	volatile u16 acqResultsShadow;	/*  0x14 */
-	volatile u16 fill4;	/*  0x16 */
-	volatile u16 acqAdcResult;	/*  0x18 */
-	volatile u16 fill5;	/*  0x1a */
-	volatile u16 dacScanCounter;	/*  0x1c */
-	volatile u16 fill6;	/*  0x1e */
-
-	volatile u16 dacControl;	/*  0x20 */
-	volatile u16 fill7;	/*  0x22 */
-	volatile s16 dacFIFO;	/*  0x24 */
-	volatile u16 fill8[2];	/*  0x26 */
-	volatile u16 dacPacerClockDiv;	/*  0x2a */
-	volatile u16 refDacs;	/*  0x2c */
-	volatile u16 fill9;	/*  0x2e */
-
-	volatile u16 dioControl;	/*  0x30 */
-	volatile s16 dioP3hsioData;	/*  0x32 */
-	volatile u16 dioP3Control;	/*  0x34 */
-	volatile u16 calEepromControl;	/*  0x36 */
-	volatile s16 dacSetting[4];	/*  0x38 */
-	volatile s16 dioP2ExpansionIO8Bit[32];	/*  0x40 */
-
-	volatile u16 ctrTmrControl;	/*  0x80 */
-	volatile u16 fill10[3];	/*  0x82 */
-	volatile s16 ctrInput[4];	/*  0x88 */
-	volatile u16 fill11[8];	/*  0x90 */
-	volatile u16 timerDivisor[2];	/*  0xa0 */
-	volatile u16 fill12[6];	/*  0xa4 */
-
-	volatile u16 dmaControl;	/*  0xb0 */
-	volatile u16 trigControl;	/*  0xb2 */
-	volatile u16 fill13[2];	/*  0xb4 */
-	volatile u16 calEeprom;	/*  0xb8 */
-	volatile u16 acqDigitalMark;	/*  0xba */
-	volatile u16 trigDacs;	/*  0xbc */
-	volatile u16 fill14;	/*  0xbe */
-	volatile s16 dioP2ExpansionIO16Bit[32];	/*  0xc0 */
-};
+/*
+ * Register Memory Map
+ */
+#define acqControl			0x00		/* u16 */
+#define acqScanListFIFO			0x02		/* u16 */
+#define acqPacerClockDivLow		0x04		/* u32 */
+#define acqScanCounter			0x08		/* u16 */
+#define acqPacerClockDivHigh		0x0a		/* u16 */
+#define acqTriggerCount			0x0c		/* u16 */
+#define acqResultsFIFO			0x10		/* u16 */
+#define acqResultsShadow		0x14		/* u16 */
+#define acqAdcResult			0x18		/* u16 */
+#define dacScanCounter			0x1c		/* u16 */
+#define dacControl			0x20		/* u16 */
+#define dacFIFO				0x24		/* s16 */
+#define dacPacerClockDiv		0x2a		/* u16 */
+#define refDacs				0x2c		/* u16 */
+#define dioControl			0x30		/* u16 */
+#define dioP3hsioData			0x32		/* s16 */
+#define dioP3Control			0x34		/* u16 */
+#define calEepromControl		0x36		/* u16 */
+#define dacSetting(x)			(0x38 + (x)*2)	/* s16 */
+#define dioP2ExpansionIO8Bit		0x40		/* s16 */
+#define ctrTmrControl			0x80		/* u16 */
+#define ctrInput(x)			(0x88 + (x)*2)	/* s16 */
+#define timerDivisor(x)			(0xa0 + (x)*2)	/* u16 */
+#define dmaControl			0xb0		/* u16 */
+#define trigControl			0xb2		/* u16 */
+#define calEeprom			0xb8		/* u16 */
+#define acqDigitalMark			0xba		/* u16 */
+#define trigDacs			0xbc		/* u16 */
+#define dioP2ExpansionIO16Bit(x)	(0xc0 + (x)*2)	/* s16 */
 
 /* Scan Sequencer programming */
 #define DAQBOARD2000_SeqStartScanList            0x0011
@@ -311,27 +276,23 @@ static const struct daq200_boardtype boardtypes[] = {
 	{"ids4", DAQBOARD2000_SUBSYSTEM_IDS4},
 };
 
-#define this_board ((const struct daq200_boardtype *)dev->board_ptr)
-
 struct daqboard2000_private {
 	enum {
 		card_daqboard_2000
 	} card;
-	void *daq;
+	void __iomem *daq;
 	void __iomem *plx;
 	unsigned int ao_readback[2];
 };
 
-#define devpriv ((struct daqboard2000_private *)dev->private)
-
 static void writeAcqScanListEntry(struct comedi_device *dev, u16 entry)
 {
-	struct daqboard2000_hw *fpga = devpriv->daq;
+	struct daqboard2000_private *devpriv = dev->private;
 
-/* udelay(4); */
-	fpga->acqScanListFIFO = entry & 0x00ff;
-/* udelay(4); */
-	fpga->acqScanListFIFO = (entry >> 8) & 0x00ff;
+	/* udelay(4); */
+	writew(entry & 0x00ff, devpriv->daq + acqScanListFIFO);
+	/* udelay(4); */
+	writew((entry >> 8) & 0x00ff, devpriv->daq + acqScanListFIFO);
 }
 
 static void setup_sampling(struct comedi_device *dev, int chan, int gain)
@@ -372,7 +333,6 @@ static void setup_sampling(struct comedi_device *dev, int chan, int gain)
 	/* These should be read from EEPROM */
 	word2 |= 0x0800;
 	word3 |= 0xc000;
-/*  printk("%d %4.4x %4.4x %4.4x %4.4x\n", chan, word0, word1, word2, word3);*/
 	writeAcqScanListEntry(dev, word0);
 	writeAcqScanListEntry(dev, word1);
 	writeAcqScanListEntry(dev, word2);
@@ -384,21 +344,22 @@ static int daqboard2000_ai_insn_read(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	int i;
-	struct daqboard2000_hw *fpga = devpriv->daq;
+	struct daqboard2000_private *devpriv = dev->private;
+	unsigned int val;
 	int gain, chan, timeout;
+	int i;
 
-	fpga->acqControl =
-	    DAQBOARD2000_AcqResetScanListFifo |
-	    DAQBOARD2000_AcqResetResultsFifo | DAQBOARD2000_AcqResetConfigPipe;
+	writew(DAQBOARD2000_AcqResetScanListFifo |
+	       DAQBOARD2000_AcqResetResultsFifo |
+	       DAQBOARD2000_AcqResetConfigPipe, devpriv->daq + acqControl);
 
 	/*
 	 * If pacer clock is not set to some high value (> 10 us), we
 	 * risk multiple samples to be put into the result FIFO.
 	 */
 	/* 1 second, should be long enough */
-	fpga->acqPacerClockDivLow = 1000000;
-	fpga->acqPacerClockDivHigh = 0;
+	writel(1000000, devpriv->daq + acqPacerClockDivLow);
+	writew(0, devpriv->daq + acqPacerClockDivHigh);
 
 	gain = CR_RANGE(insn->chanspec);
 	chan = CR_CHAN(insn->chanspec);
@@ -410,28 +371,30 @@ static int daqboard2000_ai_insn_read(struct comedi_device *dev,
 	for (i = 0; i < insn->n; i++) {
 		setup_sampling(dev, chan, gain);
 		/* Enable reading from the scanlist FIFO */
-		fpga->acqControl = DAQBOARD2000_SeqStartScanList;
+		writew(DAQBOARD2000_SeqStartScanList,
+		       devpriv->daq + acqControl);
 		for (timeout = 0; timeout < 20; timeout++) {
-			if (fpga->acqControl & DAQBOARD2000_AcqConfigPipeFull)
+			val = readw(devpriv->daq + acqControl);
+			if (val & DAQBOARD2000_AcqConfigPipeFull)
 				break;
 			/* udelay(2); */
 		}
-		fpga->acqControl = DAQBOARD2000_AdcPacerEnable;
+		writew(DAQBOARD2000_AdcPacerEnable, devpriv->daq + acqControl);
 		for (timeout = 0; timeout < 20; timeout++) {
-			if (fpga->acqControl & DAQBOARD2000_AcqLogicScanning)
+			val = readw(devpriv->daq + acqControl);
+			if (val & DAQBOARD2000_AcqLogicScanning)
 				break;
 			/* udelay(2); */
 		}
 		for (timeout = 0; timeout < 20; timeout++) {
-			if (fpga->acqControl &
-			    DAQBOARD2000_AcqResultsFIFOHasValidData) {
+			val = readw(devpriv->daq + acqControl);
+			if (val & DAQBOARD2000_AcqResultsFIFOHasValidData)
 				break;
-			}
 			/* udelay(2); */
 		}
-		data[i] = fpga->acqResultsFIFO;
-		fpga->acqControl = DAQBOARD2000_AdcPacerDisable;
-		fpga->acqControl = DAQBOARD2000_SeqStopScanList;
+		data[i] = readw(devpriv->daq + acqResultsFIFO);
+		writew(DAQBOARD2000_AdcPacerDisable, devpriv->daq + acqControl);
+		writew(DAQBOARD2000_SeqStopScanList, devpriv->daq + acqControl);
 	}
 
 	return i;
@@ -442,8 +405,9 @@ static int daqboard2000_ao_insn_read(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	int i;
+	struct daqboard2000_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
+	int i;
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[chan];
@@ -456,28 +420,39 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 				      struct comedi_insn *insn,
 				      unsigned int *data)
 {
-	int i;
+	struct daqboard2000_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
-	struct daqboard2000_hw *fpga = devpriv->daq;
+	unsigned int val;
 	int timeout;
+	int i;
 
 	for (i = 0; i < insn->n; i++) {
+#if 0
 		/*
-		 * OK, since it works OK without enabling the DAC's, let's keep
-		 * it as simple as possible...
+		 * OK, since it works OK without enabling the DAC's,
+		 * let's keep it as simple as possible...
 		 */
-		/* fpga->dacControl = (chan + 2) * 0x0010 | 0x0001; udelay(1000); */
-		fpga->dacSetting[chan] = data[i];
+		writew((chan + 2) * 0x0010 | 0x0001,
+		       devpriv->daq + dacControl);
+		udelay(1000);
+#endif
+		writew(data[i], devpriv->daq + dacSetting(chan));
 		for (timeout = 0; timeout < 20; timeout++) {
-			if ((fpga->dacControl & ((chan + 1) * 0x0010)) == 0)
+			val = readw(devpriv->daq + dacControl);
+			if ((val & ((chan + 1) * 0x0010)) == 0)
 				break;
 			/* udelay(2); */
 		}
 		devpriv->ao_readback[chan] = data[i];
+#if 0
 		/*
-		 * Since we never enabled the DAC's, we don't need to disable it...
-		 * fpga->dacControl = (chan + 2) * 0x0010 | 0x0000; udelay(1000);
+		 * Since we never enabled the DAC's, we don't need
+		 * to disable it...
 		 */
+		writew((chan + 2) * 0x0010 | 0x0000,
+		       devpriv->daq + dacControl);
+		udelay(1000);
+#endif
 	}
 
 	return i;
@@ -485,7 +460,8 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 
 static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 {
-	dev_dbg(dev->class_dev, "daqboard2000_resetLocalBus\n");
+	struct daqboard2000_private *devpriv = dev->private;
+
 	writel(DAQBOARD2000_SECRLocalBusHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRLocalBusLo, devpriv->plx + 0x6c);
@@ -494,7 +470,8 @@ static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 
 static void daqboard2000_reloadPLX(struct comedi_device *dev)
 {
-	dev_dbg(dev->class_dev, "daqboard2000_reloadPLX\n");
+	struct daqboard2000_private *devpriv = dev->private;
+
 	writel(DAQBOARD2000_SECRReloadLo, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRReloadHi, devpriv->plx + 0x6c);
@@ -505,7 +482,8 @@ static void daqboard2000_reloadPLX(struct comedi_device *dev)
 
 static void daqboard2000_pulseProgPin(struct comedi_device *dev)
 {
-	dev_dbg(dev->class_dev, "daqboard2000_pulseProgPin 1\n");
+	struct daqboard2000_private *devpriv = dev->private;
+
 	writel(DAQBOARD2000_SECRProgPinHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRProgPinLo, devpriv->plx + 0x6c);
@@ -514,6 +492,7 @@ static void daqboard2000_pulseProgPin(struct comedi_device *dev)
 
 static int daqboard2000_pollCPLD(struct comedi_device *dev, int mask)
 {
+	struct daqboard2000_private *devpriv = dev->private;
 	int result = 0;
 	int i;
 	int cpld;
@@ -533,6 +512,7 @@ static int daqboard2000_pollCPLD(struct comedi_device *dev, int mask)
 
 static int daqboard2000_writeCPLD(struct comedi_device *dev, int data)
 {
+	struct daqboard2000_private *devpriv = dev->private;
 	int result = 0;
 
 	udelay(10);
@@ -545,41 +525,29 @@ static int daqboard2000_writeCPLD(struct comedi_device *dev, int data)
 }
 
 static int initialize_daqboard2000(struct comedi_device *dev,
-				   unsigned char *cpld_array, int len)
+				   const u8 *cpld_array, size_t len)
 {
+	struct daqboard2000_private *devpriv = dev->private;
 	int result = -EIO;
 	/* Read the serial EEPROM control register */
 	int secr;
 	int retry;
-	int i;
+	size_t i;
 
 	/* Check to make sure the serial eeprom is present on the board */
 	secr = readl(devpriv->plx + 0x6c);
-	if (!(secr & DAQBOARD2000_EEPROM_PRESENT)) {
-#ifdef DEBUG_EEPROM
-		dev_dbg(dev->class_dev, "no serial eeprom\n");
-#endif
+	if (!(secr & DAQBOARD2000_EEPROM_PRESENT))
 		return -EIO;
-	}
 
 	for (retry = 0; retry < 3; retry++) {
-#ifdef DEBUG_EEPROM
-		dev_dbg(dev->class_dev, "Programming EEPROM try %x\n", retry);
-#endif
-
 		daqboard2000_resetLocalBus(dev);
 		daqboard2000_reloadPLX(dev);
 		daqboard2000_pulseProgPin(dev);
 		if (daqboard2000_pollCPLD(dev, DAQBOARD2000_CPLD_INIT)) {
 			for (i = 0; i < len; i++) {
-				if (cpld_array[i] == 0xff
-				    && cpld_array[i + 1] == 0x20) {
-#ifdef DEBUG_EEPROM
-					dev_dbg(dev->class_dev,
-						"Preamble found at %d\n", i);
-#endif
+				if (cpld_array[i] == 0xff &&
+				    cpld_array[i + 1] == 0x20)
 					break;
-				}
 			}
 			for (; i < len; i += 2) {
 				int data =
@@ -588,9 +556,6 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 					break;
 			}
 			if (i >= len) {
-#ifdef DEBUG_EEPROM
-				dev_dbg(dev->class_dev, "Programmed\n");
-#endif
 				daqboard2000_resetLocalBus(dev);
 				daqboard2000_reloadPLX(dev);
 				result = 0;
@@ -601,28 +566,45 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 	return result;
 }
 
+static int daqboard2000_upload_firmware(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct firmware *fw;
+	int ret;
+
+	ret = request_firmware(&fw, DAQBOARD2000_FIRMWARE, &pcidev->dev);
+	if (ret)
+		return ret;
+
+	ret = initialize_daqboard2000(dev, fw->data, fw->size);
+	release_firmware(fw);
+
+	return ret;
+}
+
 static void daqboard2000_adcStopDmaTransfer(struct comedi_device *dev)
 {
-/*  printk("Implement: daqboard2000_adcStopDmaTransfer\n");*/
 }
 
 static void daqboard2000_adcDisarm(struct comedi_device *dev)
 {
-	struct daqboard2000_hw *fpga = devpriv->daq;
+	struct daqboard2000_private *devpriv = dev->private;
 
 	/* Disable hardware triggers */
 	udelay(2);
-	fpga->trigControl = DAQBOARD2000_TrigAnalog | DAQBOARD2000_TrigDisable;
+	writew(DAQBOARD2000_TrigAnalog | DAQBOARD2000_TrigDisable,
+	       devpriv->daq + trigControl);
 	udelay(2);
-	fpga->trigControl = DAQBOARD2000_TrigTTL | DAQBOARD2000_TrigDisable;
+	writew(DAQBOARD2000_TrigTTL | DAQBOARD2000_TrigDisable,
+	       devpriv->daq + trigControl);
 
 	/* Stop the scan list FIFO from loading the configuration pipe */
 	udelay(2);
-	fpga->acqControl = DAQBOARD2000_SeqStopScanList;
+	writew(DAQBOARD2000_SeqStopScanList, devpriv->daq + acqControl);
 
 	/* Stop the pacer clock */
 	udelay(2);
-	fpga->acqControl = DAQBOARD2000_AdcPacerDisable;
+	writew(DAQBOARD2000_AdcPacerDisable, devpriv->daq + acqControl);
 
 	/* Stop the input dma (abort channel 1) */
 	daqboard2000_adcStopDmaTransfer(dev);
@@ -630,41 +612,39 @@ static void daqboard2000_adcDisarm(struct comedi_device *dev)
 
 static void daqboard2000_activateReferenceDacs(struct comedi_device *dev)
 {
-	struct daqboard2000_hw *fpga = devpriv->daq;
+	struct daqboard2000_private *devpriv = dev->private;
+	unsigned int val;
 	int timeout;
 
 	/*  Set the + reference dac value in the FPGA */
-	fpga->refDacs = 0x80 | DAQBOARD2000_PosRefDacSelect;
+	writew(0x80 | DAQBOARD2000_PosRefDacSelect, devpriv->daq + refDacs);
 	for (timeout = 0; timeout < 20; timeout++) {
-		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0)
+		val = readw(devpriv->daq + dacControl);
+		if ((val & DAQBOARD2000_RefBusy) == 0)
 			break;
 		udelay(2);
 	}
-/*  printk("DAQBOARD2000_PosRefDacSelect %d\n", timeout);*/
 
 	/*  Set the - reference dac value in the FPGA */
-	fpga->refDacs = 0x80 | DAQBOARD2000_NegRefDacSelect;
+	writew(0x80 | DAQBOARD2000_NegRefDacSelect, devpriv->daq + refDacs);
 	for (timeout = 0; timeout < 20; timeout++) {
-		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0)
+		val = readw(devpriv->daq + dacControl);
+		if ((val & DAQBOARD2000_RefBusy) == 0)
 			break;
 		udelay(2);
 	}
-/*  printk("DAQBOARD2000_NegRefDacSelect %d\n", timeout);*/
 }
 
 static void daqboard2000_initializeCtrs(struct comedi_device *dev)
 {
-/*  printk("Implement: daqboard2000_initializeCtrs\n");*/
 }
 
 static void daqboard2000_initializeTmrs(struct comedi_device *dev)
 {
-/*  printk("Implement: daqboard2000_initializeTmrs\n");*/
 }
 
 static void daqboard2000_dacDisarm(struct comedi_device *dev)
 {
-/*  printk("Implement: daqboard2000_dacDisarm\n");*/
 }
 
 static void daqboard2000_initializeAdc(struct comedi_device *dev)
@@ -680,92 +660,66 @@ static void daqboard2000_initializeDac(struct comedi_device *dev)
 	daqboard2000_dacDisarm(dev);
 }
 
-/*
-The test command, REMOVE!!:
-
-rmmod daqboard2000 ; rmmod comedi; make install ; modprobe daqboard2000; /usr/sbin/comedi_config /dev/comedi0 daqboard/2000 ; tail -40 /var/log/messages
-*/
-
 static int daqboard2000_8255_cb(int dir, int port, int data,
 				unsigned long ioaddr)
 {
-	int result = 0;
+	void __iomem *mmio_base = (void __iomem *)ioaddr;
+
 	if (dir) {
-		writew(data, ((void *)ioaddr) + port * 2);
-		result = 0;
+		writew(data, mmio_base + port * 2);
+		return 0;
 	} else {
-		result = readw(((void *)ioaddr) + port * 2);
+		return readw(mmio_base + port * 2);
 	}
-/*
-  printk("daqboard2000_8255_cb %x %d %d %2.2x -> %2.2x\n",
-        arg, dir, port, data, result);
-*/
-	return result;
 }
 
-static struct pci_dev *daqboard2000_find_pci_dev(struct comedi_device *dev,
-						 struct comedi_devconfig *it)
+static const void *daqboard2000_find_boardinfo(struct comedi_device *dev,
+					       struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
+	const struct daq200_boardtype *board;
 	int i;
 
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_IOTECH ||
-		    pcidev->device != 0x0409 ||
-		    pcidev->subsystem_device != PCI_VENDOR_ID_IOTECH)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
-			if (boardtypes[i].id != pcidev->subsystem_device)
-				continue;
-			dev->board_ptr = boardtypes + i;
-			return pcidev;
-		}
+	if (pcidev->subsystem_device != PCI_VENDOR_ID_IOTECH)
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
+		board = &boardtypes[i];
+		if (pcidev->subsystem_device == board->id)
+			return board;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int daqboard2000_attach(struct comedi_device *dev,
-			       struct comedi_devconfig *it)
+static int daqboard2000_attach_pci(struct comedi_device *dev,
+				   struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev;
+	const struct daq200_boardtype *board;
+	struct daqboard2000_private *devpriv;
 	struct comedi_subdevice *s;
-	resource_size_t pci_base;
-	void *aux_data;
-	unsigned int aux_len;
 	int result;
 
-	result = alloc_private(dev, sizeof(struct daqboard2000_private));
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	board = daqboard2000_find_boardinfo(dev, pcidev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
+	result = alloc_private(dev, sizeof(*devpriv));
 	if (result < 0)
 		return -ENOMEM;
+	devpriv = dev->private;
 
-	pcidev = daqboard2000_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pcidev->dev);
-
-	result = comedi_pci_enable(pcidev, "daqboard2000");
-	if (result < 0) {
-		dev_err(dev->class_dev,
-			"failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
+	result = comedi_pci_enable(pcidev, dev->driver->driver_name);
+	if (result < 0)
+		return result;
 	dev->iobase = 1;	/* the "detach" needs this */
 
-	pci_base = pci_resource_start(pcidev, 0);
-	devpriv->plx = ioremap(pci_base, DAQBOARD2000_PLX_SIZE);
-	pci_base = pci_resource_start(pcidev, 2);
-	devpriv->daq = ioremap(pci_base, DAQBOARD2000_DAQ_SIZE);
+	devpriv->plx = ioremap(pci_resource_start(pcidev, 0),
+			       pci_resource_len(pcidev, 0));
+	devpriv->daq = ioremap(pci_resource_start(pcidev, 2),
+			       pci_resource_len(pcidev, 2));
 	if (!devpriv->plx || !devpriv->daq)
 		return -ENOMEM;
 
@@ -775,36 +729,14 @@ static int daqboard2000_attach(struct comedi_device *dev,
 
 	readl(devpriv->plx + 0x6c);
 
-	/*
-	   u8 interrupt;
-	   Windows code does restore interrupts, but since we don't use them...
-	   pci_read_config_byte(pcidev, PCI_INTERRUPT_LINE, &interrupt);
-	   printk("Interrupt before is: %x\n", interrupt);
-	 */
-
-	aux_data = comedi_aux_data(it->options, 0);
-	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-
-	if (aux_data && aux_len) {
-		result = initialize_daqboard2000(dev, aux_data, aux_len);
-	} else {
-		dev_dbg(dev->class_dev,
-			"no FPGA initialization code, aborting\n");
-		result = -EIO;
-	}
+	result = daqboard2000_upload_firmware(dev);
 	if (result < 0)
-		goto out;
+		return result;
+
 	daqboard2000_initializeAdc(dev);
 	daqboard2000_initializeDac(dev);
-	/*
-	   Windows code does restore interrupts, but since we don't use them...
-	   pci_read_config_byte(pcidev, PCI_INTERRUPT_LINE, &interrupt);
-	   printk("Interrupt after is: %x\n", interrupt);
-	 */
-
-	dev->board_name = this_board->name;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -813,7 +745,7 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	s->insn_read = daqboard2000_ai_insn_read;
 	s->range_table = &range_daqboard2000_ai;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -821,22 +753,27 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	s->maxdata = 0xffff;
 	s->insn_read = daqboard2000_ao_insn_read;
 	s->insn_write = daqboard2000_ao_insn_write;
-	s->range_table = &range_daqboard2000_ao;
+	s->range_table = &range_bipolar10;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	result = subdev_8255_init(dev, s, daqboard2000_8255_cb,
-				  (unsigned long)(devpriv->daq + 0x40));
+			(unsigned long)(devpriv->daq + dioP2ExpansionIO8Bit));
+	if (result)
+		return result;
 
-out:
-	return result;
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
+
+	return 0;
 }
 
 static void daqboard2000_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct daqboard2000_private *devpriv = dev->private;
 
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
+		subdev_8255_cleanup(dev, &dev->subdevices[2]);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
@@ -855,7 +792,7 @@ static void daqboard2000_detach(struct comedi_device *dev)
 static struct comedi_driver daqboard2000_driver = {
 	.driver_name	= "daqboard2000",
 	.module		= THIS_MODULE,
-	.attach		= daqboard2000_attach,
+	.attach_pci	= daqboard2000_attach_pci,
 	.detach		= daqboard2000_detach,
 };
 
@@ -887,3 +824,4 @@ module_comedi_pci_driver(daqboard2000_driver, daqboard2000_pci_driver);
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(DAQBOARD2000_FIRMWARE);
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 67a914a10b55..5fd21fa6c1c7 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -32,8 +32,9 @@
  *   [ComputerBoards] DAS08 (isa-das08), DAS08-PGM (das08-pgm),
  *   DAS08-PGH (das08-pgh), DAS08-PGL (das08-pgl), DAS08-AOH (das08-aoh),
  *   DAS08-AOL (das08-aol), DAS08-AOM (das08-aom), DAS08/JR-AO (das08/jr-ao),
- *   DAS08/JR-16-AO (das08jr-16-ao), PCI-DAS08 (pci-das08 or das08),
+ *   DAS08/JR-16-AO (das08jr-16-ao), PCI-DAS08 (pci-das08),
  *   PC104-DAS08 (pc104-das08), DAS08/JR/16 (das08jr/16)
+ * Updated: Fri, 31 Aug 2012 19:19:06 +0100
  * Status: works
  *
  * This is a rewrite of the das08 and das08jr drivers.
@@ -41,9 +42,8 @@
  * Options (for ISA cards):
  *		[0] - base io address
  *
- * Options (for pci-das08):
- *		[0] - bus  (optional)
- *		[1] = slot (optional)
+ * Manual configuration of PCI cards is not supported; they are
+ * configured automatically.
  *
  * The das08 driver doesn't support asynchronous commands, since
  * the cheap das08 hardware doesn't really support them.  The
@@ -61,9 +61,9 @@
 
 #define DRV_NAME "das08"
 
-#define DO_COMEDI_DRIVER_REGISTER \
-	(IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) || \
-	 IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
+#define DO_ISA IS_ENABLED(CONFIG_COMEDI_DAS08_ISA)
+#define DO_PCI IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+#define DO_COMEDI_DRIVER_REGISTER (DO_ISA || DO_PCI)
 
 #define PCI_VENDOR_ID_COMPUTERBOARDS 0x1307
 #define PCI_DEVICE_ID_PCIDAS08 0x29
@@ -236,6 +236,16 @@ static const int *const das08_gainlists[] = {
 	das08_pgm_gainlist,
 };
 
+static inline bool is_isa_board(const struct das08_board_struct *board)
+{
+	return DO_ISA && board->bustype == isa;
+}
+
+static inline bool is_pci_board(const struct das08_board_struct *board)
+{
+	return DO_PCI && board->bustype == pci;
+}
+
 #define TIMEOUT 100000
 
 static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -342,9 +352,9 @@ static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	return insn->n;
 }
 
-static int __maybe_unused
-das08jr_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
-		 struct comedi_insn *insn, unsigned int *data)
+static int das08jr_di_rbits(struct comedi_device *dev,
+			    struct comedi_subdevice *s,
+			    struct comedi_insn *insn, unsigned int *data)
 {
 	data[0] = 0;
 	data[1] = inb(dev->iobase + DAS08JR_DIO);
@@ -352,9 +362,9 @@ das08jr_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	return insn->n;
 }
 
-static int __maybe_unused
-das08jr_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
-		 struct comedi_insn *insn, unsigned int *data)
+static int das08jr_do_wbits(struct comedi_device *dev,
+			    struct comedi_subdevice *s,
+			    struct comedi_insn *insn, unsigned int *data)
 {
 	struct das08_private_struct *devpriv = dev->private;
 
@@ -369,88 +379,92 @@ das08jr_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	return insn->n;
 }
 
-static int __maybe_unused
-das08jr_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		 struct comedi_insn *insn, unsigned int *data)
+static void das08_ao_set_data(struct comedi_device *dev,
+			      unsigned int chan, unsigned int data)
 {
-	int n;
-	int lsb, msb;
-	int chan;
-
-	lsb = data[0] & 0xff;
-	msb = (data[0] >> 8) & 0xff;
-
-	chan = CR_CHAN(insn->chanspec);
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct das08_private_struct *devpriv = dev->private;
+	unsigned char lsb;
+	unsigned char msb;
 
-	for (n = 0; n < insn->n; n++) {
-#if 0
-		outb(lsb, dev->iobase + devpriv->ao_offset_lsb[chan]);
-		outb(msb, dev->iobase + devpriv->ao_offset_msb[chan]);
-#else
+	lsb = data & 0xff;
+	msb = (data >> 8) & 0xff;
+	if (thisboard->is_jr) {
 		outb(lsb, dev->iobase + DAS08JR_AO_LSB(chan));
 		outb(msb, dev->iobase + DAS08JR_AO_MSB(chan));
-#endif
-
 		/* load DACs */
 		inb(dev->iobase + DAS08JR_DIO);
+	} else {
+		outb(lsb, dev->iobase + DAS08AO_AO_LSB(chan));
+		outb(msb, dev->iobase + DAS08AO_AO_MSB(chan));
+		/* load DACs */
+		inb(dev->iobase + DAS08AO_AO_UPDATE);
 	}
-
-	return n;
+	devpriv->ao_readback[chan] = data;
 }
 
-/*
- *
- * The -aox boards have the DACs at a different offset and use
- * a different method to force an update.
- *
- */
-static int __maybe_unused
-das08ao_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		 struct comedi_insn *insn, unsigned int *data)
+static void das08_ao_initialize(struct comedi_device *dev,
+				struct comedi_subdevice *s)
 {
 	int n;
-	int lsb, msb;
-	int chan;
+	unsigned int data;
+
+	data = s->maxdata / 2;	/* should be about 0 volts */
+	for (n = 0; n < s->n_chan; n++)
+		das08_ao_set_data(dev, n, data);
+}
 
-	lsb = data[0] & 0xff;
-	msb = (data[0] >> 8) & 0xf;
+static int das08_ao_winsn(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  struct comedi_insn *insn, unsigned int *data)
+{
+	unsigned int n;
+	unsigned int chan;
 
 	chan = CR_CHAN(insn->chanspec);
 
-	for (n = 0; n < insn->n; n++) {
-#if 0
-		outb(lsb, dev->iobase + devpriv->ao_offset_lsb[chan]);
-		outb(msb, dev->iobase + devpriv->ao_offset_msb[chan]);
-#else
-		outb(lsb, dev->iobase + DAS08AO_AO_LSB(chan));
-		outb(msb, dev->iobase + DAS08AO_AO_MSB(chan));
-#endif
+	for (n = 0; n < insn->n; n++)
+		das08_ao_set_data(dev, chan, *data);
 
-		/* load DACs */
-		inb(dev->iobase + DAS08AO_AO_UPDATE);
-	}
+	return n;
+}
+
+static int das08_ao_rinsn(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  struct comedi_insn *insn, unsigned int *data)
+{
+	struct das08_private_struct *devpriv = dev->private;
+	unsigned int n;
+	unsigned int chan;
+
+	chan = CR_CHAN(insn->chanspec);
+
+	for (n = 0; n < insn->n; n++)
+		data[n] = devpriv->ao_readback[chan];
 
 	return n;
 }
 
 static void i8254_initialize(struct comedi_device *dev)
 {
-	struct das08_private_struct *devpriv = dev->private;
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	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(devpriv->i8254_iobase, 0, i, mode);
+		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)
 {
-	struct das08_private_struct *devpriv = dev->private;
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
-	data[0] = i8254_read(devpriv->i8254_iobase, 0, chan);
+	data[0] = i8254_read(i8254_iobase, 0, chan);
 	return 1;
 }
 
@@ -458,10 +472,11 @@ static int das08_counter_write(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	struct das08_private_struct *devpriv = dev->private;
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
-	i8254_write(devpriv->i8254_iobase, 0, chan, data[0]);
+	i8254_write(i8254_iobase, 0, chan, data[0]);
 	return 1;
 }
 
@@ -469,18 +484,16 @@ static int das08_counter_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	struct das08_private_struct *devpriv = dev->private;
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	switch (data[0]) {
 	case INSN_CONFIG_SET_COUNTER_MODE:
-		i8254_set_mode(devpriv->i8254_iobase, 0, chan, data[1]);
+		i8254_set_mode(i8254_iobase, 0, chan, data[1]);
 		break;
 	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = i8254_status(devpriv->i8254_iobase, 0, chan);
+		data[1] = i8254_status(i8254_iobase, 0, chan);
 		break;
 	default:
 		return -EINVAL;
@@ -491,18 +504,14 @@ static int das08_counter_config(struct comedi_device *dev,
 
 #if DO_COMEDI_DRIVER_REGISTER
 static const struct das08_board_struct das08_boards[] = {
-#if IS_ENABLED(CONFIG_COMEDI_DAS08_ISA)
+#if DO_ISA
 	{
 		.name = "isa-das08",	/*  cio-das08.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pg_none,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.ao_nbits = 12,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
 		.i8255_offset = 8,
 		.i8254_offset = 4,
@@ -511,13 +520,10 @@ static const struct das08_board_struct das08_boards[] = {
 	{
 		.name = "das08-pgm",	/*  cio-das08pgx.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgm,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
 		.i8255_offset = 0,
 		.i8254_offset = 0x04,
@@ -526,44 +532,33 @@ static const struct das08_board_struct das08_boards[] = {
 	{
 		.name = "das08-pgh",	/*  cio-das08pgx.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgh,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
-		.i8255_offset = 0,
 		.i8254_offset = 0x04,
 		.iosize = 16,		/*  unchecked */
 	},
 	{
 		.name = "das08-pgl",	/*  cio-das08pgx.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgl,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
-		.i8255_offset = 0,
 		.i8254_offset = 0x04,
 		.iosize = 16,		/*  unchecked */
 	},
 	{
 		.name = "das08-aoh",	/*  cio-das08_aox.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgh,
 		.ai_encoding = das08_encode12,
-		.ao = das08ao_ao_winsn,	/*  8 */
 		.ao_nbits = 12,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
 		.i8255_offset = 0x0c,
 		.i8254_offset = 0x04,
@@ -572,14 +567,11 @@ static const struct das08_board_struct das08_boards[] = {
 	{
 		.name = "das08-aol",	/*  cio-das08_aox.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgl,
 		.ai_encoding = das08_encode12,
-		.ao = das08ao_ao_winsn,	/*  8 */
 		.ao_nbits = 12,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
 		.i8255_offset = 0x0c,
 		.i8254_offset = 0x04,
@@ -588,14 +580,11 @@ static const struct das08_board_struct das08_boards[] = {
 	{
 		.name = "das08-aom",	/*  cio-das08_aox.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_pgm,
 		.ai_encoding = das08_encode12,
-		.ao = das08ao_ao_winsn,	/*  8 */
 		.ao_nbits = 12,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
 		.i8255_offset = 0x0c,
 		.i8254_offset = 0x04,
@@ -604,152 +593,68 @@ static const struct das08_board_struct das08_boards[] = {
 	{
 		.name = "das08/jr-ao",	/*  cio-das08-jr-ao.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
+		.is_jr = true,
 		.ai_nbits = 12,
 		.ai_pg = das08_pg_none,
 		.ai_encoding = das08_encode12,
-		.ao = das08jr_ao_winsn,
 		.ao_nbits = 12,
-		.di = das08jr_di_rbits,
-		.do_ = das08jr_do_wbits,
+		.di_nchan = 8,
 		.do_nchan = 8,
-		.i8255_offset = 0,
-		.i8254_offset = 0,
 		.iosize = 16,		/*  unchecked */
 	},
 	{
 		.name = "das08jr-16-ao",	/*  cio-das08jr-16-ao.pdf */
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
+		.is_jr = true,
 		.ai_nbits = 16,
 		.ai_pg = das08_pg_none,
 		.ai_encoding = das08_encode16,
-		.ao = das08jr_ao_winsn,
 		.ao_nbits = 16,
-		.di = das08jr_di_rbits,
-		.do_ = das08jr_do_wbits,
+		.di_nchan = 8,
 		.do_nchan = 8,
-		.i8255_offset = 0,
 		.i8254_offset = 0x04,
 		.iosize = 16,		/*  unchecked */
 	},
 	{
 		.name = "pc104-das08",
-		.bustype = pc104,
-		.ai = das08_ai_rinsn,
+		.bustype = isa,
 		.ai_nbits = 12,
 		.ai_pg = das08_pg_none,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.ao_nbits = 0,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
-		.i8255_offset = 0,
 		.i8254_offset = 4,
 		.iosize = 16,		/*  unchecked */
 	},
-#if 0
-	{
-		.name = "das08/f",
-	},
-	{
-		.name = "das08jr",
-	},
-#endif
 	{
 		.name = "das08jr/16",
 		.bustype = isa,
-		.ai = das08_ai_rinsn,
+		.is_jr = true,
 		.ai_nbits = 16,
 		.ai_pg = das08_pg_none,
 		.ai_encoding = das08_encode16,
-		.ao = NULL,
-		.ao_nbits = 0,
-		.di = das08jr_di_rbits,
-		.do_ = das08jr_do_wbits,
+		.di_nchan = 8,
 		.do_nchan = 8,
-		.i8255_offset = 0,
-		.i8254_offset = 0,
 		.iosize = 16,		/*  unchecked */
 	},
-#if 0
-	{
-		.name = "das48-pga",	/*  cio-das48-pga.pdf */
-	},
-	{
-		.name = "das08-pga-g2",	/*  a KM board */
-	},
-#endif
-#endif /* IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) */
-#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+#endif /* DO_ISA */
+#if DO_PCI
 	{
 		.name = "pci-das08",	/*  pci-das08 */
 		.id = PCI_DEVICE_ID_PCIDAS08,
 		.bustype = pci,
-		.ai = das08_ai_rinsn,
 		.ai_nbits = 12,
 		.ai_pg = das08_bipolar5,
 		.ai_encoding = das08_encode12,
-		.ao = NULL,
-		.ao_nbits = 0,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
+		.di_nchan = 3,
 		.do_nchan = 4,
-		.i8255_offset = 0,
 		.i8254_offset = 4,
 		.iosize = 8,
 	},
-	{ /* wildcard entry matches any supported PCI device */
-		.name = DRV_NAME,
-		.id = PCI_ANY_ID,
-		.bustype = pci,
-	},
-#endif /* IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) */
+#endif /* DO_PCI */
 };
 #endif /* DO_COMEDI_DRIVER_REGISTER */
 
-#if IS_ENABLED(CONFIG_COMEDI_DAS08_CS)
-struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
-	{
-		.name = "pcm-das08",
-		.id = 0x0,		/*  XXX */
-		.bustype = pcmcia,
-		.ai = das08_ai_rinsn,
-		.ai_nbits = 12,
-		.ai_pg = das08_bipolar5,
-		.ai_encoding = das08_pcm_encode12,
-		.ao = NULL,
-		.ao_nbits = 0,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
-		.do_nchan = 3,
-		.i8255_offset = 0,
-		.i8254_offset = 0,
-		.iosize = 16,
-	},
-	/*  duplicate so driver name can be used also */
-	{
-		.name = "das08_cs",
-		.id = 0x0,		/*  XXX */
-		.bustype = pcmcia,
-		.ai = das08_ai_rinsn,
-		.ai_nbits = 12,
-		.ai_pg = das08_bipolar5,
-		.ai_encoding = das08_pcm_encode12,
-		.ao = NULL,
-		.ao_nbits = 0,
-		.di = das08_di_rbits,
-		.do_ = das08_do_wbits,
-		.do_nchan = 3,
-		.i8255_offset = 0,
-		.i8254_offset = 0,
-		.iosize = 16,
-	},
-};
-EXPORT_SYMBOL_GPL(das08_cs_boards);
-#endif
-
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
 	const struct das08_board_struct *thisboard = comedi_board(dev);
@@ -765,9 +670,9 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ai */
-	if (thisboard->ai) {
+	if (thisboard->ai_nbits) {
 		s->type = COMEDI_SUBD_AI;
 		/* XXX some boards actually have differential
 		 * inputs instead of single ended.
@@ -778,53 +683,56 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->n_chan = 8;
 		s->maxdata = (1 << thisboard->ai_nbits) - 1;
 		s->range_table = das08_ai_lranges[thisboard->ai_pg];
-		s->insn_read = thisboard->ai;
+		s->insn_read = das08_ai_rinsn;
 		devpriv->pg_gainlist = das08_gainlists[thisboard->ai_pg];
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* ao */
-	if (thisboard->ao) {
+	if (thisboard->ao_nbits) {
 		s->type = COMEDI_SUBD_AO;
-/* XXX lacks read-back insn */
 		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = 2;
 		s->maxdata = (1 << thisboard->ao_nbits) - 1;
 		s->range_table = &range_bipolar5;
-		s->insn_write = thisboard->ao;
+		s->insn_write = das08_ao_winsn;
+		s->insn_read = das08_ao_rinsn;
+		das08_ao_initialize(dev, s);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* di */
-	if (thisboard->di) {
+	if (thisboard->di_nchan) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
-		s->n_chan = (thisboard->di == das08_di_rbits) ? 3 : 8;
+		s->n_chan = thisboard->di_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits = thisboard->di;
+		s->insn_bits =
+			thisboard->is_jr ? das08jr_di_rbits : das08_di_rbits;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* do */
-	if (thisboard->do_) {
+	if (thisboard->do_nchan) {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 		s->n_chan = thisboard->do_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits = thisboard->do_;
+		s->insn_bits =
+			thisboard->is_jr ? das08jr_do_wbits : das08_do_wbits;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	/* 8255 */
 	if (thisboard->i8255_offset != 0) {
 		subdev_8255_init(dev, s, NULL, (unsigned long)(dev->iobase +
@@ -834,7 +742,7 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 5;
+	s = &dev->subdevices[5];
 	/* 8254 */
 	if (thisboard->i8254_offset != 0) {
 		s->type = COMEDI_SUBD_COUNTER;
@@ -844,8 +752,6 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->insn_read = das08_counter_read;
 		s->insn_write = das08_counter_write;
 		s->insn_config = das08_counter_config;
-
-		devpriv->i8254_iobase = iobase + thisboard->i8254_offset;
 		i8254_initialize(dev);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -855,50 +761,13 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 }
 EXPORT_SYMBOL_GPL(das08_common_attach);
 
-static int das08_pci_attach_common(struct comedi_device *dev,
-				   struct pci_dev *pdev)
-{
-	unsigned long iobase;
-	unsigned long pci_iobase;
-	struct das08_private_struct *devpriv = dev->private;
-
-	if (!IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
-		return -EINVAL;
-
-	devpriv->pdev = pdev;
-	/*  enable PCI device and reserve I/O spaces */
-	if (comedi_pci_enable(pdev, dev->driver->driver_name)) {
-		dev_err(dev->class_dev,
-			"Error enabling PCI device and requesting regions\n");
-		return -EIO;
-	}
-	/*  read base addresses */
-	pci_iobase = pci_resource_start(pdev, 1);
-	iobase = pci_resource_start(pdev, 2);
-	dev_info(dev->class_dev, "pcibase 0x%lx  iobase 0x%lx\n",
-		 pci_iobase, iobase);
-	devpriv->pci_iobase = pci_iobase;
-#if 0
-	/* We could enable pci-das08's interrupt here to make it possible
-	* to do timed input in this driver, but there is little point since
-	* conversions would have to be started by the interrupt handler
-	* so you might as well use comedi_rt_timer to emulate commands
-	*/
-	/* set source of interrupt trigger to counter2 output */
-	outb(CNTRL_INTR | CNTRL_DIR, pci_iobase + CNTRL);
-	/* Enable local interrupt 1 and pci interrupt */
-	outw(INTR1_ENABLE | PCI_INTR_ENABLE, pci_iobase + INTCSR);
-#endif
-	return das08_common_attach(dev, iobase);
-}
-
 static const struct das08_board_struct *
 das08_find_pci_board(struct pci_dev *pdev)
 {
 #if DO_COMEDI_DRIVER_REGISTER
 	unsigned int i;
 	for (i = 0; i < ARRAY_SIZE(das08_boards); i++)
-		if (das08_boards[i].bustype == pci &&
+		if (is_pci_board(&das08_boards[i]) &&
 		    pdev->device == das08_boards[i].id)
 			return &das08_boards[i];
 #endif
@@ -909,9 +778,10 @@ das08_find_pci_board(struct pci_dev *pdev)
 static int __devinit __maybe_unused
 das08_attach_pci(struct comedi_device *dev, struct pci_dev *pdev)
 {
+	unsigned long iobase;
 	int ret;
 
-	if (!IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
+	if (!DO_PCI)
 		return -EINVAL;
 	ret = alloc_private(dev, sizeof(struct das08_private_struct));
 	if (ret < 0)
@@ -922,65 +792,16 @@ das08_attach_pci(struct comedi_device *dev, struct pci_dev *pdev)
 		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
 		return -EINVAL;
 	}
-	/*
-	 * Need to 'get' the PCI device to match the 'put' in das08_detach().
-	 * TODO: Remove the pci_dev_get() and matching pci_dev_put() once
-	 * support for manual attachment of PCI devices via das08_attach()
-	 * has been removed.
-	 */
-	pci_dev_get(pdev);
-	return das08_pci_attach_common(dev, pdev);
-}
-
-static struct pci_dev *das08_find_pci(struct comedi_device *dev,
-				      int bus, int slot)
-{
-	const struct das08_board_struct *thisboard = comedi_board(dev);
-	struct pci_dev *pdev;
-	unsigned int matchid;
-
-	if (bus || slot)
-		dev_dbg(dev->class_dev, "Looking for %s at PCI %02X:%02X\n",
-			thisboard->name, bus, slot);
-	else
-		dev_dbg(dev->class_dev, "Looking for %s on PCI buses\n",
-			thisboard->name);
-
-	matchid = thisboard->id;
-	pdev = NULL;
-	for_each_pci_dev(pdev) {
-		if ((bus || slot) &&
-		    (bus != pdev->bus->number || slot != PCI_SLOT(pdev->devfn)))
-			continue;
-		if (pdev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
-			continue;
-		if (matchid == PCI_ANY_ID) {
-			/* wildcard board matches any supported PCI board */
-			const struct das08_board_struct *foundboard;
-			foundboard = das08_find_pci_board(pdev);
-			if (foundboard == NULL)
-				continue;
-			/* replace wildcard board_ptr */
-			dev->board_ptr = thisboard = foundboard;
-		} else {
-			/* match specific PCI board */
-			if (pdev->device != matchid)
-				continue;
-		}
-		/* found a match */
-		dev_info(dev->class_dev, "Found %s at PCI %s\n",
-			 thisboard->name, pci_name(pdev));
-		return pdev;
-	}
-	/* no match found */
-	if (bus || slot)
+	comedi_set_hw_dev(dev, &pdev->dev);
+	/*  enable PCI device and reserve I/O spaces */
+	if (comedi_pci_enable(pdev, dev->driver->driver_name)) {
 		dev_err(dev->class_dev,
-			"No %s cards found at PCI %02X:%02X\n",
-			thisboard->name, bus, slot);
-	else
-		dev_err(dev->class_dev, "No %s cards found on PCI buses\n",
-			thisboard->name);
-	return NULL;
+			"Error enabling PCI device and requesting regions\n");
+		return -EIO;
+	}
+	/*  read base addresses */
+	iobase = pci_resource_start(pdev, 2);
+	return das08_common_attach(dev, iobase);
 }
 
 static int __maybe_unused
@@ -997,14 +818,12 @@ das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv = dev->private;
 
 	dev_info(dev->class_dev, "attach\n");
-	if (IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) && thisboard->bustype == pci) {
-		struct pci_dev *pdev;
-		pdev = das08_find_pci(dev, it->options[0], it->options[1]);
-		if (pdev == NULL)
-			return -EIO;
-		return das08_pci_attach_common(dev, pdev);
-	} else if (IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) &&
-		   (thisboard->bustype == isa || thisboard->bustype == pc104)) {
+	if (is_pci_board(thisboard)) {
+		dev_err(dev->class_dev,
+			"Manual configuration of PCI board '%s' is not supported\n",
+			thisboard->name);
+		return -EIO;
+	} else if (is_isa_board(thisboard)) {
 		iobase = it->options[0];
 		dev_info(dev->class_dev, "iobase 0x%lx\n", iobase);
 		if (!request_region(iobase, thisboard->iosize, DRV_NAME)) {
@@ -1019,26 +838,23 @@ das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 void das08_common_detach(struct comedi_device *dev)
 {
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 4);
+		subdev_8255_cleanup(dev, &dev->subdevices[4]);
 }
 EXPORT_SYMBOL_GPL(das08_common_detach);
 
 static void __maybe_unused das08_detach(struct comedi_device *dev)
 {
 	const struct das08_board_struct *thisboard = comedi_board(dev);
-	struct das08_private_struct *devpriv = dev->private;
 
 	das08_common_detach(dev);
-	if (IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) &&
-	    (thisboard->bustype == isa || thisboard->bustype == pc104)) {
+	if (is_isa_board(thisboard)) {
 		if (dev->iobase)
 			release_region(dev->iobase, thisboard->iosize);
-	} else if (IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) &&
-		   thisboard->bustype == pci) {
-		if (devpriv && devpriv->pdev) {
-			if (devpriv->pci_iobase)
-				comedi_pci_disable(devpriv->pdev);
-			pci_dev_put(devpriv->pdev);
+	} else if (is_pci_board(thisboard)) {
+		struct pci_dev *pdev = comedi_to_pci_dev(dev);
+		if (pdev) {
+			if (dev->iobase)
+				comedi_pci_disable(pdev);
 		}
 	}
 }
@@ -1056,7 +872,7 @@ static struct comedi_driver das08_driver = {
 };
 #endif
 
-#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+#if DO_PCI
 static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08) },
 	{0}
@@ -1081,10 +897,10 @@ static struct pci_driver das08_pci_driver = {
 	.probe = &das08_pci_probe,
 	.remove = __devexit_p(&das08_pci_remove)
 };
-#endif /* CONFIG_COMEDI_DAS08_PCI */
+#endif /* DO_PCI */
 
 #if DO_COMEDI_DRIVER_REGISTER
-#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+#if DO_PCI
 module_comedi_pci_driver(das08_driver, das08_pci_driver);
 #else
 module_comedi_driver(das08_driver);
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 27b6d4ec9032..0314baebae39 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -24,7 +24,7 @@
 #ifndef _DAS08_H
 #define _DAS08_H
 
-enum das08_bustype { isa, pci, pcmcia, pc104 };
+enum das08_bustype { isa, pci, pcmcia };
 /* different ways ai data is encoded in first two registers */
 enum das08_ai_encoding { das08_encode12, das08_encode16, das08_pcm_encode12 };
 enum das08_lrange { das08_pg_none, das08_bipolar5, das08_pgh, das08_pgl,
@@ -35,14 +35,12 @@ struct das08_board_struct {
 	const char *name;
 	unsigned int id;	/*  id for pci/pcmcia boards */
 	enum das08_bustype bustype;
-	void *ai;
+	bool is_jr;		/* true for 'JR' boards */
 	unsigned int ai_nbits;
 	enum das08_lrange ai_pg;
 	enum das08_ai_encoding ai_encoding;
-	void *ao;
 	unsigned int ao_nbits;
-	void *di;
-	void *do_;
+	unsigned int di_nchan;
 	unsigned int do_nchan;
 	unsigned int i8255_offset;
 	unsigned int i8254_offset;
@@ -53,14 +51,9 @@ struct das08_private_struct {
 	unsigned int do_mux_bits;	/*  bits for do/mux register on boards without separate do register */
 	unsigned int do_bits;	/*  bits for do register on boards with register dedicated to digital out only */
 	const unsigned int *pg_gainlist;
-	struct pci_dev *pdev;	/*  struct for pci-das08 */
-	unsigned int pci_iobase;	/*  additional base address for pci-das08 */
-	unsigned int i8254_iobase;
+	unsigned int ao_readback[2];	/* assume 2 AO channels */
 };
 
-#define NUM_DAS08_CS_BOARDS 2
-extern struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS];
-
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase);
 void das08_common_detach(struct comedi_device *dev);
 
diff --git a/drivers/staging/comedi/drivers/das08_cs.c b/drivers/staging/comedi/drivers/das08_cs.c
index f5700de7b6c0..e4c91e675379 100644
--- a/drivers/staging/comedi/drivers/das08_cs.c
+++ b/drivers/staging/comedi/drivers/das08_cs.c
@@ -58,6 +58,32 @@ Command support does not exist, but could be added for this board.
 #include <pcmcia/cistpl.h>
 #include <pcmcia/ds.h>
 
+static const struct das08_board_struct das08_cs_boards[] = {
+	{
+		.name = "pcm-das08",
+		.id = 0x0,		/*  XXX */
+		.bustype = pcmcia,
+		.ai_nbits = 12,
+		.ai_pg = das08_bipolar5,
+		.ai_encoding = das08_pcm_encode12,
+		.di_nchan = 3,
+		.do_nchan = 3,
+		.iosize = 16,
+	},
+	/*  duplicate so driver name can be used also */
+	{
+		.name = "das08_cs",
+		.id = 0x0,		/*  XXX */
+		.bustype = pcmcia,
+		.ai_nbits = 12,
+		.ai_pg = das08_bipolar5,
+		.ai_encoding = das08_pcm_encode12,
+		.di_nchan = 3,
+		.do_nchan = 3,
+		.iosize = 16,
+	},
+};
+
 static struct pcmcia_device *cur_dev;
 
 static int das08_cs_attach(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 895cc7783c9c..fcb8a32adb2f 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -402,62 +402,42 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	int gain, start_chan, i;
 	int mask;
 
-	/* make sure triggers are valid */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
 
-	tmp = cmd->scan_begin_src;
 	mask = TRIG_FOLLOW;
 	/*  if board supports burst mode */
 	if (board->size > 0x400)
 		mask |= TRIG_TIMER | TRIG_EXT;
-	cmd->scan_begin_src &= mask;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, mask);
 
 	tmp = cmd->convert_src;
 	mask = TRIG_TIMER | TRIG_EXT;
 	/*  if board supports burst mode */
 	if (board->size > 0x400)
 		mask |= TRIG_NOW;
-	cmd->convert_src &= mask;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->convert_src, mask);
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/**
-	 * step 2: make sure trigger sources are unique and
-	 * mutually compatible
-	 */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	/*  make sure scan_begin_src and convert_src dont conflict */
 	if (cmd->scan_begin_src == TRIG_FOLLOW && cmd->convert_src == TRIG_NOW)
-		err++;
+		err |= -EINVAL;
 	if (cmd->scan_begin_src != TRIG_FOLLOW && cmd->convert_src != TRIG_NOW)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -558,7 +538,7 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 
 /* utility function that suggests a dma transfer size in bytes */
 static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
-						struct comedi_cmd cmd)
+						const struct comedi_cmd *cmd)
 {
 	unsigned int size;
 	unsigned int freq;
@@ -571,16 +551,16 @@ static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
 
 	/* otherwise, we are relying on dma terminal count interrupt,
 	 * so pick a reasonable size */
-	if (cmd.convert_src == TRIG_TIMER)
-		freq = 1000000000 / cmd.convert_arg;
-	else if (cmd.scan_begin_src == TRIG_TIMER)
-		freq = (1000000000 / cmd.scan_begin_arg) * cmd.chanlist_len;
+	if (cmd->convert_src == TRIG_TIMER)
+		freq = 1000000000 / cmd->convert_arg;
+	else if (cmd->scan_begin_src == TRIG_TIMER)
+		freq = (1000000000 / cmd->scan_begin_arg) * cmd->chanlist_len;
 	/*  return some default value */
 	else
 		freq = 0xffffffff;
 
-	if (cmd.flags & TRIG_WAKE_EOS) {
-		size = sample_size * cmd.chanlist_len;
+	if (cmd->flags & TRIG_WAKE_EOS) {
+		size = sample_size * cmd->chanlist_len;
 	} else {
 		/*  make buffer fill in no more than 1/3 second */
 		size = (freq / 3) * sample_size;
@@ -592,7 +572,7 @@ static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
 	else if (size < sample_size)
 		size = sample_size;
 
-	if (cmd.stop_src == TRIG_COUNT && size > devpriv->adc_byte_count)
+	if (cmd->stop_src == TRIG_COUNT && size > devpriv->adc_byte_count)
 		size = devpriv->adc_byte_count;
 
 	return size;
@@ -685,7 +665,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	set_dma_addr(devpriv->dma_chan,
 		     devpriv->dma_buffer_addr[devpriv->current_buffer]);
 	/*  set appropriate size of transfer */
-	devpriv->dma_transfer_size = das16_suggest_transfer_size(dev, *cmd);
+	devpriv->dma_transfer_size = das16_suggest_transfer_size(dev, cmd);
 	set_dma_count(devpriv->dma_chan, devpriv->dma_transfer_size);
 	enable_dma(devpriv->dma_chan);
 	release_dma_lock(flags);
@@ -1268,7 +1248,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* ai */
 	if (board->ai) {
@@ -1300,7 +1280,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* ao */
 	if (board->ao) {
 		s->type = COMEDI_SUBD_AO;
@@ -1318,7 +1298,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* di */
 	if (board->di) {
 		s->type = COMEDI_SUBD_DI;
@@ -1331,7 +1311,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* do */
 	if (board->do_) {
 		s->type = COMEDI_SUBD_DO;
@@ -1346,7 +1326,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	/* 8255 */
 	if (board->i8255_offset != 0) {
 		subdev_8255_init(dev, s, NULL, (dev->iobase +
@@ -1376,7 +1356,7 @@ static void das16_detach(struct comedi_device *dev)
 
 	das16_reset(dev);
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 4);
+		subdev_8255_cleanup(dev, &dev->subdevices[4]);
 	if (devpriv) {
 		int i;
 		for (i = 0; i < 2; i++) {
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 200926347861..3f87d7598e5b 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -170,42 +170,24 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	const struct das16m1_board *board = comedi_board(dev);
 	unsigned int err = 0, tmp, i;
 
-	/* make sure triggers are valid */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -650,7 +632,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* ai */
 	s->type = COMEDI_SUBD_AI;
@@ -666,7 +648,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	s->cancel = das16m1_cancel;
 	s->poll = das16m1_poll;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* di */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -675,7 +657,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = das16m1_di_rbits;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* do */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
@@ -684,7 +666,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->insn_bits = das16m1_do_wbits;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* 8255 */
 	subdev_8255_init(dev, s, NULL, dev->iobase + DAS16M1_82C55);
 
@@ -707,7 +689,7 @@ static int das16m1_attach(struct comedi_device *dev,
 static void das16m1_detach(struct comedi_device *dev)
 {
 	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 3);
+		subdev_8255_cleanup(dev, &dev->subdevices[3]);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase) {
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 25e7e56a376f..2555f3297d7b 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -656,7 +656,7 @@ static int das1800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 /* the guts of the interrupt handler, that is shared with das1800_ai_poll */
 static void das1800_ai_handler(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + 0;	/* analog input subdevice */
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int status = inb(dev->iobase + DAS1800_STATUS);
@@ -784,59 +784,35 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
 	unsigned int tmp_arg;
 	int i;
 	int unipolar;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_EXT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_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,
+					TRIG_COUNT | TRIG_EXT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
-	/*  uniqueness check */
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err++;
-	/* compatibility check */
 	if (cmd->scan_begin_src != TRIG_FOLLOW &&
 	    cmd->convert_src != TRIG_TIMER)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -958,14 +934,14 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 }
 
 /* returns appropriate bits for control register a, depending on command */
-static int control_a_bits(struct comedi_cmd cmd)
+static int control_a_bits(const struct comedi_cmd *cmd)
 {
 	int control_a;
 
 	control_a = FFEN;	/* enable fifo */
-	if (cmd.stop_src == TRIG_EXT)
+	if (cmd->stop_src == TRIG_EXT)
 		control_a |= ATEN;
-	switch (cmd.start_src) {
+	switch (cmd->start_src) {
 	case TRIG_EXT:
 		control_a |= TGEN | CGSL;
 		break;
@@ -980,7 +956,7 @@ static int control_a_bits(struct comedi_cmd cmd)
 }
 
 /* returns appropriate bits for control register c, depending on command */
-static int control_c_bits(struct comedi_cmd cmd)
+static int control_c_bits(const struct comedi_cmd *cmd)
 {
 	int control_c;
 	int aref;
@@ -988,18 +964,18 @@ static int control_c_bits(struct comedi_cmd cmd)
 	/* set clock source to internal or external, select analog reference,
 	 * select unipolar / bipolar
 	 */
-	aref = CR_AREF(cmd.chanlist[0]);
+	aref = CR_AREF(cmd->chanlist[0]);
 	control_c = UQEN;	/* enable upper qram addresses */
 	if (aref != AREF_DIFF)
 		control_c |= SD;
 	if (aref == AREF_COMMON)
 		control_c |= CMEN;
 	/* if a unipolar range was selected */
-	if (CR_RANGE(cmd.chanlist[0]) & UNIPOLAR)
+	if (CR_RANGE(cmd->chanlist[0]) & UNIPOLAR)
 		control_c |= UB;
-	switch (cmd.scan_begin_src) {
+	switch (cmd->scan_begin_src) {
 	case TRIG_FOLLOW:	/*  not in burst mode */
-		switch (cmd.convert_src) {
+		switch (cmd->convert_src) {
 		case TRIG_TIMER:
 			/* trig on cascaded counters */
 			control_c |= IPCLK;
@@ -1047,29 +1023,33 @@ static int das1800_set_frequency(struct comedi_device *dev)
 }
 
 /* sets up counters */
-static int setup_counters(struct comedi_device *dev, struct comedi_cmd cmd)
+static int setup_counters(struct comedi_device *dev,
+			  const struct comedi_cmd *cmd)
 {
+	unsigned int period;
+
 	/*  setup cascaded counters for conversion/scan frequency */
-	switch (cmd.scan_begin_src) {
+	switch (cmd->scan_begin_src) {
 	case TRIG_FOLLOW:	/*  not in burst mode */
-		if (cmd.convert_src == TRIG_TIMER) {
+		if (cmd->convert_src == TRIG_TIMER) {
 			/* set conversion frequency */
+			period = cmd->convert_arg;
 			i8253_cascade_ns_to_timer_2div(TIMER_BASE,
-						       &(devpriv->divisor1),
-						       &(devpriv->divisor2),
-						       &(cmd.convert_arg),
-						       cmd.
-						       flags & TRIG_ROUND_MASK);
+						       &devpriv->divisor1,
+						       &devpriv->divisor2,
+						       &period,
+						       cmd->flags &
+							TRIG_ROUND_MASK);
 			if (das1800_set_frequency(dev) < 0)
 				return -1;
 		}
 		break;
 	case TRIG_TIMER:	/*  in burst mode */
 		/* set scan frequency */
-		i8253_cascade_ns_to_timer_2div(TIMER_BASE, &(devpriv->divisor1),
-					       &(devpriv->divisor2),
-					       &(cmd.scan_begin_arg),
-					       cmd.flags & TRIG_ROUND_MASK);
+		period = cmd->scan_begin_arg;
+		i8253_cascade_ns_to_timer_2div(TIMER_BASE, &devpriv->divisor1,
+					       &devpriv->divisor2, &period,
+					       cmd->flags & TRIG_ROUND_MASK);
 		if (das1800_set_frequency(dev) < 0)
 			return -1;
 		break;
@@ -1078,7 +1058,7 @@ static int setup_counters(struct comedi_device *dev, struct comedi_cmd cmd)
 	}
 
 	/*  setup counter 0 for 'about triggering' */
-	if (cmd.stop_src == TRIG_EXT) {
+	if (cmd->stop_src == TRIG_EXT) {
 		/*  load counter 0 in mode 0 */
 		i8254_load(dev->iobase + DAS1800_COUNTER, 0, 0, 1, 0);
 	}
@@ -1087,7 +1067,7 @@ static int setup_counters(struct comedi_device *dev, struct comedi_cmd cmd)
 }
 
 /* utility function that suggests a dma transfer size based on the conversion period 'ns' */
-static unsigned int suggest_transfer_size(struct comedi_cmd *cmd)
+static unsigned int suggest_transfer_size(const struct comedi_cmd *cmd)
 {
 	unsigned int size = DMA_BUF_SIZE;
 	static const int sample_size = 2;	/*  size in bytes of one sample from board */
@@ -1125,7 +1105,7 @@ static unsigned int suggest_transfer_size(struct comedi_cmd *cmd)
 }
 
 /* sets up dma */
-static void setup_dma(struct comedi_device *dev, struct comedi_cmd cmd)
+static void setup_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 {
 	unsigned long lock_flags;
 	const int dual_dma = devpriv->irq_dma_bits & DMA_DUAL;
@@ -1134,7 +1114,7 @@ static void setup_dma(struct comedi_device *dev, struct comedi_cmd cmd)
 		return;
 
 	/* determine a reasonable dma transfer size */
-	devpriv->dma_transfer_size = suggest_transfer_size(&cmd);
+	devpriv->dma_transfer_size = suggest_transfer_size(cmd);
 	lock_flags = claim_dma_lock();
 	disable_dma(devpriv->dma0);
 	/* clear flip-flop to make sure 2-byte registers for
@@ -1163,14 +1143,15 @@ static void setup_dma(struct comedi_device *dev, struct comedi_cmd cmd)
 }
 
 /* programs channel/gain list into card */
-static void program_chanlist(struct comedi_device *dev, struct comedi_cmd cmd)
+static void program_chanlist(struct comedi_device *dev,
+			     const struct comedi_cmd *cmd)
 {
 	int i, n, chan_range;
 	unsigned long irq_flags;
 	const int range_mask = 0x3;	/* masks unipolar/bipolar bit off range */
 	const int range_bitshift = 8;
 
-	n = cmd.chanlist_len;
+	n = cmd->chanlist_len;
 	/*  spinlock protects indirect addressing */
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	outb(QRAM, dev->iobase + DAS1800_SELECT);	/* select QRAM for baseAddress + 0x0 */
@@ -1178,9 +1159,9 @@ static void program_chanlist(struct comedi_device *dev, struct comedi_cmd cmd)
 	/* make channel / gain list */
 	for (i = 0; i < n; i++) {
 		chan_range =
-		    CR_CHAN(cmd.
-			    chanlist[i]) | ((CR_RANGE(cmd.chanlist[i]) &
-					     range_mask) << range_bitshift);
+		    CR_CHAN(cmd->chanlist[i]) |
+		    ((CR_RANGE(cmd->chanlist[i]) & range_mask) <<
+		     range_bitshift);
 		outw(chan_range, dev->iobase + DAS1800_QRAM);
 	}
 	outb(n - 1, dev->iobase + DAS1800_QRAM_ADDRESS);	/*finish write to QRAM */
@@ -1196,7 +1177,7 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 	int ret;
 	int control_a, control_c;
 	struct comedi_async *async = s->async;
-	struct comedi_cmd cmd = async->cmd;
+	const struct comedi_cmd *cmd = &async->cmd;
 
 	if (!dev->irq) {
 		comedi_error(dev,
@@ -1206,12 +1187,12 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 
 	/* disable dma on TRIG_WAKE_EOS, or TRIG_RT
 	 * (because dma in handler is unsafe at hard real-time priority) */
-	if (cmd.flags & (TRIG_WAKE_EOS | TRIG_RT))
+	if (cmd->flags & (TRIG_WAKE_EOS | TRIG_RT))
 		devpriv->irq_dma_bits &= ~DMA_ENABLED;
 	else
 		devpriv->irq_dma_bits |= devpriv->dma_bits;
 	/*  interrupt on end of conversion for TRIG_WAKE_EOS */
-	if (cmd.flags & TRIG_WAKE_EOS) {
+	if (cmd->flags & TRIG_WAKE_EOS) {
 		/*  interrupt fifo not empty */
 		devpriv->irq_dma_bits &= ~FIMD;
 	} else {
@@ -1219,8 +1200,8 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 		devpriv->irq_dma_bits |= FIMD;
 	}
 	/*  determine how many conversions we need */
-	if (cmd.stop_src == TRIG_COUNT)
-		devpriv->count = cmd.stop_arg * cmd.chanlist_len;
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
 
 	das1800_cancel(dev, s);
 
@@ -1240,9 +1221,9 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 	/*  set conversion rate and length for burst mode */
 	if (control_c & BMDE) {
 		/*  program conversion period with number of microseconds minus 1 */
-		outb(cmd.convert_arg / 1000 - 1,
+		outb(cmd->convert_arg / 1000 - 1,
 		     dev->iobase + DAS1800_BURST_RATE);
-		outb(cmd.chanlist_len - 1, dev->iobase + DAS1800_BURST_LENGTH);
+		outb(cmd->chanlist_len - 1, dev->iobase + DAS1800_BURST_LENGTH);
 	}
 	outb(devpriv->irq_dma_bits, dev->iobase + DAS1800_CONTROL_B);	/*  enable irq/dma */
 	outb(control_a, dev->iobase + DAS1800_CONTROL_A);	/* enable fifo and triggering */
@@ -1653,7 +1634,7 @@ static int das1800_attach(struct comedi_device *dev,
 		return retval;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND | SDF_CMD_READ;
@@ -1670,7 +1651,7 @@ static int das1800_attach(struct comedi_device *dev,
 	s->cancel = das1800_cancel;
 
 	/* analog out */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (thisboard->ao_ability == 1) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE;
@@ -1683,7 +1664,7 @@ static int das1800_attach(struct comedi_device *dev,
 	}
 
 	/* di */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 4;
@@ -1692,7 +1673,7 @@ static int das1800_attach(struct comedi_device *dev,
 	s->insn_bits = das1800_di_rbits;
 
 	/* do */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 	s->n_chan = thisboard->do_n_chan;
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index e3afcfa9efc8..e134c46dedff 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -152,7 +152,7 @@ static void das6402_setcounter(struct comedi_device *dev)
 static irqreturn_t intr_handler(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 
 	if (!dev->attached || devpriv->das6402_ignoreirq) {
 		dev_warn(dev->class_dev, "BUG: spurious interrupt\n");
@@ -312,7 +312,7 @@ static int das6402_attach(struct comedi_device *dev,
 		return ret;
 
 	/* ai subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = 8;
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index a0959a5e8747..215deac0a396 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -435,7 +435,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 	if (fifo_overflow) {
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 		comedi_error(dev, "DAS800 FIFO overflow");
-		das800_cancel(dev, dev->subdevices + 0);
+		das800_cancel(dev, s);
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 		comedi_event(dev, s);
 		async->events = 0;
@@ -517,7 +517,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
@@ -531,7 +531,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->cancel = das800_cancel;
 
 	/* di */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 3;
@@ -540,7 +540,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_bits = das800_di_rbits;
 
 	/* do */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 	s->n_chan = 4;
@@ -609,44 +609,24 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	int gain, startChan;
 	int i;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 7107f590b1fe..4d5c33c4750f 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -41,6 +41,8 @@ Configuration Options:
 #include "../comedidev.h"
 #include <linux/ioport.h>
 
+#include "comedi_fc.h"
+
 /* Board register addresses */
 
 #define DMM32AT_MEMSIZE 0x10
@@ -258,47 +260,26 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	int tmp;
 	int start_chan, gain, i;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER /*| TRIG_EXT */ ;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER /*| TRIG_EXT */ ;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER /*| TRIG_EXT */);
+	err |= cfc_check_trigger_src(&cmd->convert_src,
+					TRIG_TIMER /*| TRIG_EXT */);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-	 * compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -825,7 +806,7 @@ static int dmm32at_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
@@ -841,7 +822,7 @@ static int dmm32at_attach(struct comedi_device *dev,
 	s->do_cmdtest = dmm32at_ai_cmdtest;
 	s->cancel = dmm32at_ai_cancel;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -851,7 +832,7 @@ static int dmm32at_attach(struct comedi_device *dev,
 	s->insn_write = dmm32at_ao_winsn;
 	s->insn_read = dmm32at_ao_rinsn;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 
 	/* get access to the DIO regs */
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index d332269375ab..c59a652a1194 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -532,7 +532,7 @@ static int dt2801_dio_insn_bits(struct comedi_device *dev,
 {
 	int which = 0;
 
-	if (s == dev->subdevices + 4)
+	if (s == &dev->subdevices[3])
 		which = 1;
 
 	if (data[0]) {
@@ -555,7 +555,7 @@ static int dt2801_dio_insn_config(struct comedi_device *dev,
 {
 	int which = 0;
 
-	if (s == dev->subdevices + 4)
+	if (s == &dev->subdevices[3])
 		which = 1;
 
 	/* configure */
@@ -636,7 +636,7 @@ havetype:
 
 	dev->board_name = boardtype.name;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -652,7 +652,7 @@ havetype:
 	s->range_table = ai_range_lkup(boardtype.adrangetype, it->options[3]);
 	s->insn_read = dt2801_ai_insn_read;
 
-	s++;
+	s = &dev->subdevices[1];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -664,7 +664,7 @@ havetype:
 	s->insn_read = dt2801_ao_insn_read;
 	s->insn_write = dt2801_ao_insn_write;
 
-	s++;
+	s = &dev->subdevices[2];
 	/* 1st digital subdevice */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -674,7 +674,7 @@ havetype:
 	s->insn_bits = dt2801_dio_insn_bits;
 	s->insn_config = dt2801_dio_insn_config;
 
-	s++;
+	s = &dev->subdevices[3];
 	/* 2nd digital subdevice */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index 290b933c5f96..d3a8c1aec9d6 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -510,7 +510,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		break;
 	}
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* initialize the ADC subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -530,7 +530,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		break;
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -542,7 +542,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->range_type_list[0] = dac_range_types[devpriv->dac_range[0]];
 	devpriv->range_type_list[1] = dac_range_types[devpriv->dac_range[1]];
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* di subdevice */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -551,7 +551,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* do subdevice */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 2e39ebe36fb5..064a8f215e4d 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -45,6 +45,8 @@ addition, the clock does not seem to be very accurate.
 #include <linux/ioport.h>
 #include <linux/delay.h>
 
+#include "comedi_fc.h"
+
 #define DT2814_SIZE 2
 
 #define DT2814_CSR 0
@@ -129,42 +131,22 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are
-	 * unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->stop_src != TRIG_TIMER && cmd->stop_src != TRIG_EXT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -247,7 +229,7 @@ static irqreturn_t dt2814_interrupt(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 
 	hi = inb(dev->iobase + DT2814_DATA);
 	lo = inb(dev->iobase + DT2814_DATA);
@@ -346,7 +328,7 @@ static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
diff --git a/drivers/staging/comedi/drivers/dt2815.c b/drivers/staging/comedi/drivers/dt2815.c
index 45b20bee4369..b9692ef64c41 100644
--- a/drivers/staging/comedi/drivers/dt2815.c
+++ b/drivers/staging/comedi/drivers/dt2815.c
@@ -185,7 +185,7 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (alloc_private(dev, sizeof(struct dt2815_private)) < 0)
 		return -ENOMEM;
 
-	s = dev->subdevices;
+	s = &dev->subdevices[0];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/dt2817.c b/drivers/staging/comedi/drivers/dt2817.c
index beba0447b3ee..502e42e0753e 100644
--- a/drivers/staging/comedi/drivers/dt2817.c
+++ b/drivers/staging/comedi/drivers/dt2817.c
@@ -141,7 +141,7 @@ static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 
 	s->n_chan = 32;
 	s->type = COMEDI_SUBD_DIO;
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 1f0b40e4bddd..78d340716d1e 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -312,7 +312,7 @@ static void dt282x_ao_dma_interrupt(struct comedi_device *dev)
 	void *ptr;
 	int size;
 	int i;
-	struct comedi_subdevice *s = dev->subdevices + 1;
+	struct comedi_subdevice *s = &dev->subdevices[1];
 
 	outw(devpriv->supcsr | DT2821_CLRDMADNE, dev->iobase + DT2821_SUPCSR);
 
@@ -345,7 +345,7 @@ static void dt282x_ai_dma_interrupt(struct comedi_device *dev)
 	int size;
 	int i;
 	int ret;
-	struct comedi_subdevice *s = dev->subdevices;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 
 	outw(devpriv->supcsr | DT2821_CLRDMADNE, dev->iobase + DT2821_SUPCSR);
 
@@ -457,8 +457,8 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	s = dev->subdevices + 0;
-	s_ao = dev->subdevices + 1;
+	s = &dev->subdevices[0];
+	s_ao = &dev->subdevices[1];
 	adcsr = inw(dev->iobase + DT2821_ADCSR);
 	dacsr = inw(dev->iobase + DT2821_DACSR);
 	supcsr = inw(dev->iobase + DT2821_SUPCSR);
@@ -582,47 +582,24 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique
-	 * and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -862,44 +839,22 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique
-	 * and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1275,7 +1230,7 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 
 	dev->read_subdev = s;
 	/* ai subdevice */
@@ -1294,7 +1249,7 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	    opt_ai_range_lkup(boardtype.ispgl, it->options[opt_ai_range]);
 	devpriv->ad_2scomp = it->options[opt_ai_twos];
 
-	s++;
+	s = &dev->subdevices[1];
 
 	s->n_chan = boardtype.dachan;
 	if (s->n_chan) {
@@ -1320,7 +1275,7 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s++;
+	s = &dev->subdevices[2];
 	/* dio subsystem */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 3476cda0fff0..43d05ef97157 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -63,6 +63,8 @@ AO commands are not supported.
 #include "../comedidev.h"
 #include <linux/delay.h>
 
+#include "comedi_fc.h"
+
 #define PCI_VENDOR_ID_DT	0x1116
 
 static const struct comedi_lrange range_dt3000_ai = { 4, {
@@ -330,7 +332,7 @@ static irqreturn_t dt3k_interrupt(int irq, void *d)
 	if (!dev->attached)
 		return IRQ_NONE;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	status = readw(devpriv->io_addr + DPR_Intr_Flag);
 #ifdef DEBUG
 	debug_intr_flags(status);
@@ -408,37 +410,19 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -842,7 +826,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 
 	/* ai subdevice */
@@ -857,7 +841,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->do_cmdtest = dt3k_ai_cmdtest;
 	s->cancel = dt3k_ai_cancel;
 
-	s++;
+	s = &dev->subdevices[1];
 	/* ao subsystem */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -868,7 +852,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->len_chanlist = 1;
 	s->range_table = &range_bipolar10;
 
-	s++;
+	s = &dev->subdevices[2];
 	/* dio subsystem */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -879,7 +863,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->len_chanlist = 8;
 	s->range_table = &range_digital;
 
-	s++;
+	s = &dev->subdevices[3];
 	/* mem subsystem */
 	s->type = COMEDI_SUBD_MEMORY;
 	s->subdev_flags = SDF_READABLE;
@@ -890,7 +874,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_unknown;
 
 #if 0
-	s++;
+	s = &dev->subdevices[4];
 	/* proc subsystem */
 	s->type = COMEDI_SUBD_PROC;
 #endif
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index 40821c7303ea..bc6f409b7e19 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -1041,7 +1041,7 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	/* digital input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 0;
@@ -1050,7 +1050,7 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_read = &dt9812_di_rinsn;
 
 	/* digital output subdevice */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITEABLE;
 	s->n_chan = 0;
@@ -1059,7 +1059,7 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = &dt9812_do_winsn;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = 0;
@@ -1068,7 +1068,7 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_read = &dt9812_ai_rinsn;
 
 	/* analog output subdevice */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITEABLE;
 	s->n_chan = 0;
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index 064be9aae3aa..6f612be1b0a5 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -41,7 +41,6 @@
 #include <linux/mutex.h>
 
 #define PCI_VENDOR_ID_DYNALOG		0x10b5
-#define DRV_NAME			"dyna_pci10xx"
 
 #define READ_TIMEOUT 50
 
@@ -54,59 +53,11 @@ static const struct comedi_lrange range_pci1050_ai = { 3, {
 
 static const char range_codes_pci1050_ai[] = { 0x00, 0x10, 0x30 };
 
-static const struct comedi_lrange range_pci1050_ao = { 1, {
-							  UNI_RANGE(10)
-							  }
-};
-
-static const char range_codes_pci1050_ao[] = { 0x00 };
-
-struct boardtype {
-	const char *name;
-	int device_id;
-	int ai_chans;
-	int ai_bits;
-	int ao_chans;
-	int ao_bits;
-	int di_chans;
-	int di_bits;
-	int do_chans;
-	int do_bits;
-	const struct comedi_lrange *range_ai;
-	const char *range_codes_ai;
-	const struct comedi_lrange *range_ao;
-	const char *range_codes_ao;
-};
-
-static const struct boardtype boardtypes[] = {
-	{
-	.name = "dyna_pci1050",
-	.device_id = 0x1050,
-	.ai_chans = 16,
-	.ai_bits = 12,
-	.ao_chans = 16,
-	.ao_bits = 12,
-	.di_chans = 16,
-	.di_bits = 16,
-	.do_chans = 16,
-	.do_bits = 16,
-	.range_ai = &range_pci1050_ai,
-	.range_codes_ai = range_codes_pci1050_ai,
-	.range_ao = &range_pci1050_ao,
-	.range_codes_ao = range_codes_pci1050_ao,
-	},
-	/*  dummy entry corresponding to driver name */
-	{.name = DRV_NAME},
-};
-
 struct dyna_pci10xx_private {
 	struct mutex mutex;
 	unsigned long BADR3;
 };
 
-#define thisboard ((const struct boardtype *)dev->board_ptr)
-#define devpriv ((struct dyna_pci10xx_private *)dev->private)
-
 /******************************************************************************/
 /************************** READ WRITE FUNCTIONS ******************************/
 /******************************************************************************/
@@ -116,13 +67,14 @@ static int dyna_pci10xx_insn_read_ai(struct comedi_device *dev,
 			struct comedi_subdevice *s,
 			struct comedi_insn *insn, unsigned int *data)
 {
+	struct dyna_pci10xx_private *devpriv = dev->private;
 	int n, counter;
 	u16 d = 0;
 	unsigned int chan, range;
 
 	/* get the channel number and range */
 	chan = CR_CHAN(insn->chanspec);
-	range = thisboard->range_codes_ai[CR_RANGE((insn->chanspec))];
+	range = range_codes_pci1050_ai[CR_RANGE((insn->chanspec))];
 
 	mutex_lock(&devpriv->mutex);
 	/* convert n samples */
@@ -159,11 +111,12 @@ static int dyna_pci10xx_insn_write_ao(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct dyna_pci10xx_private *devpriv = dev->private;
 	int n;
 	unsigned int chan, range;
 
 	chan = CR_CHAN(insn->chanspec);
-	range = thisboard->range_codes_ai[CR_RANGE((insn->chanspec))];
+	range = range_codes_pci1050_ai[CR_RANGE((insn->chanspec))];
 
 	mutex_lock(&devpriv->mutex);
 	for (n = 0; n < insn->n; n++) {
@@ -181,6 +134,7 @@ static int dyna_pci10xx_di_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
+	struct dyna_pci10xx_private *devpriv = dev->private;
 	u16 d = 0;
 
 	mutex_lock(&devpriv->mutex);
@@ -200,6 +154,8 @@ static int dyna_pci10xx_do_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
+	struct dyna_pci10xx_private *devpriv = dev->private;
+
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit.
 	 * s->state contains the previous write data
@@ -223,143 +179,98 @@ static int dyna_pci10xx_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static struct pci_dev *dyna_pci10xx_find_pci_dev(struct comedi_device *dev,
-						 struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
-
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (bus != pcidev->bus->number ||
-			    slot != PCI_SLOT(pcidev->devfn))
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_DYNALOG)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-			if (pcidev->device != boardtypes[i].device_id)
-				continue;
-
-			dev->board_ptr = &boardtypes[i];
-			return pcidev;
-		}
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-static int dyna_pci10xx_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int dyna_pci10xx_attach_pci(struct comedi_device *dev,
+				   struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev;
+	struct dyna_pci10xx_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	if (alloc_private(dev, sizeof(struct dyna_pci10xx_private)) < 0) {
-		printk(KERN_ERR "comedi: dyna_pci10xx: "
-			"failed to allocate memory!\n");
-		return -ENOMEM;
-	}
-
-	pcidev = dyna_pci10xx_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	dev->board_name = thisboard->name;
-	dev->irq = 0;
-
-	if (comedi_pci_enable(pcidev, DRV_NAME)) {
-		printk(KERN_ERR "comedi: dyna_pci10xx: "
-			"failed to enable PCI device and request regions!");
-		return -EIO;
-	}
-
-	mutex_init(&devpriv->mutex);
+	dev->board_name = dev->driver->driver_name;
 
-	printk(KERN_INFO "comedi: dyna_pci10xx: device found!\n");
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
 	dev->iobase = pci_resource_start(pcidev, 2);
 	devpriv->BADR3 = pci_resource_start(pcidev, 3);
 
+	mutex_init(&devpriv->mutex);
+
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
 	/* analog input */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan = thisboard->ai_chans;
+	s->n_chan = 16;
 	s->maxdata = 0x0FFF;
-	s->range_table = thisboard->range_ai;
+	s->range_table = &range_pci1050_ai;
 	s->len_chanlist = 16;
 	s->insn_read = dyna_pci10xx_insn_read_ai;
 
 	/* analog output */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
+	s->n_chan = 16;
 	s->maxdata = 0x0FFF;
-	s->range_table = thisboard->range_ao;
+	s->range_table = &range_unipolar10;
 	s->len_chanlist = 16;
 	s->insn_write = dyna_pci10xx_insn_write_ao;
 
 	/* digital input */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = thisboard->di_chans;
+	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
-	s->len_chanlist = thisboard->di_chans;
+	s->len_chanlist = 16;
 	s->insn_bits = dyna_pci10xx_di_insn_bits;
 
 	/* digital output */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-	s->n_chan = thisboard->do_chans;
+	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
-	s->len_chanlist = thisboard->do_chans;
+	s->len_chanlist = 16;
 	s->state = 0;
 	s->insn_bits = dyna_pci10xx_do_insn_bits;
 
-	printk(KERN_INFO "comedi: dyna_pci10xx: %s - device setup completed!\n",
-		thisboard->name);
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
-	return 1;
+	return 0;
 }
 
 static void dyna_pci10xx_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct dyna_pci10xx_private *devpriv = dev->private;
 
 	if (devpriv)
 		mutex_destroy(&devpriv->mutex);
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver dyna_pci10xx_driver = {
 	.driver_name	= "dyna_pci10xx",
 	.module		= THIS_MODULE,
-	.attach		= dyna_pci10xx_attach,
+	.attach_pci	= dyna_pci10xx_attach_pci,
 	.detach		= dyna_pci10xx_detach,
-	.board_name	= &boardtypes[0].name,
-	.offset		= sizeof(struct boardtype),
-	.num_names	= ARRAY_SIZE(boardtypes),
 };
 
 static int __devinit dyna_pci10xx_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index d1da80976f84..ae8e8f460295 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -140,7 +140,7 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	 * this if the definitions of the supdevices, 2 have been defined
 	 */
 	/* Analog indput */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* define subdevice as Analog In */
 	s->type = COMEDI_SUBD_AI;
 	/* you can read it from userspace */
@@ -156,7 +156,7 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_INFO "comedi: fl512: subdevice 0 initialized\n");
 
 	/* Analog output */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* define subdevice as Analog OUT */
 	s->type = COMEDI_SUBD_AO;
 	/* you can write it from userspace */
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 79f580841dee..abff6603952a 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -436,7 +436,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog input subdevice */
 	dev->read_subdev = s;
 /*	dev->write_subdev = s; */
@@ -723,45 +723,24 @@ static int di_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 		       struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
 	int i;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-	 * compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/*  uniqueness check */
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index b10ebdbc1f7e..d696d4d51e28 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -44,10 +44,7 @@ There are 4 x 12-bit Analogue Outputs.  Ranges : 5V, 10V, +/-5V, +/-10V
 
 4 x 16-bit counters
 
-Options:
- [0] - PCI bus number - if bus number and slot number are 0,
-			then driver search for first unused card
- [1] - PCI slot number
+Configuration options: not applicable, uses PCI auto config
 */
 
 #include <linux/interrupt.h>
@@ -56,16 +53,7 @@ Options:
 #include <linux/delay.h>
 #include <linux/pci.h>
 
-#include "icp_multi.h"
-
-#define DEVICE_ID	0x8000	/* Device ID */
-
-#define ICP_MULTI_EXTDEBUG
-
-/*  Hardware types of the cards */
-#define TYPE_ICP_MULTI	0
-
-#define IORANGE_ICP_MULTI 	32
+#define PCI_DEVICE_ID_ICP_MULTI	0x8000
 
 #define ICP_MULTI_ADC_CSR	0	/* R/W: ADC command/status register */
 #define ICP_MULTI_AI		2	/* R:   Analogue input data */
@@ -124,32 +112,10 @@ static const char range_codes_analog[] = { 0x00, 0x20, 0x10, 0x30 };
 	Data & Structure declarations
 ==============================================================================
 */
-static unsigned short pci_list_builded;	/*>0 list of card is known */
-
-struct boardtype {
-	const char *name;	/*  driver name */
-	int device_id;
-	int iorange;		/*  I/O range len */
-	char have_irq;		/*  1=card support IRQ */
-	char cardtype;		/*  0=ICP Multi */
-	int n_aichan;		/*  num of A/D chans */
-	int n_aichand;		/*  num of A/D chans in diff mode */
-	int n_aochan;		/*  num of D/A chans */
-	int n_dichan;		/*  num of DI chans */
-	int n_dochan;		/*  num of DO chans */
-	int n_ctrs;		/*  num of counters */
-	int ai_maxdata;		/*  resolution of A/D */
-	int ao_maxdata;		/*  resolution of D/A */
-	const struct comedi_lrange *rangelist_ai;	/*  rangelist for A/D */
-	const char *rangecode;	/*  range codes for programming */
-	const struct comedi_lrange *rangelist_ao;	/*  rangelist for D/A */
-};
 
 struct icp_multi_private {
-	struct pcilst_struct *card;	/*  pointer to card */
 	char valid;		/*  card is usable */
 	void __iomem *io_addr;		/*  Pointer to mapped io address */
-	resource_size_t phys_iobase;	/*  Physical io address */
 	unsigned int AdcCmdStatus;	/*  ADC Command/Status register */
 	unsigned int DacCmdStatus;	/*  DAC Command/Status register */
 	unsigned int IntEnable;	/*  Interrupt Enable register */
@@ -164,40 +130,14 @@ struct icp_multi_private {
 	unsigned int do_data;	/*  Remember digital output data */
 };
 
-#define devpriv ((struct icp_multi_private *)dev->private)
-#define this_board ((const struct boardtype *)dev->board_ptr)
-
-/*
-==============================================================================
-
-Name:	setup_channel_list
-
-Description:
-	This function sets the appropriate channel selection,
-	differential input mode and range bits in the ADC Command/
-	Status register.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current service structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	unsigned int *chanlist	Pointer to packed channel list
-	unsigned int n_chan	Number of channels to scan
-
-Returns:Void
-
-==============================================================================
-*/
 static void setup_channel_list(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       unsigned int *chanlist, unsigned int n_chan)
 {
+	struct icp_multi_private *devpriv = dev->private;
 	unsigned int i, range, chanprog;
 	unsigned int diff;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG:  setup_channel_list(...,%d)\n", n_chan);
-#endif
 	devpriv->act_chanlist_len = n_chan;
 	devpriv->act_chanlist_pos = 0;
 
@@ -228,50 +168,23 @@ static void setup_channel_list(struct comedi_device *dev,
 			devpriv->AdcCmdStatus |= (chanprog << 8);
 
 		/*  Get range for current channel */
-		range = this_board->rangecode[CR_RANGE(chanlist[i])];
+		range = range_codes_analog[CR_RANGE(chanlist[i])];
 		/*  Set range. bits 4-5 */
 		devpriv->AdcCmdStatus |= range;
 
 		/* Output channel, range, mode to ICP Multi */
 		writew(devpriv->AdcCmdStatus,
 		       devpriv->io_addr + ICP_MULTI_ADC_CSR);
-
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG
-		       "GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
-		       devpriv->act_chanlist[i]);
-#endif
 	}
-
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_read_ai
-
-Description:
-	This function reads a single analogue input.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to analogue input data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_read_ai(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct icp_multi_private *devpriv = dev->private;
 	int n, timeout;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG "icp multi EDBG: BGN: icp_multi_insn_read_ai(...)\n");
-#endif
 	/*  Disable A/D conversion ready interrupt */
 	devpriv->IntEnable &= ~ADC_READY;
 	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
@@ -283,12 +196,6 @@ static int icp_multi_insn_read_ai(struct comedi_device *dev,
 	/*  Set up appropriate channel, mode and range data, for specified ch */
 	setup_channel_list(dev, s, &insn->chanspec, 1);
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG "icp_multi A ST=%4x IO=%p\n",
-	       readw(devpriv->io_addr + ICP_MULTI_ADC_CSR),
-	       devpriv->io_addr + ICP_MULTI_ADC_CSR);
-#endif
-
 	for (n = 0; n < insn->n; n++) {
 		/*  Set start ADC bit */
 		devpriv->AdcCmdStatus |= ADC_ST;
@@ -296,18 +203,8 @@ static int icp_multi_insn_read_ai(struct comedi_device *dev,
 		       devpriv->io_addr + ICP_MULTI_ADC_CSR);
 		devpriv->AdcCmdStatus &= ~ADC_ST;
 
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG "icp multi B n=%d ST=%4x\n", n,
-		       readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
-#endif
-
 		udelay(1);
 
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG "icp multi C n=%d ST=%4x\n", n,
-		       readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
-#endif
-
 		/*  Wait for conversion to complete, or get fed up waiting */
 		timeout = 100;
 		while (timeout--) {
@@ -315,15 +212,6 @@ static int icp_multi_insn_read_ai(struct comedi_device *dev,
 				    ICP_MULTI_ADC_CSR) & ADC_BSY))
 				goto conv_finish;
 
-#ifdef ICP_MULTI_EXTDEBUG
-			if (!(timeout % 10))
-				printk(KERN_DEBUG
-				       "icp multi D n=%d tm=%d ST=%4x\n", n,
-				       timeout,
-				       readw(devpriv->io_addr +
-					     ICP_MULTI_ADC_CSR));
-#endif
-
 			udelay(1);
 		}
 
@@ -342,11 +230,6 @@ static int icp_multi_insn_read_ai(struct comedi_device *dev,
 		/*  Clear data received */
 		data[n] = 0;
 
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG
-		      "icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n",
-		      n);
-#endif
 		return -ETIME;
 
 conv_finish:
@@ -362,41 +245,16 @@ conv_finish:
 	devpriv->IntStatus |= ADC_READY;
 	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n", n);
-#endif
 	return n;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_write_ao
-
-Description:
-	This function writes a single analogue output.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to analogue output data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_write_ao(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	struct icp_multi_private *devpriv = dev->private;
 	int n, chan, range, timeout;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: BGN: icp_multi_insn_write_ao(...)\n");
-#endif
 	/*  Disable D/A conversion ready interrupt */
 	devpriv->IntEnable &= ~DAC_READY;
 	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
@@ -415,7 +273,7 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 	/*  Bit 5 = 1 : 10V */
 	/*  Bits 8-9 : Channel number */
 	devpriv->DacCmdStatus &= 0xfccf;
-	devpriv->DacCmdStatus |= this_board->rangecode[range];
+	devpriv->DacCmdStatus |= range_codes_analog[range];
 	devpriv->DacCmdStatus |= (chan << 8);
 
 	writew(devpriv->DacCmdStatus, devpriv->io_addr + ICP_MULTI_DAC_CSR);
@@ -429,15 +287,6 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 				    ICP_MULTI_DAC_CSR) & DAC_BSY))
 				goto dac_ready;
 
-#ifdef ICP_MULTI_EXTDEBUG
-			if (!(timeout % 10))
-				printk(KERN_DEBUG
-				       "icp multi A n=%d tm=%d ST=%4x\n", n,
-				       timeout,
-				       readw(devpriv->io_addr +
-					     ICP_MULTI_DAC_CSR));
-#endif
-
 			udelay(1);
 		}
 
@@ -456,11 +305,6 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 		/*  Clear data received */
 		devpriv->ao_data[chan] = 0;
 
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG
-		     "icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n",
-		     n);
-#endif
 		return -ETIME;
 
 dac_ready:
@@ -477,35 +321,14 @@ dac_ready:
 		devpriv->ao_data[chan] = data[n];
 	}
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n", n);
-#endif
 	return n;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_read_ao
-
-Description:
-	This function reads a single analogue output.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to analogue output data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_read_ao(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct icp_multi_private *devpriv = dev->private;
 	int n, chan;
 
 	/*  Get channel number */
@@ -518,58 +341,22 @@ static int icp_multi_insn_read_ao(struct comedi_device *dev,
 	return n;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_bits_di
-
-Description:
-	This function reads the digital inputs.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to analogue output data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_bits_di(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	struct icp_multi_private *devpriv = dev->private;
+
 	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
 
 	return insn->n;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_bits_do
-
-Description:
-	This function writes the appropriate digital outputs.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to analogue output data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_bits_do(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG "icp multi EDBG: BGN: icp_multi_insn_bits_do(...)\n");
-#endif
+	struct icp_multi_private *devpriv = dev->private;
 
 	if (data[0]) {
 		s->state &= ~data[0];
@@ -582,30 +369,9 @@ static int icp_multi_insn_bits_do(struct comedi_device *dev,
 
 	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG "icp multi EDBG: END: icp_multi_insn_bits_do(...)\n");
-#endif
 	return insn->n;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_read_ctr
-
-Description:
-	This function reads the specified counter.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to counter data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_read_ctr(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
@@ -613,24 +379,6 @@ static int icp_multi_insn_read_ctr(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_insn_write_ctr
-
-Description:
-	This function write to the specified counter.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	struct comedi_insn *insn	Pointer to current comedi instruction
-	unsigned int *data		Pointer to counter data
-
-Returns:int			Nmuber of instructions executed
-
-==============================================================================
-*/
 static int icp_multi_insn_write_ctr(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_insn *insn,
@@ -639,44 +387,18 @@ static int icp_multi_insn_write_ctr(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-
-Name:	interrupt_service_icp_multi
-
-Description:
-	This function is the interrupt service routine for all
-	interrupts generated by the icp multi board.
-
-Parameters:
-	int irq
-	void *d			Pointer to current device
-
-==============================================================================
-*/
 static irqreturn_t interrupt_service_icp_multi(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct icp_multi_private *devpriv = dev->private;
 	int int_no;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: BGN: interrupt_service_icp_multi(%d,...)\n",
-	       irq);
-#endif
-
 	/*  Is this interrupt from our board? */
 	int_no = readw(devpriv->io_addr + ICP_MULTI_INT_STAT) & Status_IRQ;
 	if (!int_no)
 		/*  No, exit */
 		return IRQ_NONE;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: interrupt_service_icp_multi() ST: %4x\n",
-	       readw(devpriv->io_addr + ICP_MULTI_INT_STAT));
-#endif
-
 	/*  Determine which interrupt is active & handle it */
 	switch (int_no) {
 	case ADC_READY:
@@ -700,44 +422,16 @@ static irqreturn_t interrupt_service_icp_multi(int irq, void *d)
 
 	}
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: END: interrupt_service_icp_multi(...)\n");
-#endif
 	return IRQ_HANDLED;
 }
 
 #if 0
-/*
-==============================================================================
-
-Name:	check_channel_list
-
-Description:
-	This function checks if the channel list, provided by user
-	is built correctly
-
-Parameters:
-	struct comedi_device *dev	Pointer to current service structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	unsigned int *chanlist	Pointer to packed channel list
-	unsigned int n_chan	Number of channels to scan
-
-Returns:int 0 = failure
-	    1 = success
-
-==============================================================================
-*/
 static int check_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      unsigned int *chanlist, unsigned int n_chan)
 {
 	unsigned int i;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG:  check_channel_list(...,%d)\n", n_chan);
-#endif
 	/*  Check that we at least have one channel to check */
 	if (n_chan < 1) {
 		comedi_error(dev, "range/channel list is empty!");
@@ -747,13 +441,13 @@ static int check_channel_list(struct comedi_device *dev,
 	for (i = 0; i < n_chan; i++) {
 		/*  Check that channel number is < maximum */
 		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
-			if (CR_CHAN(chanlist[i]) > this_board->n_aichand) {
+			if (CR_CHAN(chanlist[i]) > (s->nchan / 2)) {
 				comedi_error(dev,
 					     "Incorrect differential ai ch-nr");
 				return 0;
 			}
 		} else {
-			if (CR_CHAN(chanlist[i]) > this_board->n_aichan) {
+			if (CR_CHAN(chanlist[i]) > s->n_chan) {
 				comedi_error(dev,
 					     "Incorrect ai channel number");
 				return 0;
@@ -764,295 +458,189 @@ static int check_channel_list(struct comedi_device *dev,
 }
 #endif
 
-/*
-==============================================================================
-
-Name:	icp_multi_reset
-
-Description:
-	This function resets the icp multi device to a 'safe' state
-
-Parameters:
-	struct comedi_device *dev	Pointer to current service structure
-
-Returns:int	0 = success
-
-==============================================================================
-*/
 static int icp_multi_reset(struct comedi_device *dev)
 {
+	struct icp_multi_private *devpriv = dev->private;
 	unsigned int i;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp_multi EDBG: BGN: icp_multi_reset(...)\n");
-#endif
 	/*  Clear INT enables and requests */
 	writew(0, devpriv->io_addr + ICP_MULTI_INT_EN);
 	writew(0x00ff, devpriv->io_addr + ICP_MULTI_INT_STAT);
 
-	if (this_board->n_aochan)
-		/*  Set DACs to 0..5V range and 0V output */
-		for (i = 0; i < this_board->n_aochan; i++) {
-			devpriv->DacCmdStatus &= 0xfcce;
+	/* Set DACs to 0..5V range and 0V output */
+	for (i = 0; i < 4; i++) {
+		devpriv->DacCmdStatus &= 0xfcce;
+
+		/*  Set channel number */
+		devpriv->DacCmdStatus |= (i << 8);
 
-			/*  Set channel number */
-			devpriv->DacCmdStatus |= (i << 8);
+		/*  Output 0V */
+		writew(0, devpriv->io_addr + ICP_MULTI_AO);
 
-			/*  Output 0V */
-			writew(0, devpriv->io_addr + ICP_MULTI_AO);
+		/*  Set start conversion bit */
+		devpriv->DacCmdStatus |= DAC_ST;
 
-			/*  Set start conversion bit */
-			devpriv->DacCmdStatus |= DAC_ST;
+		/*  Output to command / status register */
+		writew(devpriv->DacCmdStatus,
+			devpriv->io_addr + ICP_MULTI_DAC_CSR);
 
-			/*  Output to command / status register */
-			writew(devpriv->DacCmdStatus,
-			       devpriv->io_addr + ICP_MULTI_DAC_CSR);
+		/*  Delay to allow DAC time to recover */
+		udelay(1);
+	}
 
-			/*  Delay to allow DAC time to recover */
-			udelay(1);
-		}
-	/*  Digital outputs to 0 */
+	/* Digital outputs to 0 */
 	writew(0, devpriv->io_addr + ICP_MULTI_DO);
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG: END: icp_multi_reset(...)\n");
-#endif
 	return 0;
 }
 
-static int icp_multi_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it)
+static int icp_multi_attach_pci(struct comedi_device *dev,
+				struct pci_dev *pcidev)
 {
+	struct icp_multi_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, subdev, n_subdevices;
-	unsigned int irq;
-	struct pcilst_struct *card = NULL;
-	resource_size_t io_addr[5], iobase;
-	unsigned char pci_bus, pci_slot, pci_func;
+	resource_size_t iobase;
+	int ret;
 
-	printk(KERN_WARNING
-	       "icp_multi EDBG: BGN: icp_multi_attach(...)\n");
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	dev->board_name = dev->driver->driver_name;
 
-	/*  Allocate private data storage space */
-	ret = alloc_private(dev, sizeof(struct icp_multi_private));
+	ret = alloc_private(dev, sizeof(*devpriv));
 	if (ret < 0)
 		return ret;
+	devpriv = dev->private;
 
-	/*  Initialise list of PCI cards in system, if not already done so */
-	if (pci_list_builded++ == 0) {
-		pci_card_list_init(PCI_VENDOR_ID_ICP,
-#ifdef ICP_MULTI_EXTDEBUG
-				   1
-#else
-				   0
-#endif
-		    );
-	}
-
-	printk(KERN_WARNING
-	       "Anne's comedi%d: icp_multi: board=%s", dev->minor,
-	       this_board->name);
-
-	card = select_and_alloc_pci_card(PCI_VENDOR_ID_ICP,
-					 this_board->device_id, it->options[0],
-					 it->options[1]);
-
-	if (card == NULL)
-		return -EIO;
-
-	devpriv->card = card;
-
-	if ((pci_card_data(card, &pci_bus, &pci_slot, &pci_func, &io_addr[0],
-			   &irq)) < 0) {
-		printk(KERN_WARNING " - Can't get configuration data!\n");
-		return -EIO;
-	}
-
-	iobase = io_addr[2];
-	devpriv->phys_iobase = iobase;
-
-	printk(KERN_WARNING
-	       ", b:s:f=%d:%d:%d, io=0x%8llx \n", pci_bus, pci_slot, pci_func,
-	       (unsigned long long)iobase);
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	iobase = pci_resource_start(pcidev, 2);
+	dev->iobase = iobase;
 
 	devpriv->io_addr = ioremap(iobase, ICP_MULTI_SIZE);
-
-	if (devpriv->io_addr == NULL) {
-		printk(KERN_WARNING "ioremap failed.\n");
+	if (!devpriv->io_addr)
 		return -ENOMEM;
-	}
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "0x%08llx mapped to %p, ", (unsigned long long)iobase,
-	       devpriv->io_addr);
-#endif
 
-	dev->board_name = this_board->name;
-
-	n_subdevices = 0;
-	if (this_board->n_aichan)
-		n_subdevices++;
-	if (this_board->n_aochan)
-		n_subdevices++;
-	if (this_board->n_dichan)
-		n_subdevices++;
-	if (this_board->n_dochan)
-		n_subdevices++;
-	if (this_board->n_ctrs)
-		n_subdevices++;
-
-	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	ret = comedi_alloc_subdevices(dev, 5);
 	if (ret)
 		return ret;
 
 	icp_multi_reset(dev);
 
-	if (this_board->have_irq) {
-		if (irq) {
-			if (request_irq(irq, interrupt_service_icp_multi,
-					IRQF_SHARED, "Inova Icp Multi", dev)) {
-				printk(KERN_WARNING
-				    "unable to allocate IRQ %u, DISABLING IT",
-				     irq);
-				irq = 0;	/* Can't use IRQ */
-			} else
-				printk(KERN_WARNING ", irq=%u", irq);
-		} else
-			printk(KERN_WARNING ", IRQ disabled");
-	} else
-		irq = 0;
-
-	dev->irq = irq;
-
-	printk(KERN_WARNING ".\n");
-
-	subdev = 0;
-
-	if (this_board->n_aichan) {
-		s = dev->subdevices + subdev;
-		dev->read_subdev = s;
-		s->type = COMEDI_SUBD_AI;
-		s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND;
-		if (this_board->n_aichand)
-			s->subdev_flags |= SDF_DIFF;
-		s->n_chan = this_board->n_aichan;
-		s->maxdata = this_board->ai_maxdata;
-		s->len_chanlist = this_board->n_aichan;
-		s->range_table = this_board->rangelist_ai;
-		s->insn_read = icp_multi_insn_read_ai;
-		subdev++;
+	if (pcidev->irq) {
+		ret = request_irq(pcidev->irq, interrupt_service_icp_multi,
+				  IRQF_SHARED, dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = pcidev->irq;
 	}
 
-	if (this_board->n_aochan) {
-		s = dev->subdevices + subdev;
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = this_board->n_aochan;
-		s->maxdata = this_board->ao_maxdata;
-		s->len_chanlist = this_board->n_aochan;
-		s->range_table = this_board->rangelist_ao;
-		s->insn_write = icp_multi_insn_write_ao;
-		s->insn_read = icp_multi_insn_read_ao;
-		subdev++;
-	}
-
-	if (this_board->n_dichan) {
-		s = dev->subdevices + subdev;
-		s->type = COMEDI_SUBD_DI;
-		s->subdev_flags = SDF_READABLE;
-		s->n_chan = this_board->n_dichan;
-		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dichan;
-		s->range_table = &range_digital;
-		s->io_bits = 0;
-		s->insn_bits = icp_multi_insn_bits_di;
-		subdev++;
-	}
-
-	if (this_board->n_dochan) {
-		s = dev->subdevices + subdev;
-		s->type = COMEDI_SUBD_DO;
-		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-		s->n_chan = this_board->n_dochan;
-		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dochan;
-		s->range_table = &range_digital;
-		s->io_bits = (1 << this_board->n_dochan) - 1;
-		s->state = 0;
-		s->insn_bits = icp_multi_insn_bits_do;
-		subdev++;
-	}
-
-	if (this_board->n_ctrs) {
-		s = dev->subdevices + subdev;
-		s->type = COMEDI_SUBD_COUNTER;
-		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = this_board->n_ctrs;
-		s->maxdata = 0xffff;
-		s->len_chanlist = this_board->n_ctrs;
-		s->state = 0;
-		s->insn_read = icp_multi_insn_read_ctr;
-		s->insn_write = icp_multi_insn_write_ctr;
-		subdev++;
-	}
+	s = &dev->subdevices[0];
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+	s->n_chan = 16;
+	s->maxdata = 0x0fff;
+	s->len_chanlist = 16;
+	s->range_table = &range_analog;
+	s->insn_read = icp_multi_insn_read_ai;
+
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan = 4;
+	s->maxdata = 0x0fff;
+	s->len_chanlist = 4;
+	s->range_table = &range_analog;
+	s->insn_write = icp_multi_insn_write_ao;
+	s->insn_read = icp_multi_insn_read_ao;
+
+	s = &dev->subdevices[2];
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->len_chanlist = 16;
+	s->range_table = &range_digital;
+	s->io_bits = 0;
+	s->insn_bits = icp_multi_insn_bits_di;
+
+	s = &dev->subdevices[3];
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->len_chanlist = 8;
+	s->range_table = &range_digital;
+	s->io_bits = 0xff;
+	s->state = 0;
+	s->insn_bits = icp_multi_insn_bits_do;
+
+	s = &dev->subdevices[4];
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->len_chanlist = 4;
+	s->state = 0;
+	s->insn_read = icp_multi_insn_read_ctr;
+	s->insn_write = icp_multi_insn_write_ctr;
 
 	devpriv->valid = 1;
 
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG "icp multi EDBG: END: icp_multi_attach(...)\n");
-#endif
+	dev_info(dev->class_dev, "%s attached, irq %sabled\n",
+		dev->board_name, dev->irq ? "en" : "dis");
 
 	return 0;
 }
 
 static void icp_multi_detach(struct comedi_device *dev)
 {
-	if (dev->private)
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct icp_multi_private *devpriv = dev->private;
+
+	if (devpriv)
 		if (devpriv->valid)
 			icp_multi_reset(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (dev->private && devpriv->io_addr)
+	if (devpriv && devpriv->io_addr)
 		iounmap(devpriv->io_addr);
-	if (dev->private && devpriv->card)
-		pci_card_free(devpriv->card);
-	if (--pci_list_builded == 0)
-		pci_card_list_cleanup(PCI_VENDOR_ID_ICP);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+	}
 }
 
-static const struct boardtype boardtypes[] = {
-	{
-		.name		= "icp_multi",
-		.device_id	= DEVICE_ID,
-		.iorange	= IORANGE_ICP_MULTI,
-		.have_irq	= 1,
-		.cardtype	= TYPE_ICP_MULTI,
-		.n_aichan	= 16,
-		.n_aichand	= 8,
-		.n_aochan	= 4,
-		.n_dichan	= 16,
-		.n_dochan	= 8,
-		.n_ctrs		= 4,
-		.ai_maxdata	= 0x0fff,
-		.ao_maxdata	= 0x0fff,
-		.rangelist_ai	= &range_analog,
-		.rangecode	= range_codes_analog,
-		.rangelist_ao	= &range_analog,
-	},
-};
-
 static struct comedi_driver icp_multi_driver = {
 	.driver_name	= "icp_multi",
 	.module		= THIS_MODULE,
-	.attach		= icp_multi_attach,
+	.attach_pci	= icp_multi_attach_pci,
 	.detach		= icp_multi_detach,
-	.num_names	= ARRAY_SIZE(boardtypes),
-	.board_name	= &boardtypes[0].name,
-	.offset		= sizeof(struct boardtype),
 };
-module_comedi_driver(icp_multi_driver);
+
+static int __devinit icp_multi_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &icp_multi_driver);
+}
+
+static void __devexit icp_multi_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(icp_multi_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ICP, PCI_DEVICE_ID_ICP_MULTI) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, icp_multi_pci_table);
+
+static struct pci_driver icp_multi_pci_driver = {
+	.name		= "icp_multi",
+	.id_table	= icp_multi_pci_table,
+	.probe		= icp_multi_pci_probe,
+	.remove		= __devexit_p(icp_multi_pci_remove),
+};
+module_comedi_pci_driver(icp_multi_driver, icp_multi_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/icp_multi.h b/drivers/staging/comedi/drivers/icp_multi.h
deleted file mode 100644
index dbf9908cfde6..000000000000
--- a/drivers/staging/comedi/drivers/icp_multi.h
+++ /dev/null
@@ -1,297 +0,0 @@
-/*
-    comedi/drivers/icp_multi.h
-
-    Stuff for ICP Multi
-
-    Author: Anne Smorthit <anne.smorthit@sfwte.ch>
-
-*/
-
-#ifndef _ICP_MULTI_H_
-#define _ICP_MULTI_H_
-
-#include "../comedidev.h"
-
-/****************************************************************************/
-
-struct pcilst_struct {
-	struct pcilst_struct *next;
-	int used;
-	struct pci_dev *pcidev;
-	unsigned short vendor;
-	unsigned short device;
-	unsigned char pci_bus;
-	unsigned char pci_slot;
-	unsigned char pci_func;
-	resource_size_t io_addr[5];
-	unsigned int irq;
-};
-
-struct pcilst_struct *inova_devices;
-/* ptr to root list of all Inova devices */
-
-/****************************************************************************/
-
-static void pci_card_list_init(unsigned short pci_vendor, char display);
-static void pci_card_list_cleanup(unsigned short pci_vendor);
-static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
-							  vendor_id,
-							  unsigned short
-							  device_id);
-static int find_free_pci_card_by_position(unsigned short vendor_id,
-					  unsigned short device_id,
-					  unsigned short pci_bus,
-					  unsigned short pci_slot,
-					  struct pcilst_struct **card);
-static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
-						       unsigned short device_id,
-						       unsigned short pci_bus,
-						       unsigned short pci_slot);
-
-static int pci_card_alloc(struct pcilst_struct *amcc);
-static int pci_card_free(struct pcilst_struct *amcc);
-static void pci_card_list_display(void);
-static int pci_card_data(struct pcilst_struct *amcc,
-			 unsigned char *pci_bus, unsigned char *pci_slot,
-			 unsigned char *pci_func, resource_size_t * io_addr,
-			 unsigned int *irq);
-
-/****************************************************************************/
-
-/* build list of Inova cards in this system */
-static void pci_card_list_init(unsigned short pci_vendor, char display)
-{
-	struct pci_dev *pcidev = NULL;
-	struct pcilst_struct *inova, *last;
-	int i;
-
-	inova_devices = NULL;
-	last = NULL;
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == pci_vendor) {
-			inova = kzalloc(sizeof(*inova), GFP_KERNEL);
-			if (!inova) {
-				printk
-				    ("icp_multi: pci_card_list_init: allocation failed\n");
-				pci_dev_put(pcidev);
-				break;
-			}
-
-			inova->pcidev = pci_dev_get(pcidev);
-			if (last) {
-				last->next = inova;
-			} else {
-				inova_devices = inova;
-			}
-			last = inova;
-
-			inova->vendor = pcidev->vendor;
-			inova->device = pcidev->device;
-			inova->pci_bus = pcidev->bus->number;
-			inova->pci_slot = PCI_SLOT(pcidev->devfn);
-			inova->pci_func = PCI_FUNC(pcidev->devfn);
-			/* Note: resources may be invalid if PCI device
-			 * not enabled, but they are corrected in
-			 * pci_card_alloc. */
-			for (i = 0; i < 5; i++)
-				inova->io_addr[i] =
-				    pci_resource_start(pcidev, i);
-			inova->irq = pcidev->irq;
-		}
-	}
-
-	if (display)
-		pci_card_list_display();
-}
-
-/****************************************************************************/
-/* free up list of amcc cards in this system */
-static void pci_card_list_cleanup(unsigned short pci_vendor)
-{
-	struct pcilst_struct *inova, *next;
-
-	for (inova = inova_devices; inova; inova = next) {
-		next = inova->next;
-		pci_dev_put(inova->pcidev);
-		kfree(inova);
-	}
-
-	inova_devices = NULL;
-}
-
-/****************************************************************************/
-/* find first unused card with this device_id */
-static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
-							  vendor_id,
-							  unsigned short
-							  device_id)
-{
-	struct pcilst_struct *inova, *next;
-
-	for (inova = inova_devices; inova; inova = next) {
-		next = inova->next;
-		if ((!inova->used) && (inova->device == device_id)
-		    && (inova->vendor == vendor_id))
-			return inova;
-
-	}
-
-	return NULL;
-}
-
-/****************************************************************************/
-/* find card on requested position */
-static int find_free_pci_card_by_position(unsigned short vendor_id,
-					  unsigned short device_id,
-					  unsigned short pci_bus,
-					  unsigned short pci_slot,
-					  struct pcilst_struct **card)
-{
-	struct pcilst_struct *inova, *next;
-
-	*card = NULL;
-	for (inova = inova_devices; inova; inova = next) {
-		next = inova->next;
-		if ((inova->vendor == vendor_id) && (inova->device == device_id)
-		    && (inova->pci_bus == pci_bus)
-		    && (inova->pci_slot == pci_slot)) {
-			if (!(inova->used)) {
-				*card = inova;
-				return 0;	/* ok, card is found */
-			} else {
-				return 2;	/* card exist but is used */
-			}
-		}
-	}
-
-	return 1;		/* no card found */
-}
-
-/****************************************************************************/
-/* mark card as used */
-static int pci_card_alloc(struct pcilst_struct *inova)
-{
-	int i;
-
-	if (!inova) {
-		printk(" - BUG!! inova is NULL!\n");
-		return -1;
-	}
-
-	if (inova->used)
-		return 1;
-	if (comedi_pci_enable(inova->pcidev, "icp_multi")) {
-		printk(" - Can't enable PCI device and request regions!\n");
-		return -1;
-	}
-	/* Resources will be accurate now. */
-	for (i = 0; i < 5; i++)
-		inova->io_addr[i] = pci_resource_start(inova->pcidev, i);
-	inova->irq = inova->pcidev->irq;
-	inova->used = 1;
-	return 0;
-}
-
-/****************************************************************************/
-/* mark card as free */
-static int pci_card_free(struct pcilst_struct *inova)
-{
-	if (!inova)
-		return -1;
-
-	if (!inova->used)
-		return 1;
-	inova->used = 0;
-	comedi_pci_disable(inova->pcidev);
-	return 0;
-}
-
-/****************************************************************************/
-/* display list of found cards */
-static void pci_card_list_display(void)
-{
-	struct pcilst_struct *inova, *next;
-
-	printk("Anne's List of pci cards\n");
-	printk("bus:slot:func vendor device io_inova io_daq irq used\n");
-
-	for (inova = inova_devices; inova; inova = next) {
-		next = inova->next;
-		printk
-		    ("%2d   %2d   %2d  0x%4x 0x%4x   0x%8llx 0x%8llx  %2u  %2d\n",
-		     inova->pci_bus, inova->pci_slot, inova->pci_func,
-		     inova->vendor, inova->device,
-		     (unsigned long long)inova->io_addr[0],
-		     (unsigned long long)inova->io_addr[2], inova->irq,
-		     inova->used);
-
-	}
-}
-
-/****************************************************************************/
-/* return all card information for driver */
-static int pci_card_data(struct pcilst_struct *inova,
-			 unsigned char *pci_bus, unsigned char *pci_slot,
-			 unsigned char *pci_func, resource_size_t * io_addr,
-			 unsigned int *irq)
-{
-	int i;
-
-	if (!inova)
-		return -1;
-	*pci_bus = inova->pci_bus;
-	*pci_slot = inova->pci_slot;
-	*pci_func = inova->pci_func;
-	for (i = 0; i < 5; i++)
-		io_addr[i] = inova->io_addr[i];
-	*irq = inova->irq;
-	return 0;
-}
-
-/****************************************************************************/
-/* select and alloc card */
-static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
-						       unsigned short device_id,
-						       unsigned short pci_bus,
-						       unsigned short pci_slot)
-{
-	struct pcilst_struct *card;
-	int err;
-
-	if ((pci_bus < 1) & (pci_slot < 1)) {	/* use autodetection */
-
-		card = find_free_pci_card_by_device(vendor_id, device_id);
-		if (card == NULL) {
-			printk(" - Unused card not found in system!\n");
-			return NULL;
-		}
-	} else {
-		switch (find_free_pci_card_by_position(vendor_id, device_id,
-						       pci_bus, pci_slot,
-						       &card)) {
-		case 1:
-			printk
-			    (" - Card not found on requested position b:s %d:%d!\n",
-			     pci_bus, pci_slot);
-			return NULL;
-		case 2:
-			printk
-			    (" - Card on requested position is used b:s %d:%d!\n",
-			     pci_bus, pci_slot);
-			return NULL;
-		}
-	}
-
-	err = pci_card_alloc(card);
-	if (err != 0) {
-		if (err > 0)
-			printk(" - Can't allocate card!\n");
-		/* else: error already printed. */
-		return NULL;
-	}
-
-	return card;
-}
-
-#endif
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 0f9cfe662b9a..65ff1c9b973c 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -224,7 +224,7 @@ static int pci20xxx_attach(struct comedi_device *dev,
 	       dev->minor, devpriv->ioaddr);
 
 	for (i = 0; i < PCI20000_MODULES; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		id = readb(devpriv->ioaddr + (i + 1) * PCI20000_OFFSET);
 		s->private = devpriv->subdev_private + i;
 		sdp = s->private;
@@ -259,7 +259,7 @@ static int pci20xxx_attach(struct comedi_device *dev,
 	}
 
 	/* initialize struct pci20xxx_private */
-	pci20xxx_dio_init(dev, dev->subdevices + PCI20000_MODULES);
+	pci20xxx_dio_init(dev, &dev->subdevices[PCI20000_MODULES]);
 
 	return 1;
 }
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 93f94cd7bae2..4a108ea8a9aa 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -362,10 +362,11 @@ static int jr3_pci_open(struct comedi_device *dev)
 	return 0;
 }
 
-int read_idm_word(const u8 * data, size_t size, int *pos, unsigned int *val)
+static int read_idm_word(const u8 *data, size_t size, int *pos,
+			 unsigned int *val)
 {
 	int result = 0;
-	if (pos != 0 && val != 0) {
+	if (pos && val) {
 		/*  Skip over non hex */
 		for (; *pos < size && !isxdigit(data[*pos]); (*pos)++) {
 		}
@@ -875,7 +876,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 			p->maxdata_list[56] = 0xffff;
 			p->maxdata_list[57] = 0xffff;
 			/*  Channel specific range and maxdata */
-			dev->subdevices[i].range_table = 0;
+			dev->subdevices[i].range_table = NULL;
 			dev->subdevices[i].range_table_list =
 			    p->range_table_list;
 			dev->subdevices[i].maxdata = 0;
@@ -884,7 +885,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	}
 
 	/*  Reset DSP card */
-	devpriv->iobase->channel[0].reset = 0;
+	writel(0, &devpriv->iobase->channel[0].reset);
 
 	result = comedi_load_firmware(dev, "jr3pci.idm", jr3_download_firmware);
 	dev_dbg(dev->class_dev, "Firmare load %d\n", result);
diff --git a/drivers/staging/comedi/drivers/jr3_pci.h b/drivers/staging/comedi/drivers/jr3_pci.h
index a1469611d84e..9c42653d8f18 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.h
+++ b/drivers/staging/comedi/drivers/jr3_pci.h
@@ -97,7 +97,7 @@ enum {
 	mz = 0x0020,
 	changeV2 = 0x0040,
 	changeV1 = 0x0080
-} vect_bits_t;
+};
 
 /* WARNING_BITS */
 /* The warning_bits structure shows the bit pattern for the warning
@@ -116,7 +116,7 @@ enum {
 	mx_near_sat = 0x0008,
 	my_near_sat = 0x0010,
 	mz_near_sat = 0x0020
-} warning_bits_t;
+};
 
 /*  ERROR_BITS */
 /*  XX_SAT */
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index d4e9292483a0..e867b720f666 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -28,11 +28,7 @@ Author: Michael Hillmann
 Updated: Mon, 14 Apr 2008 15:42:42 +0100
 Status: tested
 
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+Configuration Options: not applicable, uses PCI auto config
 
 This driver is a simple driver to read the counter values from
 Kolter Electronic PCI Counter Card.
@@ -111,82 +107,53 @@ static int cnt_rinsn(struct comedi_device *dev,
 	return 1;
 }
 
-static struct pci_dev *cnt_find_pci_dev(struct comedi_device *dev,
-					struct comedi_devconfig *it)
+static const void *cnt_find_boardinfo(struct comedi_device *dev,
+				      struct pci_dev *pcidev)
 {
 	const struct cnt_board_struct *board;
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
 	int i;
 
-	/* Probe the device to determine what device in the series it is. */
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_KOLTER)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(cnt_boards); i++) {
-			board = &cnt_boards[i];
-			if (board->device_id != pcidev->device)
-				continue;
-
-			dev->board_ptr = board;
-			return pcidev;
-		}
+	for (i = 0; i < ARRAY_SIZE(cnt_boards); i++) {
+		board = &cnt_boards[i];
+		if (board->device_id == pcidev->device)
+			return board;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int cnt_attach_pci(struct comedi_device *dev,
+			  struct pci_dev *pcidev)
 {
 	const struct cnt_board_struct *board;
-	struct pci_dev *pcidev;
-	struct comedi_subdevice *subdevice;
-	unsigned long io_base;
-	int error;
-
-	pcidev = cnt_find_pci_dev(dev, it);
-	if (!pcidev)
-		return -EIO;
+	struct comedi_subdevice *s;
+	int ret;
+
 	comedi_set_hw_dev(dev, &pcidev->dev);
-	board = comedi_board(dev);
 
+	board = cnt_find_boardinfo(dev, pcidev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
 	dev->board_name = board->name;
 
-	/* enable PCI device and request regions */
-	error = comedi_pci_enable(pcidev, CNT_DRIVER_NAME);
-	if (error < 0) {
-		printk(KERN_WARNING "comedi%d: "
-		       "failed to enable PCI device and request regions!\n",
-		       dev->minor);
-		return error;
-	}
-
-	/* read register base address [PCI_BASE_ADDRESS #0] */
-	io_base = pci_resource_start(pcidev, 0);
-	dev->iobase = io_base;
+	ret = comedi_pci_enable(pcidev, dev->board_name);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, 0);
 
-	error = comedi_alloc_subdevices(dev, 1);
-	if (error)
-		return error;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
-	subdevice = dev->subdevices + 0;
-	dev->read_subdev = subdevice;
+	s = &dev->subdevices[0];
+	dev->read_subdev = s;
 
-	subdevice->type = COMEDI_SUBD_COUNTER;
-	subdevice->subdev_flags = SDF_READABLE /* | SDF_COMMON */ ;
-	subdevice->n_chan = board->cnt_channel_nbr;
-	subdevice->maxdata = (1 << board->cnt_bits) - 1;
-	subdevice->insn_read = cnt_rinsn;
-	subdevice->insn_write = cnt_winsn;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_READABLE /* | SDF_COMMON */ ;
+	s->n_chan = board->cnt_channel_nbr;
+	s->maxdata = (1 << board->cnt_bits) - 1;
+	s->insn_read = cnt_rinsn;
+	s->insn_write = cnt_winsn;
 
 	/*  select 20MHz clock */
 	outb(3, dev->iobase + 248);
@@ -196,8 +163,9 @@ static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	outb(0, dev->iobase + 0x20);
 	outb(0, dev->iobase + 0x40);
 
-	printk(KERN_INFO "comedi%d: " CNT_DRIVER_NAME " attached.\n",
-	       dev->minor);
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
+
 	return 0;
 }
 
@@ -208,14 +176,13 @@ static void cnt_detach(struct comedi_device *dev)
 	if (pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(pcidev);
-		pci_dev_put(pcidev);
 	}
 }
 
 static struct comedi_driver ke_counter_driver = {
 	.driver_name	= "ke_counter",
 	.module		= THIS_MODULE,
-	.attach		= cnt_attach,
+	.attach_pci	= cnt_attach_pci,
 	.detach		= cnt_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 9a8258e6fa45..22db35d091f8 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -35,13 +35,7 @@ Supports:
     - Digital I/O
     - Counter
 
-Configuration Options:
-
-    [0] - PCI bus number (optional)
-    [1] - PCI slot number (optional)
-
-    If bus/slot is not specified, the first available PCI
-    device will be used.
+Configuration Options: not applicable, uses PCI auto config
 
 The firmware required by these boards is available in the
 comedi_nonfree_firmware tarball available from
@@ -58,51 +52,306 @@ broken.
 #include <linux/list.h>
 #include <linux/spinlock.h>
 
-#include "me4000.h"
+#include "comedi_fc.h"
+#include "8253.h"
+
 #if 0
 /* file removed due to GPL incompatibility */
 #include "me4000_fw.h"
 #endif
 
-static const struct me4000_board me4000_boards[] = {
-	{"ME-4650", 0x4650, {0, 0}, {16, 0, 0, 0}, {4}, {0} },
-
-	{"ME-4660", 0x4660, {0, 0}, {32, 0, 16, 0}, {4}, {3} },
-	{"ME-4660i", 0x4661, {0, 0}, {32, 0, 16, 0}, {4}, {3} },
-	{"ME-4660s", 0x4662, {0, 0}, {32, 8, 16, 0}, {4}, {3} },
-	{"ME-4660is", 0x4663, {0, 0}, {32, 8, 16, 0}, {4}, {3} },
-
-	{"ME-4670", 0x4670, {4, 0}, {32, 0, 16, 1}, {4}, {3} },
-	{"ME-4670i", 0x4671, {4, 0}, {32, 0, 16, 1}, {4}, {3} },
-	{"ME-4670s", 0x4672, {4, 0}, {32, 8, 16, 1}, {4}, {3} },
-	{"ME-4670is", 0x4673, {4, 0}, {32, 8, 16, 1}, {4}, {3} },
+#define PCI_VENDOR_ID_MEILHAUS		0x1402
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4650	0x4650
+#define PCI_DEVICE_ID_MEILHAUS_ME4660	0x4660
+#define PCI_DEVICE_ID_MEILHAUS_ME4660I	0x4661
+#define PCI_DEVICE_ID_MEILHAUS_ME4660S	0x4662
+#define PCI_DEVICE_ID_MEILHAUS_ME4660IS	0x4663
+#define PCI_DEVICE_ID_MEILHAUS_ME4670	0x4670
+#define PCI_DEVICE_ID_MEILHAUS_ME4670I	0x4671
+#define PCI_DEVICE_ID_MEILHAUS_ME4670S	0x4672
+#define PCI_DEVICE_ID_MEILHAUS_ME4670IS	0x4673
+#define PCI_DEVICE_ID_MEILHAUS_ME4680	0x4680
+#define PCI_DEVICE_ID_MEILHAUS_ME4680I	0x4681
+#define PCI_DEVICE_ID_MEILHAUS_ME4680S	0x4682
+#define PCI_DEVICE_ID_MEILHAUS_ME4680IS	0x4683
 
-	{"ME-4680", 0x4680, {4, 4}, {32, 0, 16, 1}, {4}, {3} },
-	{"ME-4680i", 0x4681, {4, 4}, {32, 0, 16, 1}, {4}, {3} },
-	{"ME-4680s", 0x4682, {4, 4}, {32, 8, 16, 1}, {4}, {3} },
-	{"ME-4680is", 0x4683, {4, 4}, {32, 8, 16, 1}, {4}, {3} },
+/*
+ * ME4000 Register map and bit defines
+ */
+#define ME4000_AO_CHAN(x)			((x) * 0x18)
+
+#define ME4000_AO_CTRL_REG(x)			(0x00 + ME4000_AO_CHAN(x))
+#define ME4000_AO_CTRL_BIT_MODE_0		(1 << 0)
+#define ME4000_AO_CTRL_BIT_MODE_1		(1 << 1)
+#define ME4000_AO_CTRL_MASK_MODE		(3 << 0)
+#define ME4000_AO_CTRL_BIT_STOP			(1 << 2)
+#define ME4000_AO_CTRL_BIT_ENABLE_FIFO		(1 << 3)
+#define ME4000_AO_CTRL_BIT_ENABLE_EX_TRIG	(1 << 4)
+#define ME4000_AO_CTRL_BIT_EX_TRIG_EDGE		(1 << 5)
+#define ME4000_AO_CTRL_BIT_IMMEDIATE_STOP	(1 << 7)
+#define ME4000_AO_CTRL_BIT_ENABLE_DO		(1 << 8)
+#define ME4000_AO_CTRL_BIT_ENABLE_IRQ		(1 << 9)
+#define ME4000_AO_CTRL_BIT_RESET_IRQ		(1 << 10)
+#define ME4000_AO_STATUS_REG(x)			(0x04 + ME4000_AO_CHAN(x))
+#define ME4000_AO_STATUS_BIT_FSM		(1 << 0)
+#define ME4000_AO_STATUS_BIT_FF			(1 << 1)
+#define ME4000_AO_STATUS_BIT_HF			(1 << 2)
+#define ME4000_AO_STATUS_BIT_EF			(1 << 3)
+#define ME4000_AO_FIFO_REG(x)			(0x08 + ME4000_AO_CHAN(x))
+#define ME4000_AO_SINGLE_REG(x)			(0x0c + ME4000_AO_CHAN(x))
+#define ME4000_AO_TIMER_REG(x)			(0x10 + ME4000_AO_CHAN(x))
+#define ME4000_AI_CTRL_REG			0x74
+#define ME4000_AI_STATUS_REG			0x74
+#define ME4000_AI_CTRL_BIT_MODE_0		(1 << 0)
+#define ME4000_AI_CTRL_BIT_MODE_1		(1 << 1)
+#define ME4000_AI_CTRL_BIT_MODE_2		(1 << 2)
+#define ME4000_AI_CTRL_BIT_SAMPLE_HOLD		(1 << 3)
+#define ME4000_AI_CTRL_BIT_IMMEDIATE_STOP	(1 << 4)
+#define ME4000_AI_CTRL_BIT_STOP			(1 << 5)
+#define ME4000_AI_CTRL_BIT_CHANNEL_FIFO		(1 << 6)
+#define ME4000_AI_CTRL_BIT_DATA_FIFO		(1 << 7)
+#define ME4000_AI_CTRL_BIT_FULLSCALE		(1 << 8)
+#define ME4000_AI_CTRL_BIT_OFFSET		(1 << 9)
+#define ME4000_AI_CTRL_BIT_EX_TRIG_ANALOG	(1 << 10)
+#define ME4000_AI_CTRL_BIT_EX_TRIG		(1 << 11)
+#define ME4000_AI_CTRL_BIT_EX_TRIG_FALLING	(1 << 12)
+#define ME4000_AI_CTRL_BIT_EX_IRQ		(1 << 13)
+#define ME4000_AI_CTRL_BIT_EX_IRQ_RESET		(1 << 14)
+#define ME4000_AI_CTRL_BIT_LE_IRQ		(1 << 15)
+#define ME4000_AI_CTRL_BIT_LE_IRQ_RESET		(1 << 16)
+#define ME4000_AI_CTRL_BIT_HF_IRQ		(1 << 17)
+#define ME4000_AI_CTRL_BIT_HF_IRQ_RESET		(1 << 18)
+#define ME4000_AI_CTRL_BIT_SC_IRQ		(1 << 19)
+#define ME4000_AI_CTRL_BIT_SC_IRQ_RESET		(1 << 20)
+#define ME4000_AI_CTRL_BIT_SC_RELOAD		(1 << 21)
+#define ME4000_AI_STATUS_BIT_EF_CHANNEL		(1 << 22)
+#define ME4000_AI_STATUS_BIT_HF_CHANNEL		(1 << 23)
+#define ME4000_AI_STATUS_BIT_FF_CHANNEL		(1 << 24)
+#define ME4000_AI_STATUS_BIT_EF_DATA		(1 << 25)
+#define ME4000_AI_STATUS_BIT_HF_DATA		(1 << 26)
+#define ME4000_AI_STATUS_BIT_FF_DATA		(1 << 27)
+#define ME4000_AI_STATUS_BIT_LE			(1 << 28)
+#define ME4000_AI_STATUS_BIT_FSM		(1 << 29)
+#define ME4000_AI_CTRL_BIT_EX_TRIG_BOTH		(1 << 31)
+#define ME4000_AI_CHANNEL_LIST_REG		0x78
+#define ME4000_AI_LIST_INPUT_SINGLE_ENDED	(0 << 5)
+#define ME4000_AI_LIST_INPUT_DIFFERENTIAL	(1 << 5)
+#define ME4000_AI_LIST_RANGE_BIPOLAR_10		(0 << 6)
+#define ME4000_AI_LIST_RANGE_BIPOLAR_2_5	(1 << 6)
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_10	(2 << 6)
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_2_5	(3 << 6)
+#define ME4000_AI_LIST_LAST_ENTRY		(1 << 8)
+#define ME4000_AI_DATA_REG			0x7c
+#define ME4000_AI_CHAN_TIMER_REG		0x80
+#define ME4000_AI_CHAN_PRE_TIMER_REG		0x84
+#define ME4000_AI_SCAN_TIMER_LOW_REG		0x88
+#define ME4000_AI_SCAN_TIMER_HIGH_REG		0x8c
+#define ME4000_AI_SCAN_PRE_TIMER_LOW_REG	0x90
+#define ME4000_AI_SCAN_PRE_TIMER_HIGH_REG	0x94
+#define ME4000_AI_START_REG			0x98
+#define ME4000_IRQ_STATUS_REG			0x9c
+#define ME4000_IRQ_STATUS_BIT_EX		(1 << 0)
+#define ME4000_IRQ_STATUS_BIT_LE		(1 << 1)
+#define ME4000_IRQ_STATUS_BIT_AI_HF		(1 << 2)
+#define ME4000_IRQ_STATUS_BIT_AO_0_HF		(1 << 3)
+#define ME4000_IRQ_STATUS_BIT_AO_1_HF		(1 << 4)
+#define ME4000_IRQ_STATUS_BIT_AO_2_HF		(1 << 5)
+#define ME4000_IRQ_STATUS_BIT_AO_3_HF		(1 << 6)
+#define ME4000_IRQ_STATUS_BIT_SC		(1 << 7)
+#define ME4000_DIO_PORT_0_REG			0xa0
+#define ME4000_DIO_PORT_1_REG			0xa4
+#define ME4000_DIO_PORT_2_REG			0xa8
+#define ME4000_DIO_PORT_3_REG			0xac
+#define ME4000_DIO_DIR_REG			0xb0
+#define ME4000_AO_LOADSETREG_XX			0xb4
+#define ME4000_DIO_CTRL_REG			0xb8
+#define ME4000_DIO_CTRL_BIT_MODE_0		(1 << 0)
+#define ME4000_DIO_CTRL_BIT_MODE_1		(1 << 1)
+#define ME4000_DIO_CTRL_BIT_MODE_2		(1 << 2)
+#define ME4000_DIO_CTRL_BIT_MODE_3		(1 << 3)
+#define ME4000_DIO_CTRL_BIT_MODE_4		(1 << 4)
+#define ME4000_DIO_CTRL_BIT_MODE_5		(1 << 5)
+#define ME4000_DIO_CTRL_BIT_MODE_6		(1 << 6)
+#define ME4000_DIO_CTRL_BIT_MODE_7		(1 << 7)
+#define ME4000_DIO_CTRL_BIT_FUNCTION_0		(1 << 8)
+#define ME4000_DIO_CTRL_BIT_FUNCTION_1		(1 << 9)
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_0		(1 << 10)
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_1		(1 << 11)
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_2		(1 << 12)
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_3		(1 << 13)
+#define ME4000_AO_DEMUX_ADJUST_REG		0xbc
+#define ME4000_AO_DEMUX_ADJUST_VALUE		0x4c
+#define ME4000_AI_SAMPLE_COUNTER_REG		0xc0
 
-	{0},
+/*
+ * PLX Register map and bit defines
+ */
+#define PLX_INTCSR				0x4c
+#define PLX_INTCSR_LOCAL_INT1_EN		(1 << 0)
+#define PLX_INTCSR_LOCAL_INT1_POL		(1 << 1)
+#define PLX_INTCSR_LOCAL_INT1_STATE		(1 << 2)
+#define PLX_INTCSR_LOCAL_INT2_EN		(1 << 3)
+#define PLX_INTCSR_LOCAL_INT2_POL		(1 << 4)
+#define PLX_INTCSR_LOCAL_INT2_STATE		(1 << 5)
+#define PLX_INTCSR_PCI_INT_EN			(1 << 6)
+#define PLX_INTCSR_SOFT_INT			(1 << 7)
+#define PLX_ICR					0x50
+#define PLX_ICR_BIT_EEPROM_CLOCK_SET		(1 << 24)
+#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		(1 << 25)
+#define PLX_ICR_BIT_EEPROM_WRITE		(1 << 26)
+#define PLX_ICR_BIT_EEPROM_READ			(1 << 27)
+#define PLX_ICR_BIT_EEPROM_VALID		(1 << 28)
+#define PLX_ICR_MASK_EEPROM			(0x1f << 24)
+
+#define EEPROM_DELAY				1
+
+#define ME4000_AI_FIFO_COUNT			2048
+
+#define ME4000_AI_MIN_TICKS			66
+#define ME4000_AI_MIN_SAMPLE_TIME		2000
+#define ME4000_AI_BASE_FREQUENCY		(unsigned int) 33E6
+
+#define ME4000_AI_CHANNEL_LIST_COUNT		1024
+
+struct me4000_info {
+	unsigned long plx_regbase;
+	unsigned long timer_regbase;
+
+	unsigned int ao_readback[4];
 };
 
-#define ME4000_BOARD_VERSIONS (ARRAY_SIZE(me4000_boards) - 1)
-
-/*-----------------------------------------------------------------------------
-  Meilhaus function prototypes
-  ---------------------------------------------------------------------------*/
-static int get_registers(struct comedi_device *dev, struct pci_dev *pci_dev_p);
-static int init_board_info(struct comedi_device *dev,
-			   struct pci_dev *pci_dev_p);
-static int init_ao_context(struct comedi_device *dev);
-static int init_ai_context(struct comedi_device *dev);
-static int init_dio_context(struct comedi_device *dev);
-static int init_cnt_context(struct comedi_device *dev);
-static int xilinx_download(struct comedi_device *dev);
-static int reset_board(struct comedi_device *dev);
+struct me4000_board {
+	const char *name;
+	unsigned short device_id;
+	int ao_nchan;
+	int ao_fifo;
+	int ai_nchan;
+	int ai_diff_nchan;
+	int ai_sh_nchan;
+	int ex_trig_analog;
+	int dio_nchan;
+	int has_counter;
+};
 
-static int ai_write_chanlist(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_cmd *cmd);
+static const struct me4000_board me4000_boards[] = {
+	{
+		.name		= "ME-4650",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4650,
+		.ai_nchan	= 16,
+		.dio_nchan	= 32,
+	}, {
+		.name		= "ME-4660",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4660i",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660I,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4660s",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660S,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4660is",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4660IS,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4670",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670,
+		.ao_nchan	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4670i",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670I,
+		.ao_nchan	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4670s",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670S,
+		.ao_nchan	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4670is",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4670IS,
+		.ao_nchan	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4680",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680,
+		.ao_nchan	= 4,
+		.ao_fifo	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4680i",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680I,
+		.ao_nchan	= 4,
+		.ao_fifo	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4680s",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680S,
+		.ao_nchan	= 4,
+		.ao_fifo	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	}, {
+		.name		= "ME-4680is",
+		.device_id	= PCI_DEVICE_ID_MEILHAUS_ME4680IS,
+		.ao_nchan	= 4,
+		.ao_fifo	= 4,
+		.ai_nchan	= 32,
+		.ai_diff_nchan	= 16,
+		.ai_sh_nchan	= 8,
+		.ex_trig_analog	= 1,
+		.dio_nchan	= 32,
+		.has_counter	= 1,
+	},
+};
 
 static const struct comedi_lrange me4000_ai_range = {
 	4,
@@ -114,380 +363,6 @@ static const struct comedi_lrange me4000_ai_range = {
 	 }
 };
 
-static const struct comedi_lrange me4000_ao_range = {
-	1,
-	{
-	 BIP_RANGE(10),
-	 }
-};
-
-static int me4000_probe(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct pci_dev *pci_device = NULL;
-	int result, i;
-	struct me4000_board *board;
-
-	/* Allocate private memory */
-	if (alloc_private(dev, sizeof(struct me4000_info)) < 0)
-		return -ENOMEM;
-
-	/*
-	 * Probe the device to determine what device in the series it is.
-	 */
-	for_each_pci_dev(pci_device) {
-		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
-			for (i = 0; i < ME4000_BOARD_VERSIONS; i++) {
-				if (me4000_boards[i].device_id ==
-				    pci_device->device) {
-					/*
-					 * Was a particular
-					 * bus/slot requested?
-					 */
-					if ((it->options[0] != 0)
-					    || (it->options[1] != 0)) {
-						/*
-						 * Are we on the wrong
-						 * bus/slot?
-						 */
-						if (pci_device->bus->number !=
-						    it->options[0]
-						    ||
-						    PCI_SLOT(pci_device->devfn)
-						    != it->options[1]) {
-							continue;
-						}
-					}
-					dev->board_ptr = me4000_boards + i;
-					board =
-					    (struct me4000_board *)
-					    dev->board_ptr;
-					info->pci_dev_p = pci_device;
-					goto found;
-				}
-			}
-		}
-	}
-
-	printk(KERN_ERR
-	       "comedi%d: me4000: me4000_probe(): "
-	       "No supported board found (req. bus/slot : %d/%d)\n",
-	       dev->minor, it->options[0], it->options[1]);
-	return -ENODEV;
-
-found:
-
-	printk(KERN_INFO
-	       "comedi%d: me4000: me4000_probe(): "
-	       "Found %s at PCI bus %d, slot %d\n",
-	       dev->minor, me4000_boards[i].name, pci_device->bus->number,
-	       PCI_SLOT(pci_device->devfn));
-
-	/* Set data in device structure */
-	dev->board_name = board->name;
-
-	/* Enable PCI device and request regions */
-	result = comedi_pci_enable(pci_device, dev->board_name);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): Cannot enable PCI "
-		       "device and request I/O regions\n", dev->minor);
-		return result;
-	}
-
-	/* Get the PCI base registers */
-	result = get_registers(dev, pci_device);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot get registers\n", dev->minor);
-		return result;
-	}
-	/* Initialize board info */
-	result = init_board_info(dev, pci_device);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot init baord info\n", dev->minor);
-		return result;
-	}
-
-	/* Init analog output context */
-	result = init_ao_context(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot init ao context\n", dev->minor);
-		return result;
-	}
-
-	/* Init analog input context */
-	result = init_ai_context(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot init ai context\n", dev->minor);
-		return result;
-	}
-
-	/* Init digital I/O context */
-	result = init_dio_context(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot init dio context\n", dev->minor);
-		return result;
-	}
-
-	/* Init counter context */
-	result = init_cnt_context(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Cannot init cnt context\n", dev->minor);
-		return result;
-	}
-
-	/* Download the xilinx firmware */
-	result = xilinx_download(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): "
-		       "Can't download firmware\n", dev->minor);
-		return result;
-	}
-
-	/* Make a hardware reset */
-	result = reset_board(dev);
-	if (result) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_probe(): Can't reset board\n",
-		       dev->minor);
-		return result;
-	}
-
-	return 0;
-}
-
-static int get_registers(struct comedi_device *dev, struct pci_dev *pci_dev_p)
-{
-    /*--------------------------- plx regbase -------------------------------*/
-
-	info->plx_regbase = pci_resource_start(pci_dev_p, 1);
-	if (info->plx_regbase == 0) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: get_registers(): "
-		       "PCI base address 1 is not available\n", dev->minor);
-		return -ENODEV;
-	}
-	info->plx_regbase_size = pci_resource_len(pci_dev_p, 1);
-
-    /*--------------------------- me4000 regbase ----------------------------*/
-
-	info->me4000_regbase = pci_resource_start(pci_dev_p, 2);
-	if (info->me4000_regbase == 0) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: get_registers(): "
-		       "PCI base address 2 is not available\n", dev->minor);
-		return -ENODEV;
-	}
-	info->me4000_regbase_size = pci_resource_len(pci_dev_p, 2);
-
-    /*--------------------------- timer regbase ------------------------------*/
-
-	info->timer_regbase = pci_resource_start(pci_dev_p, 3);
-	if (info->timer_regbase == 0) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: get_registers(): "
-		       "PCI base address 3 is not available\n", dev->minor);
-		return -ENODEV;
-	}
-	info->timer_regbase_size = pci_resource_len(pci_dev_p, 3);
-
-    /*--------------------------- program regbase ----------------------------*/
-
-	info->program_regbase = pci_resource_start(pci_dev_p, 5);
-	if (info->program_regbase == 0) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: get_registers(): "
-		       "PCI base address 5 is not available\n", dev->minor);
-		return -ENODEV;
-	}
-	info->program_regbase_size = pci_resource_len(pci_dev_p, 5);
-
-	return 0;
-}
-
-static int init_board_info(struct comedi_device *dev, struct pci_dev *pci_dev_p)
-{
-	int result;
-
-	/* Init spin locks */
-	/* spin_lock_init(&info->preload_lock); */
-	/* spin_lock_init(&info->ai_ctrl_lock); */
-
-	/* Get the serial number */
-	result = pci_read_config_dword(pci_dev_p, 0x2C, &info->serial_no);
-	if (result != PCIBIOS_SUCCESSFUL)
-		return result;
-
-	/* Get the hardware revision */
-	result = pci_read_config_byte(pci_dev_p, 0x08, &info->hw_revision);
-	if (result != PCIBIOS_SUCCESSFUL)
-		return result;
-
-	/* Get the vendor id */
-	info->vendor_id = pci_dev_p->vendor;
-
-	/* Get the device id */
-	info->device_id = pci_dev_p->device;
-
-	/* Get the irq assigned to the board */
-	info->irq = pci_dev_p->irq;
-
-	return 0;
-}
-
-static int init_ao_context(struct comedi_device *dev)
-{
-	int i;
-
-	for (i = 0; i < thisboard->ao.count; i++) {
-		/* spin_lock_init(&info->ao_context[i].use_lock); */
-		info->ao_context[i].irq = info->irq;
-
-		switch (i) {
-		case 0:
-			info->ao_context[i].ctrl_reg =
-			    info->me4000_regbase + ME4000_AO_00_CTRL_REG;
-			info->ao_context[i].status_reg =
-			    info->me4000_regbase + ME4000_AO_00_STATUS_REG;
-			info->ao_context[i].fifo_reg =
-			    info->me4000_regbase + ME4000_AO_00_FIFO_REG;
-			info->ao_context[i].single_reg =
-			    info->me4000_regbase + ME4000_AO_00_SINGLE_REG;
-			info->ao_context[i].timer_reg =
-			    info->me4000_regbase + ME4000_AO_00_TIMER_REG;
-			info->ao_context[i].irq_status_reg =
-			    info->me4000_regbase + ME4000_IRQ_STATUS_REG;
-			info->ao_context[i].preload_reg =
-			    info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
-			break;
-		case 1:
-			info->ao_context[i].ctrl_reg =
-			    info->me4000_regbase + ME4000_AO_01_CTRL_REG;
-			info->ao_context[i].status_reg =
-			    info->me4000_regbase + ME4000_AO_01_STATUS_REG;
-			info->ao_context[i].fifo_reg =
-			    info->me4000_regbase + ME4000_AO_01_FIFO_REG;
-			info->ao_context[i].single_reg =
-			    info->me4000_regbase + ME4000_AO_01_SINGLE_REG;
-			info->ao_context[i].timer_reg =
-			    info->me4000_regbase + ME4000_AO_01_TIMER_REG;
-			info->ao_context[i].irq_status_reg =
-			    info->me4000_regbase + ME4000_IRQ_STATUS_REG;
-			info->ao_context[i].preload_reg =
-			    info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
-			break;
-		case 2:
-			info->ao_context[i].ctrl_reg =
-			    info->me4000_regbase + ME4000_AO_02_CTRL_REG;
-			info->ao_context[i].status_reg =
-			    info->me4000_regbase + ME4000_AO_02_STATUS_REG;
-			info->ao_context[i].fifo_reg =
-			    info->me4000_regbase + ME4000_AO_02_FIFO_REG;
-			info->ao_context[i].single_reg =
-			    info->me4000_regbase + ME4000_AO_02_SINGLE_REG;
-			info->ao_context[i].timer_reg =
-			    info->me4000_regbase + ME4000_AO_02_TIMER_REG;
-			info->ao_context[i].irq_status_reg =
-			    info->me4000_regbase + ME4000_IRQ_STATUS_REG;
-			info->ao_context[i].preload_reg =
-			    info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
-			break;
-		case 3:
-			info->ao_context[i].ctrl_reg =
-			    info->me4000_regbase + ME4000_AO_03_CTRL_REG;
-			info->ao_context[i].status_reg =
-			    info->me4000_regbase + ME4000_AO_03_STATUS_REG;
-			info->ao_context[i].fifo_reg =
-			    info->me4000_regbase + ME4000_AO_03_FIFO_REG;
-			info->ao_context[i].single_reg =
-			    info->me4000_regbase + ME4000_AO_03_SINGLE_REG;
-			info->ao_context[i].timer_reg =
-			    info->me4000_regbase + ME4000_AO_03_TIMER_REG;
-			info->ao_context[i].irq_status_reg =
-			    info->me4000_regbase + ME4000_IRQ_STATUS_REG;
-			info->ao_context[i].preload_reg =
-			    info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
-			break;
-		default:
-			break;
-		}
-	}
-
-	return 0;
-}
-
-static int init_ai_context(struct comedi_device *dev)
-{
-	info->ai_context.irq = info->irq;
-
-	info->ai_context.ctrl_reg = info->me4000_regbase + ME4000_AI_CTRL_REG;
-	info->ai_context.status_reg =
-	    info->me4000_regbase + ME4000_AI_STATUS_REG;
-	info->ai_context.channel_list_reg =
-	    info->me4000_regbase + ME4000_AI_CHANNEL_LIST_REG;
-	info->ai_context.data_reg = info->me4000_regbase + ME4000_AI_DATA_REG;
-	info->ai_context.chan_timer_reg =
-	    info->me4000_regbase + ME4000_AI_CHAN_TIMER_REG;
-	info->ai_context.chan_pre_timer_reg =
-	    info->me4000_regbase + ME4000_AI_CHAN_PRE_TIMER_REG;
-	info->ai_context.scan_timer_low_reg =
-	    info->me4000_regbase + ME4000_AI_SCAN_TIMER_LOW_REG;
-	info->ai_context.scan_timer_high_reg =
-	    info->me4000_regbase + ME4000_AI_SCAN_TIMER_HIGH_REG;
-	info->ai_context.scan_pre_timer_low_reg =
-	    info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_LOW_REG;
-	info->ai_context.scan_pre_timer_high_reg =
-	    info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_HIGH_REG;
-	info->ai_context.start_reg = info->me4000_regbase + ME4000_AI_START_REG;
-	info->ai_context.irq_status_reg =
-	    info->me4000_regbase + ME4000_IRQ_STATUS_REG;
-	info->ai_context.sample_counter_reg =
-	    info->me4000_regbase + ME4000_AI_SAMPLE_COUNTER_REG;
-
-	return 0;
-}
-
-static int init_dio_context(struct comedi_device *dev)
-{
-	info->dio_context.dir_reg = info->me4000_regbase + ME4000_DIO_DIR_REG;
-	info->dio_context.ctrl_reg = info->me4000_regbase + ME4000_DIO_CTRL_REG;
-	info->dio_context.port_0_reg =
-	    info->me4000_regbase + ME4000_DIO_PORT_0_REG;
-	info->dio_context.port_1_reg =
-	    info->me4000_regbase + ME4000_DIO_PORT_1_REG;
-	info->dio_context.port_2_reg =
-	    info->me4000_regbase + ME4000_DIO_PORT_2_REG;
-	info->dio_context.port_3_reg =
-	    info->me4000_regbase + ME4000_DIO_PORT_3_REG;
-
-	return 0;
-}
-
-static int init_cnt_context(struct comedi_device *dev)
-{
-	info->cnt_context.ctrl_reg = info->timer_regbase + ME4000_CNT_CTRL_REG;
-	info->cnt_context.counter_0_reg =
-	    info->timer_regbase + ME4000_CNT_COUNTER_0_REG;
-	info->cnt_context.counter_1_reg =
-	    info->timer_regbase + ME4000_CNT_COUNTER_1_REG;
-	info->cnt_context.counter_2_reg =
-	    info->timer_regbase + ME4000_CNT_COUNTER_2_REG;
-
-	return 0;
-}
-
 #define FIRMWARE_NOT_AVAILABLE 1
 #if FIRMWARE_NOT_AVAILABLE
 extern unsigned char *xilinx_firm;
@@ -495,11 +370,17 @@ extern unsigned char *xilinx_firm;
 
 static int xilinx_download(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct me4000_info *info = dev->private;
+	unsigned long xilinx_iobase = pci_resource_start(pcidev, 5);
 	u32 value = 0;
 	wait_queue_head_t queue;
 	int idx = 0;
 	int size = 0;
 
+	if (!xilinx_iobase)
+		return -ENODEV;
+
 	init_waitqueue_head(&queue);
 
 	/*
@@ -514,14 +395,12 @@ static int xilinx_download(struct comedi_device *dev)
 	outl(value, info->plx_regbase + PLX_ICR);
 
 	/* Init Xilinx with CS1 */
-	inb(info->program_regbase + 0xC8);
+	inb(xilinx_iobase + 0xC8);
 
 	/* Wait until /INIT pin is set */
 	udelay(20);
 	if (!(inl(info->plx_regbase + PLX_INTCSR) & 0x20)) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: xilinx_download(): "
-		       "Can't init Xilinx\n", dev->minor);
+		dev_err(dev->class_dev, "Can't init Xilinx\n");
 		return -EIO;
 	}
 
@@ -530,8 +409,8 @@ static int xilinx_download(struct comedi_device *dev)
 	value &= ~0x100;
 	outl(value, info->plx_regbase + PLX_ICR);
 	if (FIRMWARE_NOT_AVAILABLE) {
-		comedi_error(dev, "xilinx firmware unavailable "
-			     "due to licensing, aborting");
+		dev_err(dev->class_dev,
+			"xilinx firmware unavailable due to licensing, aborting");
 		return -EIO;
 	} else {
 		/* Download Xilinx firmware */
@@ -540,15 +419,14 @@ static int xilinx_download(struct comedi_device *dev)
 		udelay(10);
 
 		for (idx = 0; idx < size; idx++) {
-			outb(xilinx_firm[16 + idx], info->program_regbase);
+			outb(xilinx_firm[16 + idx], xilinx_iobase);
 			udelay(10);
 
 			/* Check if BUSY flag is low */
 			if (inl(info->plx_regbase + PLX_ICR) & 0x20) {
-				printk(KERN_ERR
-				       "comedi%d: me4000: xilinx_download(): "
-				       "Xilinx is still busy (idx = %d)\n",
-				       dev->minor, idx);
+				dev_err(dev->class_dev,
+					"Xilinx is still busy (idx = %d)\n",
+					idx);
 				return -EIO;
 			}
 		}
@@ -557,12 +435,8 @@ static int xilinx_download(struct comedi_device *dev)
 	/* If done flag is high download was successful */
 	if (inl(info->plx_regbase + PLX_ICR) & 0x4) {
 	} else {
-		printk(KERN_ERR
-		       "comedi%d: me4000: xilinx_download(): "
-		       "DONE flag is not set\n", dev->minor);
-		printk(KERN_ERR
-		       "comedi%d: me4000: xilinx_download(): "
-		       "Download not successful\n", dev->minor);
+		dev_err(dev->class_dev, "DONE flag is not set\n");
+		dev_err(dev->class_dev, "Download not successful\n");
 		return -EIO;
 	}
 
@@ -574,52 +448,45 @@ static int xilinx_download(struct comedi_device *dev)
 	return 0;
 }
 
-static int reset_board(struct comedi_device *dev)
+static void me4000_reset(struct comedi_device *dev)
 {
-	unsigned long icr;
+	struct me4000_info *info = dev->private;
+	unsigned long val;
+	int chan;
 
 	/* Make a hardware reset */
-	icr = inl(info->plx_regbase + PLX_ICR);
-	icr |= 0x40000000;
-	outl(icr, info->plx_regbase + PLX_ICR);
-	icr &= ~0x40000000;
-	outl(icr, info->plx_regbase + PLX_ICR);
+	val = inl(info->plx_regbase + PLX_ICR);
+	val |= 0x40000000;
+	outl(val, info->plx_regbase + PLX_ICR);
+	val &= ~0x40000000;
+	outl(val , info->plx_regbase + PLX_ICR);
 
 	/* 0x8000 to the DACs means an output voltage of 0V */
-	outl(0x8000, info->me4000_regbase + ME4000_AO_00_SINGLE_REG);
-	outl(0x8000, info->me4000_regbase + ME4000_AO_01_SINGLE_REG);
-	outl(0x8000, info->me4000_regbase + ME4000_AO_02_SINGLE_REG);
-	outl(0x8000, info->me4000_regbase + ME4000_AO_03_SINGLE_REG);
+	for (chan = 0; chan < 4; chan++)
+		outl(0x8000, dev->iobase + ME4000_AO_SINGLE_REG(chan));
 
 	/* Set both stop bits in the analog input control register */
 	outl(ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
-		info->me4000_regbase + ME4000_AI_CTRL_REG);
+		dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Set both stop bits in the analog output control register */
-	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		info->me4000_regbase + ME4000_AO_00_CTRL_REG);
-	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		info->me4000_regbase + ME4000_AO_01_CTRL_REG);
-	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		info->me4000_regbase + ME4000_AO_02_CTRL_REG);
-	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		info->me4000_regbase + ME4000_AO_03_CTRL_REG);
+	val = ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP;
+	for (chan = 0; chan < 4; chan++)
+		outl(val, dev->iobase + ME4000_AO_CTRL_REG(chan));
 
 	/* Enable interrupts on the PLX */
 	outl(0x43, info->plx_regbase + PLX_INTCSR);
 
 	/* Set the adustment register for AO demux */
 	outl(ME4000_AO_DEMUX_ADJUST_VALUE,
-		    info->me4000_regbase + ME4000_AO_DEMUX_ADJUST_REG);
+		    dev->iobase + ME4000_AO_DEMUX_ADJUST_REG);
 
 	/*
 	 * Set digital I/O direction for port 0
 	 * to output on isolated versions
 	 */
-	if (!(inl(info->me4000_regbase + ME4000_DIO_DIR_REG) & 0x1))
-		outl(0x1, info->me4000_regbase + ME4000_DIO_CTRL_REG);
-
-	return 0;
+	if (!(inl(dev->iobase + ME4000_DIO_DIR_REG) & 0x1))
+		outl(0x1, dev->iobase + ME4000_DIO_CTRL_REG);
 }
 
 /*=============================================================================
@@ -630,7 +497,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *subdevice,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-
+	const struct me4000_board *thisboard = comedi_board(dev);
 	int chan = CR_CHAN(insn->chanspec);
 	int rang = CR_RANGE(insn->chanspec);
 	int aref = CR_AREF(insn->chanspec);
@@ -642,9 +509,8 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_insn_read(): "
-		       "Invalid instruction length %d\n", dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid instruction length %d\n",
+			insn->n);
 		return -EINVAL;
 	}
 
@@ -662,19 +528,16 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 		entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
 		break;
 	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_insn_read(): "
-		       "Invalid range specified\n", dev->minor);
+		dev_err(dev->class_dev, "Invalid range specified\n");
 		return -EINVAL;
 	}
 
 	switch (aref) {
 	case AREF_GROUND:
 	case AREF_COMMON:
-		if (chan >= thisboard->ai.count) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_insn_read(): "
-			       "Analog input is not available\n", dev->minor);
+		if (chan >= thisboard->ai_nchan) {
+			dev_err(dev->class_dev,
+				"Analog input is not available\n");
 			return -EINVAL;
 		}
 		entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED | chan;
@@ -682,68 +545,61 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 
 	case AREF_DIFF:
 		if (rang == 0 || rang == 1) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_insn_read(): "
-			       "Range must be bipolar when aref = diff\n",
-			       dev->minor);
+			dev_err(dev->class_dev,
+				"Range must be bipolar when aref = diff\n");
 			return -EINVAL;
 		}
 
-		if (chan >= thisboard->ai.diff_count) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_insn_read(): "
-			       "Analog input is not available\n", dev->minor);
+		if (chan >= thisboard->ai_diff_nchan) {
+			dev_err(dev->class_dev,
+				"Analog input is not available\n");
 			return -EINVAL;
 		}
 		entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL | chan;
 		break;
 	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_insn_read(): "
-		       "Invalid aref specified\n", dev->minor);
+		dev_err(dev->class_dev, "Invalid aref specified\n");
 		return -EINVAL;
 	}
 
 	entry |= ME4000_AI_LIST_LAST_ENTRY;
 
 	/* Clear channel list, data fifo and both stop bits */
-	tmp = inl(info->ai_context.ctrl_reg);
+	tmp = inl(dev->iobase + ME4000_AI_CTRL_REG);
 	tmp &= ~(ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
 		 ME4000_AI_CTRL_BIT_DATA_FIFO |
 		 ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Set the acquisition mode to single */
 	tmp &= ~(ME4000_AI_CTRL_BIT_MODE_0 | ME4000_AI_CTRL_BIT_MODE_1 |
 		 ME4000_AI_CTRL_BIT_MODE_2);
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Enable channel list and data fifo */
 	tmp |= ME4000_AI_CTRL_BIT_CHANNEL_FIFO | ME4000_AI_CTRL_BIT_DATA_FIFO;
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Generate channel list entry */
-	outl(entry, info->ai_context.channel_list_reg);
+	outl(entry, dev->iobase + ME4000_AI_CHANNEL_LIST_REG);
 
 	/* Set the timer to maximum sample rate */
-	outl(ME4000_AI_MIN_TICKS, info->ai_context.chan_timer_reg);
-	outl(ME4000_AI_MIN_TICKS, info->ai_context.chan_pre_timer_reg);
+	outl(ME4000_AI_MIN_TICKS, dev->iobase + ME4000_AI_CHAN_TIMER_REG);
+	outl(ME4000_AI_MIN_TICKS, dev->iobase + ME4000_AI_CHAN_PRE_TIMER_REG);
 
 	/* Start conversion by dummy read */
-	inl(info->ai_context.start_reg);
+	inl(dev->iobase + ME4000_AI_START_REG);
 
 	/* Wait until ready */
 	udelay(10);
-	if (!(inl(info->ai_context.status_reg) &
+	if (!(inl(dev->iobase + ME4000_AI_STATUS_REG) &
 	     ME4000_AI_STATUS_BIT_EF_DATA)) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_insn_read(): "
-		       "Value not available after wait\n", dev->minor);
+		dev_err(dev->class_dev, "Value not available after wait\n");
 		return -EIO;
 	}
 
 	/* Read value from data fifo */
-	lval = inl(info->ai_context.data_reg) & 0xFFFF;
+	lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 	data[0] = lval ^ 0x8000;
 
 	return 1;
@@ -755,12 +611,12 @@ static int me4000_ai_cancel(struct comedi_device *dev,
 	unsigned long tmp;
 
 	/* Stop any running conversion */
-	tmp = inl(info->ai_context.ctrl_reg);
+	tmp = inl(dev->iobase + ME4000_AI_CTRL_REG);
 	tmp &= ~(ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Clear the control register */
-	outl(0x0, info->ai_context.ctrl_reg);
+	outl(0x0, dev->iobase + ME4000_AI_CTRL_REG);
 
 	return 0;
 }
@@ -768,30 +624,25 @@ static int me4000_ai_cancel(struct comedi_device *dev,
 static int ai_check_chanlist(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct me4000_board *thisboard = comedi_board(dev);
 	int aref;
 	int i;
 
 	/* Check whether a channel list is available */
 	if (!cmd->chanlist_len) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: ai_check_chanlist(): "
-		       "No channel list available\n", dev->minor);
+		dev_err(dev->class_dev, "No channel list available\n");
 		return -EINVAL;
 	}
 
 	/* Check the channel list size */
 	if (cmd->chanlist_len > ME4000_AI_CHANNEL_LIST_COUNT) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: ai_check_chanlist(): "
-		       "Channel list is to large\n", dev->minor);
+		dev_err(dev->class_dev, "Channel list is to large\n");
 		return -EINVAL;
 	}
 
 	/* Check the pointer */
 	if (!cmd->chanlist) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: ai_check_chanlist(): "
-		       "NULL pointer to channel list\n", dev->minor);
+		dev_err(dev->class_dev, "NULL pointer to channel list\n");
 		return -EFAULT;
 	}
 
@@ -799,10 +650,8 @@ static int ai_check_chanlist(struct comedi_device *dev,
 	aref = CR_AREF(cmd->chanlist[0]);
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		if (CR_AREF(cmd->chanlist[i]) != aref) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: ai_check_chanlist(): "
-			       "Mode is not equal for all entries\n",
-			       dev->minor);
+			dev_err(dev->class_dev,
+				"Mode is not equal for all entries\n");
 			return -EINVAL;
 		}
 	}
@@ -811,19 +660,17 @@ static int ai_check_chanlist(struct comedi_device *dev,
 	if (aref == SDF_DIFF) {
 		for (i = 0; i < cmd->chanlist_len; i++) {
 			if (CR_CHAN(cmd->chanlist[i]) >=
-			    thisboard->ai.diff_count) {
-				printk(KERN_ERR
-				       "comedi%d: me4000: ai_check_chanlist():"
-				       " Channel number to high\n", dev->minor);
+			    thisboard->ai_diff_nchan) {
+				dev_err(dev->class_dev,
+					"Channel number to high\n");
 				return -EINVAL;
 			}
 		}
 	} else {
 		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai.count) {
-				printk(KERN_ERR
-				       "comedi%d: me4000: ai_check_chanlist(): "
-				       "Channel number to high\n", dev->minor);
+			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai_nchan) {
+				dev_err(dev->class_dev,
+					"Channel number to high\n");
 				return -EINVAL;
 			}
 		}
@@ -834,11 +681,8 @@ static int ai_check_chanlist(struct comedi_device *dev,
 		for (i = 0; i < cmd->chanlist_len; i++) {
 			if (CR_RANGE(cmd->chanlist[i]) != 1 &&
 			    CR_RANGE(cmd->chanlist[i]) != 2) {
-				printk(KERN_ERR
-				       "comedi%d: me4000: ai_check_chanlist(): "
-				       "Bipolar is not selected in "
-				       "differential mode\n",
-				       dev->minor);
+				dev_err(dev->class_dev,
+				       "Bipolar is not selected in differential mode\n");
 				return -EINVAL;
 			}
 		}
@@ -906,16 +750,52 @@ static void ai_write_timer(struct comedi_device *dev,
 			   unsigned int init_ticks,
 			   unsigned int scan_ticks, unsigned int chan_ticks)
 {
-	outl(init_ticks - 1, info->ai_context.scan_pre_timer_low_reg);
-	outl(0x0, info->ai_context.scan_pre_timer_high_reg);
+	outl(init_ticks - 1, dev->iobase + ME4000_AI_SCAN_PRE_TIMER_LOW_REG);
+	outl(0x0, dev->iobase + ME4000_AI_SCAN_PRE_TIMER_HIGH_REG);
 
 	if (scan_ticks) {
-		outl(scan_ticks - 1, info->ai_context.scan_timer_low_reg);
-		outl(0x0, info->ai_context.scan_timer_high_reg);
+		outl(scan_ticks - 1, dev->iobase + ME4000_AI_SCAN_TIMER_LOW_REG);
+		outl(0x0, dev->iobase + ME4000_AI_SCAN_TIMER_HIGH_REG);
 	}
 
-	outl(chan_ticks - 1, info->ai_context.chan_pre_timer_reg);
-	outl(chan_ticks - 1, info->ai_context.chan_timer_reg);
+	outl(chan_ticks - 1, dev->iobase + ME4000_AI_CHAN_PRE_TIMER_REG);
+	outl(chan_ticks - 1, dev->iobase + ME4000_AI_CHAN_TIMER_REG);
+}
+
+static int ai_write_chanlist(struct comedi_device *dev,
+			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
+{
+	unsigned int entry;
+	unsigned int chan;
+	unsigned int rang;
+	unsigned int aref;
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		rang = CR_RANGE(cmd->chanlist[i]);
+		aref = CR_AREF(cmd->chanlist[i]);
+
+		entry = chan;
+
+		if (rang == 0)
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
+		else if (rang == 1)
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
+		else if (rang == 2)
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
+		else
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
+
+		if (aref == SDF_DIFF)
+			entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL;
+		else
+			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
+
+		outl(entry, dev->iobase + ME4000_AI_CHANNEL_LIST_REG);
+	}
+
+	return 0;
 }
 
 static int ai_prepare(struct comedi_device *dev,
@@ -931,7 +811,7 @@ static int ai_prepare(struct comedi_device *dev,
 	ai_write_timer(dev, init_ticks, scan_ticks, chan_ticks);
 
 	/* Reset control register */
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Start sources */
 	if ((cmd->start_src == TRIG_EXT &&
@@ -965,19 +845,19 @@ static int ai_prepare(struct comedi_device *dev,
 	/* Stop triggers */
 	if (cmd->stop_src == TRIG_COUNT) {
 		outl(cmd->chanlist_len * cmd->stop_arg,
-			    info->ai_context.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,
-			    info->ai_context.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;
 	}
 
 	/* Write the setup to the control register */
-	outl(tmp, info->ai_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 	/* Write the channel list */
 	ai_write_chanlist(dev, s, cmd);
@@ -985,42 +865,6 @@ static int ai_prepare(struct comedi_device *dev,
 	return 0;
 }
 
-static int ai_write_chanlist(struct comedi_device *dev,
-			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
-{
-	unsigned int entry;
-	unsigned int chan;
-	unsigned int rang;
-	unsigned int aref;
-	int i;
-
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		chan = CR_CHAN(cmd->chanlist[i]);
-		rang = CR_RANGE(cmd->chanlist[i]);
-		aref = CR_AREF(cmd->chanlist[i]);
-
-		entry = chan;
-
-		if (rang == 0)
-			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
-		else if (rang == 1)
-			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
-		else if (rang == 2)
-			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
-		else
-			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
-
-		if (aref == SDF_DIFF)
-			entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL;
-		else
-			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
-
-		outl(entry, info->ai_context.channel_list_reg);
-	}
-
-	return 0;
-}
-
 static int me4000_ai_do_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -1047,23 +891,11 @@ static int me4000_ai_do_cmd(struct comedi_device *dev,
 		return err;
 
 	/* Start acquistion by dummy read */
-	inl(info->ai_context.start_reg);
+	inl(dev->iobase + ME4000_AI_START_REG);
 
 	return 0;
 }
 
-/*
- * me4000_ai_do_cmd_test():
- *
- * The demo cmd.c in ./comedilib/demo specifies 6 return values:
- * - success
- * - invalid source
- * - source conflict
- * - invalid argument
- * - argument conflict
- * - invalid chanlist
- * So I tried to adopt this scheme.
- */
 static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
@@ -1080,91 +912,29 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	/* Round the timer arguments */
 	ai_round_cmd_args(dev, s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
 
-	/*
-	 * Stage 1. Check if the trigger sources are generally valid.
-	 */
-	switch (cmd->start_src) {
-	case TRIG_NOW:
-	case TRIG_EXT:
-		break;
-	case TRIG_ANY:
-		cmd->start_src &= TRIG_NOW | TRIG_EXT;
-		err++;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid start source\n", dev->minor);
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
-	switch (cmd->scan_begin_src) {
-	case TRIG_FOLLOW:
-	case TRIG_TIMER:
-	case TRIG_EXT:
-		break;
-	case TRIG_ANY:
-		cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT;
-		err++;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid scan begin source\n", dev->minor);
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
-	switch (cmd->convert_src) {
-	case TRIG_TIMER:
-	case TRIG_EXT:
-		break;
-	case TRIG_ANY:
-		cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-		err++;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid convert source\n", dev->minor);
-		cmd->convert_src = TRIG_TIMER;
-		err++;
-	}
-	switch (cmd->scan_end_src) {
-	case TRIG_NONE:
-	case TRIG_COUNT:
-		break;
-	case TRIG_ANY:
-		cmd->scan_end_src &= TRIG_NONE | TRIG_COUNT;
-		err++;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid scan end source\n", dev->minor);
-		cmd->scan_end_src = TRIG_NONE;
-		err++;
-	}
-	switch (cmd->stop_src) {
-	case TRIG_NONE:
-	case TRIG_COUNT:
-		break;
-	case TRIG_ANY:
-		cmd->stop_src &= TRIG_NONE | TRIG_COUNT;
-		err++;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid stop source\n", dev->minor);
-		cmd->stop_src = TRIG_NONE;
-		err++;
-	}
+	/* 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 | 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_NONE | TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE | TRIG_COUNT);
+
 	if (err)
 		return 1;
 
-	/*
-	 * Stage 2. Check for trigger source conflicts.
-	 */
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
+
 	if (cmd->start_src == TRIG_NOW &&
 	    cmd->scan_begin_src == TRIG_TIMER &&
 	    cmd->convert_src == TRIG_TIMER) {
@@ -1184,13 +954,7 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 		   cmd->scan_begin_src == TRIG_EXT &&
 		   cmd->convert_src == TRIG_EXT) {
 	} else {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid start trigger combination\n", dev->minor);
-		cmd->start_src = TRIG_NOW;
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		cmd->convert_src = TRIG_TIMER;
-		err++;
+		err |= -EINVAL;
 	}
 
 	if (cmd->stop_src == TRIG_NONE && cmd->scan_end_src == TRIG_NONE) {
@@ -1201,13 +965,9 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	} else if (cmd->stop_src == TRIG_COUNT &&
 		   cmd->scan_end_src == TRIG_COUNT) {
 	} else {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Invalid stop trigger combination\n", dev->minor);
-		cmd->stop_src = TRIG_NONE;
-		cmd->scan_end_src = TRIG_NONE;
-		err++;
+		err |= -EINVAL;
 	}
+
 	if (err)
 		return 2;
 
@@ -1215,30 +975,22 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	 * Stage 3. Check if arguments are generally valid.
 	 */
 	if (cmd->chanlist_len < 1) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "No channel list\n", dev->minor);
+		dev_err(dev->class_dev, "No channel list\n");
 		cmd->chanlist_len = 1;
 		err++;
 	}
 	if (init_ticks < 66) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Start arg to low\n", dev->minor);
+		dev_err(dev->class_dev, "Start arg to low\n");
 		cmd->start_arg = 2000;
 		err++;
 	}
 	if (scan_ticks && scan_ticks < 67) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Scan begin arg to low\n", dev->minor);
+		dev_err(dev->class_dev, "Scan begin arg to low\n");
 		cmd->scan_begin_arg = 2031;
 		err++;
 	}
 	if (chan_ticks < 66) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-		       "Convert arg to low\n", dev->minor);
+		dev_err(dev->class_dev, "Convert arg to low\n");
 		cmd->convert_arg = 2000;
 		err++;
 	}
@@ -1255,23 +1007,17 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (chan_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid convert arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid convert arg\n");
 			cmd->convert_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid scan end arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid scan end arg\n");
 
 			/*  At least one tick more */
 			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;
@@ -1283,16 +1029,12 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (chan_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid convert arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid convert arg\n");
 			cmd->convert_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
@@ -1302,23 +1044,17 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (chan_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid convert arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid convert arg\n");
 			cmd->convert_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid scan end arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid scan end arg\n");
 
 			/*  At least one tick more */
 			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;
@@ -1330,16 +1066,12 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (chan_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid convert arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid convert arg\n");
 			cmd->convert_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
@@ -1349,16 +1081,12 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 		if (chan_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid convert arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid convert arg\n");
 			cmd->convert_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
@@ -1368,27 +1096,21 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 
 		/* Check timer arguments */
 		if (init_ticks < ME4000_AI_MIN_TICKS) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid start arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid start arg\n");
 			cmd->start_arg = 2000;	/*  66 ticks at least */
 			err++;
 		}
 	}
 	if (cmd->stop_src == TRIG_COUNT) {
 		if (cmd->stop_arg == 0) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid stop arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid stop arg\n");
 			cmd->stop_arg = 1;
 			err++;
 		}
 	}
 	if (cmd->scan_end_src == TRIG_COUNT) {
 		if (cmd->scan_end_arg == 0) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_do_cmd_test(): "
-			       "Invalid scan end arg\n", dev->minor);
+			dev_err(dev->class_dev, "Invalid scan end arg\n");
 			cmd->scan_end_arg = 1;
 			err++;
 		}
@@ -1410,8 +1132,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 {
 	unsigned int tmp;
 	struct comedi_device *dev = dev_id;
-	struct comedi_subdevice *s = dev->subdevices;
-	struct me4000_ai_context *ai_context = &info->ai_context;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int i;
 	int c = 0;
 	long lval;
@@ -1423,17 +1144,15 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 	s->async->events = 0;
 
 	/* Check if irq number is right */
-	if (irq != ai_context->irq) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ai_isr(): "
-		       "Incorrect interrupt num: %d\n", dev->minor, irq);
+	if (irq != dev->irq) {
+		dev_err(dev->class_dev, "Incorrect interrupt num: %d\n", irq);
 		return IRQ_HANDLED;
 	}
 
-	if (inl(ai_context->irq_status_reg) &
+	if (inl(dev->iobase + ME4000_IRQ_STATUS_REG) &
 	    ME4000_IRQ_STATUS_BIT_AI_HF) {
 		/* Read status register to find out what happened */
-		tmp = inl(ai_context->ctrl_reg);
+		tmp = inl(dev->iobase + ME4000_AI_CTRL_REG);
 
 		if (!(tmp & ME4000_AI_STATUS_BIT_FF_DATA) &&
 		    !(tmp & ME4000_AI_STATUS_BIT_HF_DATA) &&
@@ -1447,13 +1166,11 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 				 ME4000_AI_CTRL_BIT_SC_IRQ);
-			outl(tmp, ai_context->ctrl_reg);
+			outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_isr(): "
-			       "FIFO overflow\n", dev->minor);
+			dev_err(dev->class_dev, "FIFO overflow\n");
 		} else if ((tmp & ME4000_AI_STATUS_BIT_FF_DATA)
 			   && !(tmp & ME4000_AI_STATUS_BIT_HF_DATA)
 			   && (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
@@ -1461,9 +1178,8 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 
 			c = ME4000_AI_FIFO_COUNT / 2;
 		} else {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_isr(): "
-			       "Can't determine state of fifo\n", dev->minor);
+			dev_err(dev->class_dev,
+				"Can't determine state of fifo\n");
 			c = 0;
 
 			/*
@@ -1473,18 +1189,16 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 				 ME4000_AI_CTRL_BIT_SC_IRQ);
-			outl(tmp, ai_context->ctrl_reg);
+			outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_ai_isr(): "
-			       "Undefined FIFO state\n", dev->minor);
+			dev_err(dev->class_dev, "Undefined FIFO state\n");
 		}
 
 		for (i = 0; i < c; i++) {
 			/* Read value from data fifo */
-			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
 			if (!comedi_buf_put(s->async, lval)) {
@@ -1495,13 +1209,11 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 				tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 				tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 					 ME4000_AI_CTRL_BIT_SC_IRQ);
-				outl(tmp, ai_context->ctrl_reg);
+				outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 				s->async->events |= COMEDI_CB_OVERFLOW;
 
-				printk(KERN_ERR
-				       "comedi%d: me4000: me4000_ai_isr(): "
-				       "Buffer overflow\n", dev->minor);
+				dev_err(dev->class_dev, "Buffer overflow\n");
 
 				break;
 			}
@@ -1509,33 +1221,33 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 
 		/* Work is done, so reset the interrupt */
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
-		outl(tmp, ai_context->ctrl_reg);
+		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 		tmp &= ~ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
-		outl(tmp, ai_context->ctrl_reg);
+		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 	}
 
-	if (inl(ai_context->irq_status_reg) & ME4000_IRQ_STATUS_BIT_SC) {
+	if (inl(dev->iobase + ME4000_IRQ_STATUS_REG) &
+	    ME4000_IRQ_STATUS_BIT_SC) {
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOA;
 
 		/*
 		 * Acquisition is complete, so stop
 		 * conversion and disable all interrupts
 		 */
-		tmp = inl(ai_context->ctrl_reg);
+		tmp = inl(dev->iobase + ME4000_AI_CTRL_REG);
 		tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 		tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ);
-		outl(tmp, ai_context->ctrl_reg);
+		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 
 		/* Poll data until fifo empty */
-		while (inl(ai_context->ctrl_reg) & ME4000_AI_STATUS_BIT_EF_DATA) {
+		while (inl(dev->iobase + ME4000_AI_CTRL_REG) &
+		       ME4000_AI_STATUS_BIT_EF_DATA) {
 			/* Read value from data fifo */
-			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
 			if (!comedi_buf_put(s->async, lval)) {
-				printk(KERN_ERR
-				       "comedi%d: me4000: me4000_ai_isr(): "
-				       "Buffer overflow\n", dev->minor);
+				dev_err(dev->class_dev, "Buffer overflow\n");
 				s->async->events |= COMEDI_CB_OVERFLOW;
 				break;
 			}
@@ -1543,9 +1255,9 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 
 		/* Work is done, so reset the interrupt */
 		tmp |= ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
-		outl(tmp, ai_context->ctrl_reg);
+		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 		tmp &= ~ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
-		outl(tmp, ai_context->ctrl_reg);
+		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 	}
 
 	if (s->async->events)
@@ -1562,7 +1274,8 @@ static int me4000_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-
+	const struct me4000_board *thisboard = comedi_board(dev);
+	struct me4000_info *info = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 	int rang = CR_RANGE(insn->chanspec);
 	int aref = CR_AREF(insn->chanspec);
@@ -1571,46 +1284,39 @@ static int me4000_ao_insn_write(struct comedi_device *dev,
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ao_insn_write(): "
-		       "Invalid instruction length %d\n", dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid instruction length %d\n",
+			insn->n);
 		return -EINVAL;
 	}
 
-	if (chan >= thisboard->ao.count) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ao_insn_write(): "
-		       "Invalid channel %d\n", dev->minor, insn->n);
+	if (chan >= thisboard->ao_nchan) {
+		dev_err(dev->class_dev, "Invalid channel %d\n", insn->n);
 		return -EINVAL;
 	}
 
 	if (rang != 0) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ao_insn_write(): "
-		       "Invalid range %d\n", dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid range %d\n", insn->n);
 		return -EINVAL;
 	}
 
 	if (aref != AREF_GROUND && aref != AREF_COMMON) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_ao_insn_write(): "
-		       "Invalid aref %d\n", dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid aref %d\n", insn->n);
 		return -EINVAL;
 	}
 
 	/* Stop any running conversion */
-	tmp = inl(info->ao_context[chan].ctrl_reg);
+	tmp = inl(dev->iobase + ME4000_AO_CTRL_REG(chan));
 	tmp |= ME4000_AO_CTRL_BIT_IMMEDIATE_STOP;
-	outl(tmp, info->ao_context[chan].ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_AO_CTRL_REG(chan));
 
 	/* Clear control register and set to single mode */
-	outl(0x0, info->ao_context[chan].ctrl_reg);
+	outl(0x0, dev->iobase + ME4000_AO_CTRL_REG(chan));
 
 	/* Write data value */
-	outl(data[0], info->ao_context[chan].single_reg);
+	outl(data[0], dev->iobase + ME4000_AO_SINGLE_REG(chan));
 
 	/* Store in the mirror */
-	info->ao_context[chan].mirror = data[0];
+	info->ao_readback[chan] = data[0];
 
 	return 1;
 }
@@ -1619,18 +1325,17 @@ static int me4000_ao_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct me4000_info *info = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
-		printk
-		    ("comedi%d: me4000: me4000_ao_insn_read(): "
-		     "Invalid instruction length\n", dev->minor);
+		dev_err(dev->class_dev, "Invalid instruction length\n");
 		return -EINVAL;
 	}
 
-	data[0] = info->ao_context[chan].mirror;
+	data[0] = info->ao_readback[chan];
 
 	return 1;
 }
@@ -1659,21 +1364,21 @@ static int me4000_dio_insn_bits(struct comedi_device *dev,
 
 		/* Write out the new digital output lines */
 		outl((s->state >> 0) & 0xFF,
-			    info->dio_context.port_0_reg);
+			    dev->iobase + ME4000_DIO_PORT_0_REG);
 		outl((s->state >> 8) & 0xFF,
-			    info->dio_context.port_1_reg);
+			    dev->iobase + ME4000_DIO_PORT_1_REG);
 		outl((s->state >> 16) & 0xFF,
-			    info->dio_context.port_2_reg);
+			    dev->iobase + ME4000_DIO_PORT_2_REG);
 		outl((s->state >> 24) & 0xFF,
-			    info->dio_context.port_3_reg);
+			    dev->iobase + ME4000_DIO_PORT_3_REG);
 	}
 
 	/* On return, data[1] contains the value of
 	   the digital input and output lines. */
-	data[1] = ((inl(info->dio_context.port_0_reg) & 0xFF) << 0) |
-		  ((inl(info->dio_context.port_1_reg) & 0xFF) << 8) |
-		  ((inl(info->dio_context.port_2_reg) & 0xFF) << 16) |
-		  ((inl(info->dio_context.port_3_reg) & 0xFF) << 24);
+	data[1] = ((inl(dev->iobase + ME4000_DIO_PORT_0_REG) & 0xFF) << 0) |
+		  ((inl(dev->iobase + ME4000_DIO_PORT_1_REG) & 0xFF) << 8) |
+		  ((inl(dev->iobase + ME4000_DIO_PORT_2_REG) & 0xFF) << 16) |
+		  ((inl(dev->iobase + ME4000_DIO_PORT_3_REG) & 0xFF) << 24);
 
 	return insn->n;
 }
@@ -1705,7 +1410,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 	 * On the ME-4000 it is only possible to switch port wise (8 bit)
 	 */
 
-	tmp = inl(info->dio_context.ctrl_reg);
+	tmp = inl(dev->iobase + ME4000_DIO_CTRL_REG);
 
 	if (data[0] == INSN_CONFIG_DIO_OUTPUT) {
 		if (chan < 8) {
@@ -1719,7 +1424,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 			 * If one the first port is a fixed output
 			 * port and the second is a fixed input port.
 			 */
-			if (!inl(info->dio_context.dir_reg))
+			if (!inl(dev->iobase + ME4000_DIO_DIR_REG))
 				return -ENODEV;
 
 			s->io_bits |= 0xFF00;
@@ -1746,7 +1451,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 			 * If one the first port is a fixed output
 			 * port and the second is a fixed input port.
 			 */
-			if (!inl(info->dio_context.dir_reg))
+			if (!inl(dev->iobase + ME4000_DIO_DIR_REG))
 				return -ENODEV;
 
 			s->io_bits &= ~0xFF;
@@ -1769,7 +1474,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 		}
 	}
 
-	outl(tmp, info->dio_context.ctrl_reg);
+	outl(tmp, dev->iobase + ME4000_DIO_CTRL_REG);
 
 	return 1;
 }
@@ -1778,177 +1483,56 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
   Counter section
   ===========================================================================*/
 
-static int cnt_reset(struct comedi_device *dev, unsigned int channel)
-{
-	switch (channel) {
-	case 0:
-		outb(0x30, info->cnt_context.ctrl_reg);
-		outb(0x00, info->cnt_context.counter_0_reg);
-		outb(0x00, info->cnt_context.counter_0_reg);
-		break;
-	case 1:
-		outb(0x70, info->cnt_context.ctrl_reg);
-		outb(0x00, info->cnt_context.counter_1_reg);
-		outb(0x00, info->cnt_context.counter_1_reg);
-		break;
-	case 2:
-		outb(0xB0, info->cnt_context.ctrl_reg);
-		outb(0x00, info->cnt_context.counter_2_reg);
-		outb(0x00, info->cnt_context.counter_2_reg);
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: cnt_reset(): Invalid channel\n",
-		       dev->minor);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int cnt_config(struct comedi_device *dev, unsigned int channel,
-		      unsigned int mode)
-{
-	int tmp = 0;
-
-	switch (channel) {
-	case 0:
-		tmp |= ME4000_CNT_COUNTER_0;
-		break;
-	case 1:
-		tmp |= ME4000_CNT_COUNTER_1;
-		break;
-	case 2:
-		tmp |= ME4000_CNT_COUNTER_2;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: cnt_config(): Invalid channel\n",
-		       dev->minor);
-		return -EINVAL;
-	}
-
-	switch (mode) {
-	case 0:
-		tmp |= ME4000_CNT_MODE_0;
-		break;
-	case 1:
-		tmp |= ME4000_CNT_MODE_1;
-		break;
-	case 2:
-		tmp |= ME4000_CNT_MODE_2;
-		break;
-	case 3:
-		tmp |= ME4000_CNT_MODE_3;
-		break;
-	case 4:
-		tmp |= ME4000_CNT_MODE_4;
-		break;
-	case 5:
-		tmp |= ME4000_CNT_MODE_5;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: cnt_config(): Invalid counter mode\n",
-		       dev->minor);
-		return -EINVAL;
-	}
-
-	/* Write the control word */
-	tmp |= 0x30;
-	outb(tmp, info->cnt_context.ctrl_reg);
-
-	return 0;
-}
-
 static int me4000_cnt_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-
+	struct me4000_info *info = dev->private;
 	int err;
 
 	switch (data[0]) {
 	case GPCT_RESET:
-		if (insn->n != 1) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_cnt_insn_config(): "
-			       "Invalid instruction length%d\n",
-			       dev->minor, insn->n);
+		if (insn->n != 1)
 			return -EINVAL;
-		}
 
-		err = cnt_reset(dev, insn->chanspec);
+		err = i8254_load(info->timer_regbase, 0, insn->chanspec, 0,
+				I8254_MODE0 | I8254_BINARY);
 		if (err)
 			return err;
 		break;
 	case GPCT_SET_OPERATION:
-		if (insn->n != 2) {
-			printk(KERN_ERR
-			       "comedi%d: me4000: me4000_cnt_insn_config(): "
-			       "Invalid instruction length%d\n",
-			       dev->minor, insn->n);
+		if (insn->n != 2)
 			return -EINVAL;
-		}
 
-		err = cnt_config(dev, insn->chanspec, data[1]);
+		err = i8254_set_mode(info->timer_regbase, 0, insn->chanspec,
+				(data[1] << 1) | I8254_BINARY);
 		if (err)
 			return err;
 		break;
 	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_cnt_insn_config(): "
-		       "Invalid instruction\n", dev->minor);
 		return -EINVAL;
 	}
 
-	return 2;
+	return insn->n;
 }
 
 static int me4000_cnt_insn_read(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-
-	unsigned short tmp;
+	struct me4000_info *info = dev->private;
 
 	if (insn->n == 0)
 		return 0;
 
 	if (insn->n > 1) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_cnt_insn_read(): "
-		       "Invalid instruction length %d\n",
-		       dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid instruction length %d\n",
+			insn->n);
 		return -EINVAL;
 	}
 
-	switch (insn->chanspec) {
-	case 0:
-		tmp = inb(info->cnt_context.counter_0_reg);
-		data[0] = tmp;
-		tmp = inb(info->cnt_context.counter_0_reg);
-		data[0] |= tmp << 8;
-		break;
-	case 1:
-		tmp = inb(info->cnt_context.counter_1_reg);
-		data[0] = tmp;
-		tmp = inb(info->cnt_context.counter_1_reg);
-		data[0] |= tmp << 8;
-		break;
-	case 2:
-		tmp = inb(info->cnt_context.counter_2_reg);
-		data[0] = tmp;
-		tmp = inb(info->cnt_context.counter_2_reg);
-		data[0] |= tmp << 8;
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_cnt_insn_read(): "
-		       "Invalid channel %d\n",
-		       dev->minor, insn->chanspec);
-		return -EINVAL;
-	}
+	data[0] = i8254_read(info->timer_regbase, 0, insn->chanspec);
 
 	return 1;
 }
@@ -1957,58 +1541,72 @@ static int me4000_cnt_insn_write(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
-
-	unsigned short tmp;
+	struct me4000_info *info = dev->private;
 
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_cnt_insn_write(): "
-		       "Invalid instruction length %d\n",
-		       dev->minor, insn->n);
+		dev_err(dev->class_dev, "Invalid instruction length %d\n",
+			insn->n);
 		return -EINVAL;
 	}
 
-	switch (insn->chanspec) {
-	case 0:
-		tmp = data[0] & 0xFF;
-		outb(tmp, info->cnt_context.counter_0_reg);
-		tmp = (data[0] >> 8) & 0xFF;
-		outb(tmp, info->cnt_context.counter_0_reg);
-		break;
-	case 1:
-		tmp = data[0] & 0xFF;
-		outb(tmp, info->cnt_context.counter_1_reg);
-		tmp = (data[0] >> 8) & 0xFF;
-		outb(tmp, info->cnt_context.counter_1_reg);
-		break;
-	case 2:
-		tmp = data[0] & 0xFF;
-		outb(tmp, info->cnt_context.counter_2_reg);
-		tmp = (data[0] >> 8) & 0xFF;
-		outb(tmp, info->cnt_context.counter_2_reg);
-		break;
-	default:
-		printk(KERN_ERR
-		       "comedi%d: me4000: me4000_cnt_insn_write(): "
-		       "Invalid channel %d\n",
-		       dev->minor, insn->chanspec);
-		return -EINVAL;
-	}
+	i8254_write(info->timer_regbase, 0, insn->chanspec, data[0]);
 
 	return 1;
 }
 
-static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static const void *me4000_find_boardinfo(struct comedi_device *dev,
+					 struct pci_dev *pcidev)
 {
+	const struct me4000_board *thisboard;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(me4000_boards); i++) {
+		thisboard = &me4000_boards[i];
+		if (thisboard->device_id == pcidev->device)
+			return thisboard;
+	}
+	return NULL;
+}
+
+static int me4000_attach_pci(struct comedi_device *dev,
+			     struct pci_dev *pcidev)
+{
+	const struct me4000_board *thisboard;
+	struct me4000_info *info;
 	struct comedi_subdevice *s;
 	int result;
 
-	result = me4000_probe(dev, it);
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	thisboard = me4000_find_boardinfo(dev, pcidev);
+	if (!thisboard)
+		return -ENODEV;
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	result = alloc_private(dev, sizeof(*info));
+	if (result)
+		return result;
+	info = dev->private;
+
+	result = comedi_pci_enable(pcidev, dev->board_name);
+	if (result)
+		return result;
+
+	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)
+		return -ENODEV;
+
+	result = xilinx_download(dev);
 	if (result)
 		return result;
 
+	me4000_reset(dev);
+
 	result = comedi_alloc_subdevices(dev, 4);
 	if (result)
 		return result;
@@ -2017,35 +1615,34 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
       Analog input subdevice
       ========================================================================*/
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 
-	if (thisboard->ai.count) {
+	if (thisboard->ai_nchan) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags =
 		    SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
-		s->n_chan = thisboard->ai.count;
+		s->n_chan = thisboard->ai_nchan;
 		s->maxdata = 0xFFFF;	/*  16 bit ADC */
 		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
 		s->range_table = &me4000_ai_range;
 		s->insn_read = me4000_ai_insn_read;
 
-		if (info->irq > 0) {
-			if (request_irq(info->irq, me4000_ai_isr,
-					IRQF_SHARED, "ME-4000", dev)) {
-				printk
-				    ("comedi%d: me4000: me4000_attach(): "
-				     "Unable to allocate irq\n", dev->minor);
+		if (pcidev->irq > 0) {
+			if (request_irq(pcidev->irq, me4000_ai_isr,
+					IRQF_SHARED, dev->board_name, dev)) {
+				dev_warn(dev->class_dev,
+					"request_irq failed\n");
 			} else {
 				dev->read_subdev = s;
 				s->subdev_flags |= SDF_CMD_READ;
 				s->cancel = me4000_ai_cancel;
 				s->do_cmdtest = me4000_ai_do_cmd_test;
 				s->do_cmd = me4000_ai_do_cmd;
+
+				dev->irq = pcidev->irq;
 			}
 		} else {
-			printk(KERN_WARNING
-			       "comedi%d: me4000: me4000_attach(): "
-			       "No interrupt available\n", dev->minor);
+			dev_warn(dev->class_dev, "No interrupt available\n");
 		}
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -2055,14 +1652,14 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
       Analog output subdevice
       ========================================================================*/
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 
-	if (thisboard->ao.count) {
+	if (thisboard->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
-		s->n_chan = thisboard->ao.count;
+		s->n_chan = thisboard->ao_nchan;
 		s->maxdata = 0xFFFF;	/*  16 bit DAC */
-		s->range_table = &me4000_ao_range;
+		s->range_table = &range_bipolar10;
 		s->insn_write = me4000_ao_insn_write;
 		s->insn_read = me4000_ao_insn_read;
 	} else {
@@ -2073,12 +1670,12 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
       Digital I/O subdevice
       ========================================================================*/
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 
-	if (thisboard->dio.count) {
+	if (thisboard->dio_nchan) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->dio.count * 8;
+		s->n_chan = thisboard->dio_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->insn_bits = me4000_dio_insn_bits;
@@ -2091,21 +1688,22 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	 * Check for optoisolated ME-4000 version. If one the first
 	 * port is a fixed output port and the second is a fixed input port.
 	 */
-	if (!inl(info->dio_context.dir_reg)) {
+	if (!inl(dev->iobase + ME4000_DIO_DIR_REG)) {
 		s->io_bits |= 0xFF;
-		outl(ME4000_DIO_CTRL_BIT_MODE_0, info->dio_context.dir_reg);
+		outl(ME4000_DIO_CTRL_BIT_MODE_0,
+			dev->iobase + ME4000_DIO_DIR_REG);
 	}
 
     /*=========================================================================
       Counter subdevice
       ========================================================================*/
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 
-	if (thisboard->cnt.count) {
+	if (thisboard->has_counter) {
 		s->type = COMEDI_SUBD_COUNTER;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->cnt.count;
+		s->n_chan = 3;
 		s->maxdata = 0xFFFF;	/*  16 bit counters */
 		s->insn_read = me4000_cnt_insn_read;
 		s->insn_write = me4000_cnt_insn_write;
@@ -2119,12 +1717,14 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void me4000_detach(struct comedi_device *dev)
 {
-	if (info) {
-		if (info->pci_dev_p) {
-			reset_board(dev);
-			if (info->plx_regbase)
-				comedi_pci_disable(info->pci_dev_p);
-			pci_dev_put(info->pci_dev_p);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (pcidev) {
+		if (dev->iobase) {
+			me4000_reset(dev);
+			comedi_pci_disable(pcidev);
 		}
 	}
 }
@@ -2132,7 +1732,7 @@ static void me4000_detach(struct comedi_device *dev)
 static struct comedi_driver me4000_driver = {
 	.driver_name	= "me4000",
 	.module		= THIS_MODULE,
-	.attach		= me4000_attach,
+	.attach_pci	= me4000_attach_pci,
 	.detach		= me4000_detach,
 };
 
@@ -2148,20 +1748,20 @@ static void __devexit me4000_pci_remove(struct pci_dev *dev)
 }
 
 static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4650) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4660) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4661) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4662) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4663) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4670) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4671) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4672) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4673) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4680) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4681) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4682) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4683) },
-	{ 0 }
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4650)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660I)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660S)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660IS)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670I)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670S)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670IS)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680I)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680S)},
+	{PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680IS)},
+	{0}
 };
 MODULE_DEVICE_TABLE(pci, me4000_pci_table);
 
diff --git a/drivers/staging/comedi/drivers/me4000.h b/drivers/staging/comedi/drivers/me4000.h
deleted file mode 100644
index 5a4df4e4b236..000000000000
--- a/drivers/staging/comedi/drivers/me4000.h
+++ /dev/null
@@ -1,409 +0,0 @@
-/*
-    me4000.h
-    Register descriptions and defines for the ME-4000 board family
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998-9 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.
-
-    You should have received a copy of the GNU General Public License
-    along with this program; if not, write to the Free Software
-    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
-
-*/
-
-#ifndef _ME4000_H_
-#define _ME4000_H_
-
-/*=============================================================================
-  PCI vendor and device IDs
-  ===========================================================================*/
-
-#define PCI_VENDOR_ID_MEILHAUS 0x1402
-
-#define PCI_DEVICE_ID_MEILHAUS_ME4650	0x4650	/*  Low Cost version */
-
-#define PCI_DEVICE_ID_MEILHAUS_ME4660	0x4660	/*  Standard version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4660I	0x4661	/*  Isolated version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4660S	0x4662	/*  Standard version with Sample and Hold */
-#define PCI_DEVICE_ID_MEILHAUS_ME4660IS	0x4663	/*  Isolated version with Sample and Hold */
-
-#define PCI_DEVICE_ID_MEILHAUS_ME4670	0x4670	/*  Standard version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4670I	0x4671	/*  Isolated version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4670S	0x4672	/*  Standard version with Sample and Hold */
-#define PCI_DEVICE_ID_MEILHAUS_ME4670IS	0x4673	/*  Isolated version with Sample and Hold */
-
-#define PCI_DEVICE_ID_MEILHAUS_ME4680	0x4680	/*  Standard version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4680I	0x4681	/*  Isolated version */
-#define PCI_DEVICE_ID_MEILHAUS_ME4680S	0x4682	/*  Standard version with Sample and Hold */
-#define PCI_DEVICE_ID_MEILHAUS_ME4680IS	0x4683	/*  Isolated version with Sample and Hold */
-
-/*=============================================================================
-  ME-4000 base register offsets
-  ===========================================================================*/
-
-#define ME4000_AO_00_CTRL_REG			0x00	/*  R/W */
-#define ME4000_AO_00_STATUS_REG			0x04	/*  R/_ */
-#define ME4000_AO_00_FIFO_REG			0x08	/*  _/W */
-#define ME4000_AO_00_SINGLE_REG			0x0C	/*  R/W */
-#define ME4000_AO_00_TIMER_REG			0x10	/*  _/W */
-
-#define ME4000_AO_01_CTRL_REG			0x18	/*  R/W */
-#define ME4000_AO_01_STATUS_REG			0x1C	/*  R/_ */
-#define ME4000_AO_01_FIFO_REG			0x20	/*  _/W */
-#define ME4000_AO_01_SINGLE_REG			0x24	/*  R/W */
-#define ME4000_AO_01_TIMER_REG			0x28	/*  _/W */
-
-#define ME4000_AO_02_CTRL_REG			0x30	/*  R/W */
-#define ME4000_AO_02_STATUS_REG			0x34	/*  R/_ */
-#define ME4000_AO_02_FIFO_REG			0x38	/*  _/W */
-#define ME4000_AO_02_SINGLE_REG			0x3C	/*  R/W */
-#define ME4000_AO_02_TIMER_REG			0x40	/*  _/W */
-
-#define ME4000_AO_03_CTRL_REG			0x48	/*  R/W */
-#define ME4000_AO_03_STATUS_REG			0x4C	/*  R/_ */
-#define ME4000_AO_03_FIFO_REG			0x50	/*  _/W */
-#define ME4000_AO_03_SINGLE_REG			0x54	/*  R/W */
-#define ME4000_AO_03_TIMER_REG			0x58	/*  _/W */
-
-#define ME4000_AI_CTRL_REG			0x74	/*  _/W */
-#define ME4000_AI_STATUS_REG			0x74	/*  R/_ */
-#define ME4000_AI_CHANNEL_LIST_REG		0x78	/*  _/W */
-#define ME4000_AI_DATA_REG			0x7C	/*  R/_ */
-#define ME4000_AI_CHAN_TIMER_REG		0x80	/*  _/W */
-#define ME4000_AI_CHAN_PRE_TIMER_REG		0x84	/*  _/W */
-#define ME4000_AI_SCAN_TIMER_LOW_REG		0x88	/*  _/W */
-#define ME4000_AI_SCAN_TIMER_HIGH_REG		0x8C	/*  _/W */
-#define ME4000_AI_SCAN_PRE_TIMER_LOW_REG	0x90	/*  _/W */
-#define ME4000_AI_SCAN_PRE_TIMER_HIGH_REG	0x94	/*  _/W */
-#define ME4000_AI_START_REG			0x98	/*  R/_ */
-
-#define ME4000_IRQ_STATUS_REG			0x9C	/*  R/_ */
-
-#define ME4000_DIO_PORT_0_REG			0xA0	/*  R/W */
-#define ME4000_DIO_PORT_1_REG			0xA4	/*  R/W */
-#define ME4000_DIO_PORT_2_REG			0xA8	/*  R/W */
-#define ME4000_DIO_PORT_3_REG			0xAC	/*  R/W */
-#define ME4000_DIO_DIR_REG			0xB0	/*  R/W */
-
-#define ME4000_AO_LOADSETREG_XX			0xB4	/*  R/W */
-
-#define ME4000_DIO_CTRL_REG			0xB8	/*  R/W */
-
-#define ME4000_AO_DEMUX_ADJUST_REG		0xBC	/*  -/W */
-
-#define ME4000_AI_SAMPLE_COUNTER_REG		0xC0	/*  _/W */
-
-/*=============================================================================
-  Value to adjust Demux
-  ===========================================================================*/
-
-#define ME4000_AO_DEMUX_ADJUST_VALUE            0x4C
-
-/*=============================================================================
-  Counter base register offsets
-  ===========================================================================*/
-
-#define ME4000_CNT_COUNTER_0_REG		0x00
-#define ME4000_CNT_COUNTER_1_REG		0x01
-#define ME4000_CNT_COUNTER_2_REG		0x02
-#define ME4000_CNT_CTRL_REG			0x03
-
-/*=============================================================================
-  PLX base register offsets
-  ===========================================================================*/
-
-#define PLX_INTCSR	0x4C	/*  Interrupt control and status register */
-#define PLX_ICR		0x50	/*  Initialization control register */
-
-/*=============================================================================
-  Bits for the PLX_ICSR register
-  ===========================================================================*/
-
-#define PLX_INTCSR_LOCAL_INT1_EN             0x01	/*  If set, local interrupt 1 is enabled (r/w) */
-#define PLX_INTCSR_LOCAL_INT1_POL            0x02	/*  If set, local interrupt 1 polarity is active high (r/w) */
-#define PLX_INTCSR_LOCAL_INT1_STATE          0x04	/*  If set, local interrupt 1 is active (r/_) */
-#define PLX_INTCSR_LOCAL_INT2_EN             0x08	/*  If set, local interrupt 2 is enabled (r/w) */
-#define PLX_INTCSR_LOCAL_INT2_POL            0x10	/*  If set, local interrupt 2 polarity is active high (r/w) */
-#define PLX_INTCSR_LOCAL_INT2_STATE          0x20	/*  If set, local interrupt 2 is active  (r/_) */
-#define PLX_INTCSR_PCI_INT_EN                0x40	/*  If set, PCI interrupt is enabled (r/w) */
-#define PLX_INTCSR_SOFT_INT                  0x80	/*  If set, a software interrupt is generated (r/w) */
-
-/*=============================================================================
-  Bits for the PLX_ICR register
-  ===========================================================================*/
-
-#define PLX_ICR_BIT_EEPROM_CLOCK_SET		0x01000000
-#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		0x02000000
-#define PLX_ICR_BIT_EEPROM_WRITE		0x04000000
-#define PLX_ICR_BIT_EEPROM_READ			0x08000000
-#define PLX_ICR_BIT_EEPROM_VALID		0x10000000
-
-#define PLX_ICR_MASK_EEPROM			0x1F000000
-
-#define EEPROM_DELAY				1
-
-/*=============================================================================
-  Bits for the ME4000_AO_CTRL_REG register
-  ===========================================================================*/
-
-#define ME4000_AO_CTRL_BIT_MODE_0		0x001
-#define ME4000_AO_CTRL_BIT_MODE_1		0x002
-#define ME4000_AO_CTRL_MASK_MODE		0x003
-#define ME4000_AO_CTRL_BIT_STOP			0x004
-#define ME4000_AO_CTRL_BIT_ENABLE_FIFO		0x008
-#define ME4000_AO_CTRL_BIT_ENABLE_EX_TRIG	0x010
-#define ME4000_AO_CTRL_BIT_EX_TRIG_EDGE		0x020
-#define ME4000_AO_CTRL_BIT_IMMEDIATE_STOP	0x080
-#define ME4000_AO_CTRL_BIT_ENABLE_DO		0x100
-#define ME4000_AO_CTRL_BIT_ENABLE_IRQ		0x200
-#define ME4000_AO_CTRL_BIT_RESET_IRQ		0x400
-
-/*=============================================================================
-  Bits for the ME4000_AO_STATUS_REG register
-  ===========================================================================*/
-
-#define ME4000_AO_STATUS_BIT_FSM		0x01
-#define ME4000_AO_STATUS_BIT_FF			0x02
-#define ME4000_AO_STATUS_BIT_HF			0x04
-#define ME4000_AO_STATUS_BIT_EF			0x08
-
-/*=============================================================================
-  Bits for the ME4000_AI_CTRL_REG register
-  ===========================================================================*/
-
-#define ME4000_AI_CTRL_BIT_MODE_0		0x00000001
-#define ME4000_AI_CTRL_BIT_MODE_1		0x00000002
-#define ME4000_AI_CTRL_BIT_MODE_2		0x00000004
-#define ME4000_AI_CTRL_BIT_SAMPLE_HOLD		0x00000008
-#define ME4000_AI_CTRL_BIT_IMMEDIATE_STOP	0x00000010
-#define ME4000_AI_CTRL_BIT_STOP			0x00000020
-#define ME4000_AI_CTRL_BIT_CHANNEL_FIFO		0x00000040
-#define ME4000_AI_CTRL_BIT_DATA_FIFO		0x00000080
-#define ME4000_AI_CTRL_BIT_FULLSCALE		0x00000100
-#define ME4000_AI_CTRL_BIT_OFFSET		0x00000200
-#define ME4000_AI_CTRL_BIT_EX_TRIG_ANALOG	0x00000400
-#define ME4000_AI_CTRL_BIT_EX_TRIG		0x00000800
-#define ME4000_AI_CTRL_BIT_EX_TRIG_FALLING	0x00001000
-#define ME4000_AI_CTRL_BIT_EX_IRQ		0x00002000
-#define ME4000_AI_CTRL_BIT_EX_IRQ_RESET		0x00004000
-#define ME4000_AI_CTRL_BIT_LE_IRQ		0x00008000
-#define ME4000_AI_CTRL_BIT_LE_IRQ_RESET		0x00010000
-#define ME4000_AI_CTRL_BIT_HF_IRQ		0x00020000
-#define ME4000_AI_CTRL_BIT_HF_IRQ_RESET		0x00040000
-#define ME4000_AI_CTRL_BIT_SC_IRQ		0x00080000
-#define ME4000_AI_CTRL_BIT_SC_IRQ_RESET		0x00100000
-#define ME4000_AI_CTRL_BIT_SC_RELOAD		0x00200000
-#define ME4000_AI_CTRL_BIT_EX_TRIG_BOTH		0x80000000
-
-/*=============================================================================
-  Bits for the ME4000_AI_STATUS_REG register
-  ===========================================================================*/
-
-#define ME4000_AI_STATUS_BIT_EF_CHANNEL		0x00400000
-#define ME4000_AI_STATUS_BIT_HF_CHANNEL		0x00800000
-#define ME4000_AI_STATUS_BIT_FF_CHANNEL		0x01000000
-#define ME4000_AI_STATUS_BIT_EF_DATA		0x02000000
-#define ME4000_AI_STATUS_BIT_HF_DATA		0x04000000
-#define ME4000_AI_STATUS_BIT_FF_DATA		0x08000000
-#define ME4000_AI_STATUS_BIT_LE			0x10000000
-#define ME4000_AI_STATUS_BIT_FSM		0x20000000
-
-/*=============================================================================
-  Bits for the ME4000_IRQ_STATUS_REG register
-  ===========================================================================*/
-
-#define ME4000_IRQ_STATUS_BIT_EX		0x01
-#define ME4000_IRQ_STATUS_BIT_LE		0x02
-#define ME4000_IRQ_STATUS_BIT_AI_HF		0x04
-#define ME4000_IRQ_STATUS_BIT_AO_0_HF		0x08
-#define ME4000_IRQ_STATUS_BIT_AO_1_HF		0x10
-#define ME4000_IRQ_STATUS_BIT_AO_2_HF		0x20
-#define ME4000_IRQ_STATUS_BIT_AO_3_HF		0x40
-#define ME4000_IRQ_STATUS_BIT_SC		0x80
-
-/*=============================================================================
-  Bits for the ME4000_DIO_CTRL_REG register
-  ===========================================================================*/
-
-#define ME4000_DIO_CTRL_BIT_MODE_0		0x0001
-#define ME4000_DIO_CTRL_BIT_MODE_1		0x0002
-#define ME4000_DIO_CTRL_BIT_MODE_2		0x0004
-#define ME4000_DIO_CTRL_BIT_MODE_3		0x0008
-#define ME4000_DIO_CTRL_BIT_MODE_4		0x0010
-#define ME4000_DIO_CTRL_BIT_MODE_5		0x0020
-#define ME4000_DIO_CTRL_BIT_MODE_6		0x0040
-#define ME4000_DIO_CTRL_BIT_MODE_7		0x0080
-
-#define ME4000_DIO_CTRL_BIT_FUNCTION_0		0x0100
-#define ME4000_DIO_CTRL_BIT_FUNCTION_1		0x0200
-
-#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_0		0x0400
-#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_1		0x0800
-#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_2		0x1000
-#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_3		0x2000
-
-/*=============================================================================
-  Information about the hardware capabilities
-  ===========================================================================*/
-
-struct me4000_ao_info {
-	int count;
-	int fifo_count;
-};
-
-struct me4000_ai_info {
-	int count;
-	int sh_count;
-	int diff_count;
-	int ex_trig_analog;
-};
-
-struct me4000_dio_info {
-	int count;
-};
-
-struct me4000_cnt_info {
-	int count;
-};
-
-struct me4000_board {
-	const char *name;
-	unsigned short device_id;
-	struct me4000_ao_info ao;
-	struct me4000_ai_info ai;
-	struct me4000_dio_info dio;
-	struct me4000_cnt_info cnt;
-};
-
-#define thisboard ((const struct me4000_board *)dev->board_ptr)
-
-/*=============================================================================
-  Global board and subdevice information structures
-  ===========================================================================*/
-
-struct me4000_ao_context {
-	int irq;
-
-	unsigned long mirror;	/*  Store the last written value */
-
-	unsigned long ctrl_reg;
-	unsigned long status_reg;
-	unsigned long fifo_reg;
-	unsigned long single_reg;
-	unsigned long timer_reg;
-	unsigned long irq_status_reg;
-	unsigned long preload_reg;
-};
-
-struct me4000_ai_context {
-	int irq;
-
-	unsigned long ctrl_reg;
-	unsigned long status_reg;
-	unsigned long channel_list_reg;
-	unsigned long data_reg;
-	unsigned long chan_timer_reg;
-	unsigned long chan_pre_timer_reg;
-	unsigned long scan_timer_low_reg;
-	unsigned long scan_timer_high_reg;
-	unsigned long scan_pre_timer_low_reg;
-	unsigned long scan_pre_timer_high_reg;
-	unsigned long start_reg;
-	unsigned long irq_status_reg;
-	unsigned long sample_counter_reg;
-};
-
-struct me4000_dio_context {
-	unsigned long dir_reg;
-	unsigned long ctrl_reg;
-	unsigned long port_0_reg;
-	unsigned long port_1_reg;
-	unsigned long port_2_reg;
-	unsigned long port_3_reg;
-};
-
-struct me4000_cnt_context {
-	unsigned long ctrl_reg;
-	unsigned long counter_0_reg;
-	unsigned long counter_1_reg;
-	unsigned long counter_2_reg;
-};
-
-struct me4000_info {
-	unsigned long plx_regbase;	/*  PLX configuration space base address */
-	unsigned long me4000_regbase;	/*  Base address of the ME4000 */
-	unsigned long timer_regbase;	/*  Base address of the timer circuit */
-	unsigned long program_regbase;	/*  Base address to set the program pin for the xilinx */
-
-	unsigned long plx_regbase_size;	/*  PLX register set space */
-	unsigned long me4000_regbase_size;	/*  ME4000 register set space */
-	unsigned long timer_regbase_size;	/*  Timer circuit register set space */
-	unsigned long program_regbase_size;	/*  Size of program base address of the ME4000 */
-
-	unsigned int serial_no;	/*  Serial number of the board */
-	unsigned char hw_revision;	/*  Hardware revision of the board */
-	unsigned short vendor_id;	/*  Meilhaus vendor id */
-	unsigned short device_id;	/*  Device id */
-
-	struct pci_dev *pci_dev_p;	/*  General PCI information */
-
-	unsigned int irq;	/*  IRQ assigned from the PCI BIOS */
-
-	struct me4000_ai_context ai_context;	/*  Analog input  specific context */
-	struct me4000_ao_context ao_context[4];	/*  Vector with analog output specific context */
-	struct me4000_dio_context dio_context;	/*  Digital I/O specific context */
-	struct me4000_cnt_context cnt_context;	/*  Counter specific context */
-};
-
-#define info	((struct me4000_info *)dev->private)
-
-/*-----------------------------------------------------------------------------
-  Defines for analog input
- ----------------------------------------------------------------------------*/
-
-/* General stuff */
-#define ME4000_AI_FIFO_COUNT			2048
-
-#define ME4000_AI_MIN_TICKS			66
-#define ME4000_AI_MIN_SAMPLE_TIME		2000	/*  Minimum sample time [ns] */
-#define ME4000_AI_BASE_FREQUENCY		(unsigned int) 33E6
-
-/* Channel list defines and masks */
-#define ME4000_AI_CHANNEL_LIST_COUNT		1024
-
-#define ME4000_AI_LIST_INPUT_SINGLE_ENDED	0x000
-#define ME4000_AI_LIST_INPUT_DIFFERENTIAL	0x020
-
-#define ME4000_AI_LIST_RANGE_BIPOLAR_10		0x000
-#define ME4000_AI_LIST_RANGE_BIPOLAR_2_5	0x040
-#define ME4000_AI_LIST_RANGE_UNIPOLAR_10	0x080
-#define ME4000_AI_LIST_RANGE_UNIPOLAR_2_5	0x0C0
-
-#define ME4000_AI_LIST_LAST_ENTRY		0x100
-
-/*-----------------------------------------------------------------------------
-  Defines for counters
- ----------------------------------------------------------------------------*/
-
-#define ME4000_CNT_COUNTER_0  0x00
-#define ME4000_CNT_COUNTER_1  0x40
-#define ME4000_CNT_COUNTER_2  0x80
-
-#define ME4000_CNT_MODE_0     0x00	/*  Change state if zero crossing */
-#define ME4000_CNT_MODE_1     0x02	/*  Retriggerable One-Shot */
-#define ME4000_CNT_MODE_2     0x04	/*  Asymmetrical divider */
-#define ME4000_CNT_MODE_3     0x06	/*  Symmetrical divider */
-#define ME4000_CNT_MODE_4     0x08	/*  Counter start by software trigger */
-#define ME4000_CNT_MODE_5     0x0A	/*  Counter start by hardware trigger */
-
-#endif
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 8c6f8b93b277..2ce0b14af589 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -41,22 +41,14 @@ Configuration options:
 
     If bus/slot is not specified, the first available PCI
     device will be used.
-
-The 2600 requires a firmware upload, which can be accomplished
-using the -i or --init-data option of comedi_config.
-The firmware can be
-found in the comedi_nonfree_firmware tarball available
-from http://www.comedi.org
-
 */
 
 #include <linux/interrupt.h>
 #include <linux/sched.h>
+#include <linux/firmware.h>
 #include "../comedidev.h"
 
-/*#include "me2600_fw.h" */
-
-#define ME_DRIVER_NAME		"me_daq"
+#define ME2600_FIRMWARE		"me2600_firmware.bin"
 
 #define PCI_VENDOR_ID_MEILHAUS	0x1402
 #define ME2000_DEVICE_ID	0x2000
@@ -198,8 +190,7 @@ struct me_board {
 
 static const struct me_board me_boards[] = {
 	{
-	 /* -- ME-2600i -- */
-	 .name = ME_DRIVER_NAME,
+	 .name = "me-2600i",
 	 .device_id = ME2600_DEVICE_ID,
 	 /* Analog Output */
 	 .ao_channel_nbr = 4,
@@ -214,8 +205,7 @@ static const struct me_board me_boards[] = {
 	 .dio_channel_nbr = 32,
 	 },
 	{
-	 /* -- ME-2000i -- */
-	 .name = ME_DRIVER_NAME,
+	 .name = "me-2000i",
 	 .device_id = ME2000_DEVICE_ID,
 	 /* Analog Output */
 	 .ao_channel_nbr = 0,
@@ -341,7 +331,7 @@ static int me_dio_insn_bits(struct comedi_device *dev,
 
 /* Analog instant input */
 static int me_ai_insn_read(struct comedi_device *dev,
-			   struct comedi_subdevice *subdevice,
+			   struct comedi_subdevice *s,
 			   struct comedi_insn *insn, unsigned int *data)
 {
 	unsigned short value;
@@ -435,7 +425,7 @@ static int me_ai_do_cmd_test(struct comedi_device *dev,
 
 /* Analog input command */
 static int me_ai_do_cmd(struct comedi_device *dev,
-			struct comedi_subdevice *subdevice)
+			struct comedi_subdevice *s)
 {
 	return 0;
 }
@@ -524,8 +514,7 @@ static int me_ao_insn_read(struct comedi_device *dev,
 
 /* Xilinx firmware download for card: ME-2600i */
 static int me2600_xilinx_download(struct comedi_device *dev,
-				  unsigned char *me2600_firmware,
-				  unsigned int length)
+				  const u8 *data, size_t size)
 {
 	unsigned int value;
 	unsigned int file_length;
@@ -552,19 +541,20 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 	 * Byte 8-11:  date
 	 * Byte 12-15: reserved
 	 */
-	if (length < 16)
+	if (size < 16)
 		return -EINVAL;
-	file_length = (((unsigned int)me2600_firmware[0] & 0xff) << 24) +
-	    (((unsigned int)me2600_firmware[1] & 0xff) << 16) +
-	    (((unsigned int)me2600_firmware[2] & 0xff) << 8) +
-	    ((unsigned int)me2600_firmware[3] & 0xff);
+
+	file_length = (((unsigned int)data[0] & 0xff) << 24) +
+	    (((unsigned int)data[1] & 0xff) << 16) +
+	    (((unsigned int)data[2] & 0xff) << 8) +
+	    ((unsigned int)data[3] & 0xff);
 
 	/*
 	 * Loop for writing firmware byte by byte to xilinx
 	 * Firmware data start at offfset 16
 	 */
 	for (i = 0; i < file_length; i++)
-		writeb((me2600_firmware[16 + i] & 0xff),
+		writeb((data[16 + i] & 0xff),
 		       dev_private->me_regbase + 0x0);
 
 	/* Write 5 dummy values to xilinx */
@@ -590,6 +580,22 @@ static int me2600_xilinx_download(struct comedi_device *dev,
 	return 0;
 }
 
+static int me2600_upload_firmware(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	const struct firmware *fw;
+	int ret;
+
+	ret = request_firmware(&fw, ME2600_FIRMWARE, &pcidev->dev);
+	if (ret)
+		return ret;
+
+	ret = me2600_xilinx_download(dev, fw->data, fw->size);
+	release_firmware(fw);
+
+	return ret;
+}
+
 /* Reset device */
 static int me_reset(struct comedi_device *dev)
 {
@@ -607,44 +613,24 @@ static int me_reset(struct comedi_device *dev)
 	return 0;
 }
 
-static struct pci_dev *me_find_pci_dev(struct comedi_device *dev,
-				       struct comedi_devconfig *it)
+static const void *me_find_boardinfo(struct comedi_device *dev,
+				     struct pci_dev *pcidev)
 {
 	const struct me_board *board;
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
 	int i;
 
-	for_each_pci_dev(pcidev) {
-		if (bus || slot) {
-			if (pcidev->bus->number != bus ||
-			    PCI_SLOT(pcidev->devfn) != slot)
-				continue;
-		}
-		if (pcidev->vendor != PCI_VENDOR_ID_MEILHAUS)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(me_boards); i++) {
-			board = &me_boards[i];
-			if (board->device_id != pcidev->device)
-				continue;
-
-			dev->board_ptr = board;
-			return pcidev;
-		}
+	for (i = 0; i < ARRAY_SIZE(me_boards); i++) {
+		board = &me_boards[i];
+		if (board->device_id == pcidev->device)
+			return board;
 	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
 	return NULL;
 }
 
-static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int me_attach_pci(struct comedi_device *dev, struct pci_dev *pcidev)
 {
-	struct pci_dev *pci_device;
-	struct comedi_subdevice *subdevice;
-	struct me_board *board;
+	const struct me_board *board;
+	struct comedi_subdevice *s;
 	resource_size_t plx_regbase_tmp;
 	unsigned long plx_regbase_size_tmp;
 	resource_size_t me_regbase_tmp;
@@ -654,29 +640,28 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	resource_size_t regbase_tmp;
 	int result, error;
 
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	board = me_find_boardinfo(dev, pcidev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
+
 	/* Allocate private memory */
 	if (alloc_private(dev, sizeof(struct me_private_data)) < 0)
 		return -ENOMEM;
 
-	pci_device = me_find_pci_dev(dev, it);
-	if (!pci_device)
-		return -EIO;
-	comedi_set_hw_dev(dev, &pci_device->dev);
-	board = (struct me_board *)dev->board_ptr;
-
 	/* Enable PCI device and request PCI regions */
-	if (comedi_pci_enable(pci_device, ME_DRIVER_NAME) < 0) {
+	if (comedi_pci_enable(pcidev, dev->board_name) < 0) {
 		printk(KERN_ERR "comedi%d: Failed to enable PCI device and "
 		       "request regions\n", dev->minor);
 		return -EIO;
 	}
 
-	/* Set data in device structure */
-	dev->board_name = board->name;
-
 	/* Read PLX register base address [PCI_BASE_ADDRESS #0]. */
-	plx_regbase_tmp = pci_resource_start(pci_device, 0);
-	plx_regbase_size_tmp = pci_resource_len(pci_device, 0);
+	plx_regbase_tmp = pci_resource_start(pcidev, 0);
+	plx_regbase_size_tmp = pci_resource_len(pcidev, 0);
 	dev_private->plx_regbase =
 	    ioremap(plx_regbase_tmp, plx_regbase_size_tmp);
 	dev_private->plx_regbase_size = plx_regbase_size_tmp;
@@ -687,8 +672,8 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* Read Swap base address [PCI_BASE_ADDRESS #5]. */
 
-	swap_regbase_tmp = pci_resource_start(pci_device, 5);
-	swap_regbase_size_tmp = pci_resource_len(pci_device, 5);
+	swap_regbase_tmp = pci_resource_start(pcidev, 5);
+	swap_regbase_size_tmp = pci_resource_len(pcidev, 5);
 
 	if (!swap_regbase_tmp)
 		printk(KERN_ERR "comedi%d: Swap not present\n", dev->minor);
@@ -702,20 +687,20 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			plx_regbase_tmp = swap_regbase_tmp;
 			swap_regbase_tmp = regbase_tmp;
 
-			result = pci_write_config_dword(pci_device,
+			result = pci_write_config_dword(pcidev,
 							PCI_BASE_ADDRESS_0,
 							plx_regbase_tmp);
 			if (result != PCIBIOS_SUCCESSFUL)
 				return -EIO;
 
-			result = pci_write_config_dword(pci_device,
+			result = pci_write_config_dword(pcidev,
 							PCI_BASE_ADDRESS_5,
 							swap_regbase_tmp);
 			if (result != PCIBIOS_SUCCESSFUL)
 				return -EIO;
 		} else {
 			plx_regbase_tmp -= 0x80;
-			result = pci_write_config_dword(pci_device,
+			result = pci_write_config_dword(pcidev,
 							PCI_BASE_ADDRESS_0,
 							plx_regbase_tmp);
 			if (result != PCIBIOS_SUCCESSFUL)
@@ -726,8 +711,8 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* Read Meilhaus register base address [PCI_BASE_ADDRESS #2]. */
 
-	me_regbase_tmp = pci_resource_start(pci_device, 2);
-	me_regbase_size_tmp = pci_resource_len(pci_device, 2);
+	me_regbase_tmp = pci_resource_start(pcidev, 2);
+	me_regbase_size_tmp = pci_resource_len(pcidev, 2);
 	dev_private->me_regbase_size = me_regbase_size_tmp;
 	dev_private->me_regbase = ioremap(me_regbase_tmp, me_regbase_size_tmp);
 	if (!dev_private->me_regbase) {
@@ -735,64 +720,55 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		       dev->minor);
 		return -ENOMEM;
 	}
+
 	/* Download firmware and reset card */
 	if (board->device_id == ME2600_DEVICE_ID) {
-		unsigned char *aux_data;
-		int aux_len;
-
-		aux_data = comedi_aux_data(it->options, 0);
-		aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-
-		if (!aux_data || aux_len < 1) {
-			comedi_error(dev, "You must provide me2600 firmware "
-				     "using the --init-data option of "
-				     "comedi_config");
-			return -EINVAL;
-		}
-		me2600_xilinx_download(dev, aux_data, aux_len);
+		result = me2600_upload_firmware(dev);
+		if (result < 0)
+			return result;
 	}
-
 	me_reset(dev);
 
 	error = comedi_alloc_subdevices(dev, 3);
 	if (error)
 		return error;
 
-	subdevice = dev->subdevices + 0;
-	subdevice->type = COMEDI_SUBD_AI;
-	subdevice->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
-	subdevice->n_chan = board->ai_channel_nbr;
-	subdevice->maxdata = board->ai_resolution_mask;
-	subdevice->len_chanlist = board->ai_channel_nbr;
-	subdevice->range_table = board->ai_range_list;
-	subdevice->cancel = me_ai_cancel;
-	subdevice->insn_read = me_ai_insn_read;
-	subdevice->do_cmdtest = me_ai_do_cmd_test;
-	subdevice->do_cmd = me_ai_do_cmd;
-
-	subdevice = dev->subdevices + 1;
-	subdevice->type = COMEDI_SUBD_AO;
-	subdevice->subdev_flags = SDF_WRITEABLE | SDF_COMMON;
-	subdevice->n_chan = board->ao_channel_nbr;
-	subdevice->maxdata = board->ao_resolution_mask;
-	subdevice->len_chanlist = board->ao_channel_nbr;
-	subdevice->range_table = board->ao_range_list;
-	subdevice->insn_read = me_ao_insn_read;
-	subdevice->insn_write = me_ao_insn_write;
-
-	subdevice = dev->subdevices + 2;
-	subdevice->type = COMEDI_SUBD_DIO;
-	subdevice->subdev_flags = SDF_READABLE | SDF_WRITEABLE;
-	subdevice->n_chan = board->dio_channel_nbr;
-	subdevice->maxdata = 1;
-	subdevice->len_chanlist = board->dio_channel_nbr;
-	subdevice->range_table = &range_digital;
-	subdevice->insn_bits = me_dio_insn_bits;
-	subdevice->insn_config = me_dio_insn_config;
-	subdevice->io_bits = 0;
-
-	printk(KERN_INFO "comedi%d: " ME_DRIVER_NAME " attached.\n",
-	       dev->minor);
+	s = &dev->subdevices[0];
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
+	s->n_chan = board->ai_channel_nbr;
+	s->maxdata = board->ai_resolution_mask;
+	s->len_chanlist = board->ai_channel_nbr;
+	s->range_table = board->ai_range_list;
+	s->cancel = me_ai_cancel;
+	s->insn_read = me_ai_insn_read;
+	s->do_cmdtest = me_ai_do_cmd_test;
+	s->do_cmd = me_ai_do_cmd;
+
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE | SDF_COMMON;
+	s->n_chan = board->ao_channel_nbr;
+	s->maxdata = board->ao_resolution_mask;
+	s->len_chanlist = board->ao_channel_nbr;
+	s->range_table = board->ao_range_list;
+	s->insn_read = me_ao_insn_read;
+	s->insn_write = me_ao_insn_write;
+
+	s = &dev->subdevices[2];
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITEABLE;
+	s->n_chan = board->dio_channel_nbr;
+	s->maxdata = 1;
+	s->len_chanlist = board->dio_channel_nbr;
+	s->range_table = &range_digital;
+	s->insn_bits = me_dio_insn_bits;
+	s->insn_config = me_dio_insn_config;
+	s->io_bits = 0;
+
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
+
 	return 0;
 }
 
@@ -818,7 +794,7 @@ static void me_detach(struct comedi_device *dev)
 static struct comedi_driver me_daq_driver = {
 	.driver_name	= "me_daq",
 	.module		= THIS_MODULE,
-	.attach		= me_attach,
+	.attach_pci	= me_attach_pci,
 	.detach		= me_detach,
 };
 
@@ -851,3 +827,4 @@ module_comedi_pci_driver(me_daq_driver, me_daq_pci_driver);
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(ME2600_FIRMWARE);
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index a93166d6a8f8..e27850f628ce 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -49,6 +49,8 @@
 
 /* #define USE_KMALLOC */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "mite.h"
 
 #include "comedi_fc.h"
@@ -59,52 +61,38 @@
 #define PCI_DAQ_SIZE		4096
 #define PCI_DAQ_SIZE_660X       8192
 
-struct mite_struct *mite_devices;
-EXPORT_SYMBOL(mite_devices);
-
 #define TOP_OF_PAGE(x) ((x)|(~(PAGE_MASK)))
 
-void mite_init(void)
+struct mite_struct *mite_alloc(struct pci_dev *pcidev)
 {
-	struct pci_dev *pcidev = NULL;
 	struct mite_struct *mite;
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_NI) {
-			unsigned i;
-
-			mite = kzalloc(sizeof(*mite), GFP_KERNEL);
-			if (!mite) {
-				printk(KERN_ERR "mite: allocation failed\n");
-				pci_dev_put(pcidev);
-				return;
-			}
-			spin_lock_init(&mite->lock);
-			mite->pcidev = pci_dev_get(pcidev);
-			for (i = 0; i < MAX_MITE_DMA_CHANNELS; ++i) {
-				mite->channels[i].mite = mite;
-				mite->channels[i].channel = i;
-				mite->channels[i].done = 1;
-			}
-			mite->next = mite_devices;
-			mite_devices = mite;
+	unsigned int i;
+
+	mite = kzalloc(sizeof(*mite), GFP_KERNEL);
+	if (mite) {
+		spin_lock_init(&mite->lock);
+		mite->pcidev = pcidev;
+		for (i = 0; i < MAX_MITE_DMA_CHANNELS; ++i) {
+			mite->channels[i].mite = mite;
+			mite->channels[i].channel = i;
+			mite->channels[i].done = 1;
 		}
 	}
+	return mite;
 }
+EXPORT_SYMBOL(mite_alloc);
 
 static void dump_chip_signature(u32 csigr_bits)
 {
-	printk(KERN_INFO "mite: version = %i, type = %i, mite mode = %i,"
-	       "interface mode = %i\n",
-	       mite_csigr_version(csigr_bits), mite_csigr_type(csigr_bits),
-	       mite_csigr_mmode(csigr_bits), mite_csigr_imode(csigr_bits));
-	printk(KERN_INFO "mite: num channels = %i, write post fifo depth = %i,"
-	       "wins = %i, iowins = %i\n",
-	       mite_csigr_dmac(csigr_bits), mite_csigr_wpdep(csigr_bits),
-	       mite_csigr_wins(csigr_bits), mite_csigr_iowins(csigr_bits));
+	pr_info("version = %i, type = %i, mite mode = %i, interface mode = %i\n",
+		mite_csigr_version(csigr_bits), mite_csigr_type(csigr_bits),
+		mite_csigr_mmode(csigr_bits), mite_csigr_imode(csigr_bits));
+	pr_info("num channels = %i, write post fifo depth = %i, wins = %i, iowins = %i\n",
+		mite_csigr_dmac(csigr_bits), mite_csigr_wpdep(csigr_bits),
+		mite_csigr_wins(csigr_bits), mite_csigr_iowins(csigr_bits));
 }
 
-unsigned mite_fifo_size(struct mite_struct *mite, unsigned channel)
+static unsigned mite_fifo_size(struct mite_struct *mite, unsigned channel)
 {
 	unsigned fcr_bits = readl(mite->mite_io_addr + MITE_FCR(channel));
 	unsigned empty_count = (fcr_bits >> 16) & 0xff;
@@ -121,7 +109,8 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 	unsigned unknown_dma_burst_bits;
 
 	if (comedi_pci_enable(mite->pcidev, "mite")) {
-		printk(KERN_ERR "error enabling mite and requesting io regions\n");
+		dev_err(&mite->pcidev->dev,
+			"error enabling mite and requesting io regions\n");
 		return -EIO;
 	}
 	pci_set_master(mite->pcidev);
@@ -130,11 +119,10 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 	mite->mite_phys_addr = addr;
 	mite->mite_io_addr = ioremap(addr, PCI_MITE_SIZE);
 	if (!mite->mite_io_addr) {
-		printk(KERN_ERR "Failed to remap mite io memory address\n");
+		dev_err(&mite->pcidev->dev,
+			"Failed to remap mite io memory address\n");
 		return -ENOMEM;
 	}
-	printk(KERN_INFO "MITE:0x%08llx mapped to %p ",
-	       (unsigned long long)mite->mite_phys_addr, mite->mite_io_addr);
 
 	addr = pci_resource_start(mite->pcidev, 1);
 	mite->daq_phys_addr = addr;
@@ -145,15 +133,15 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 	 */
 	mite->daq_io_addr = ioremap(mite->daq_phys_addr, length);
 	if (!mite->daq_io_addr) {
-		printk(KERN_ERR "Failed to remap daq io memory address\n");
+		dev_err(&mite->pcidev->dev,
+			"Failed to remap daq io memory address\n");
 		return -ENOMEM;
 	}
-	printk(KERN_INFO "DAQ:0x%08llx mapped to %p\n",
-	       (unsigned long long)mite->daq_phys_addr, mite->daq_io_addr);
 
 	if (use_iodwbsr_1) {
 		writel(0, mite->mite_io_addr + MITE_IODWBSR);
-		printk(KERN_INFO "mite: using I/O Window Base Size register 1\n");
+		dev_info(&mite->pcidev->dev,
+			 "using I/O Window Base Size register 1\n");
 		writel(mite->daq_phys_addr | WENAB |
 		       MITE_IODWBSR_1_WSIZE_bits(length),
 		       mite->mite_io_addr + MITE_IODWBSR_1);
@@ -178,9 +166,9 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 	csigr_bits = readl(mite->mite_io_addr + MITE_CSIGR);
 	mite->num_channels = mite_csigr_dmac(csigr_bits);
 	if (mite->num_channels > MAX_MITE_DMA_CHANNELS) {
-		printk(KERN_WARNING "mite: bug? chip claims to have %i dma "
-		       "channels. Setting to %i.\n",
-		       mite->num_channels, MAX_MITE_DMA_CHANNELS);
+		dev_warn(&mite->pcidev->dev,
+			 "mite: bug? chip claims to have %i dma channels. Setting to %i.\n",
+			 mite->num_channels, MAX_MITE_DMA_CHANNELS);
 		mite->num_channels = MAX_MITE_DMA_CHANNELS;
 	}
 	dump_chip_signature(csigr_bits);
@@ -193,9 +181,7 @@ int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
 		       mite->mite_io_addr + MITE_CHCR(i));
 	}
 	mite->fifo_size = mite_fifo_size(mite, 0);
-	printk(KERN_INFO "mite: fifo size is %i.\n", mite->fifo_size);
-	mite->used = 1;
-
+	dev_info(&mite->pcidev->dev, "fifo size is %i.\n", mite->fifo_size);
 	return 0;
 }
 EXPORT_SYMBOL(mite_setup2);
@@ -206,17 +192,6 @@ int mite_setup(struct mite_struct *mite)
 }
 EXPORT_SYMBOL(mite_setup);
 
-void mite_cleanup(void)
-{
-	struct mite_struct *mite, *next;
-
-	for (mite = mite_devices; mite; mite = next) {
-		pci_dev_put(mite->pcidev);
-		next = mite->next;
-		kfree(mite);
-	}
-}
-
 void mite_unsetup(struct mite_struct *mite)
 {
 	/* unsigned long offset, start, length; */
@@ -236,26 +211,43 @@ void mite_unsetup(struct mite_struct *mite)
 		comedi_pci_disable(mite->pcidev);
 		mite->mite_phys_addr = 0;
 	}
-
-	mite->used = 0;
 }
 EXPORT_SYMBOL(mite_unsetup);
 
-void mite_list_devices(void)
+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;
+	ring->hw_dev = get_device(&mite->pcidev->dev);
+	if (ring->hw_dev == NULL) {
+		kfree(ring);
+		return NULL;
+	}
+	ring->n_links = 0;
+	ring->descriptors = NULL;
+	ring->descriptors_dma_addr = 0;
+	return ring;
+};
+EXPORT_SYMBOL(mite_alloc_ring);
+
+void mite_free_ring(struct mite_dma_descriptor_ring *ring)
 {
-	struct mite_struct *mite, *next;
-
-	printk(KERN_INFO "Available NI device IDs:");
-	if (mite_devices)
-		for (mite = mite_devices; mite; mite = next) {
-			next = mite->next;
-			printk(KERN_INFO " 0x%04x", mite_device_id(mite));
-			if (mite->used)
-				printk(KERN_INFO "(used)");
+	if (ring) {
+		if (ring->descriptors) {
+			dma_free_coherent(ring->hw_dev,
+					  ring->n_links *
+					  sizeof(struct mite_dma_descriptor),
+					  ring->descriptors,
+					  ring->descriptors_dma_addr);
 		}
-	printk(KERN_INFO "\n");
-}
-EXPORT_SYMBOL(mite_list_devices);
+		put_device(ring->hw_dev);
+		kfree(ring);
+	}
+};
+EXPORT_SYMBOL(mite_free_ring);
 
 struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
 						   struct
@@ -317,7 +309,7 @@ void mite_dma_arm(struct mite_channel *mite_chan)
 	int chor;
 	unsigned long flags;
 
-	MDPRINTK("mite_dma_arm ch%i\n", channel);
+	MDPRINTK("mite_dma_arm ch%i\n", mite_chan->channel);
 	/*
 	 * memory barrier is intended to insure any twiddling with the buffer
 	 * is done before writing to the mite to arm dma transfer
@@ -365,7 +357,8 @@ int mite_buf_change(struct mite_dma_descriptor_ring *ring,
 			       n_links * sizeof(struct mite_dma_descriptor),
 			       &ring->descriptors_dma_addr, GFP_KERNEL);
 	if (!ring->descriptors) {
-		printk(KERN_ERR "mite: ring buffer allocation failed\n");
+		dev_err(async->subdevice->device->class_dev,
+			"mite: ring buffer allocation failed\n");
 		return -ENOMEM;
 	}
 	ring->n_links = n_links;
@@ -442,8 +435,7 @@ void mite_prep_dma(struct mite_channel *mite_chan,
 		mcr |= CR_PSIZE32;
 		break;
 	default:
-		printk(KERN_WARNING "mite: bug! invalid mem bit width for dma "
-		       "transfer\n");
+		pr_warn("bug! invalid mem bit width for dma transfer\n");
 		break;
 	}
 	writel(mcr, mite->mite_io_addr + MITE_MCR(mite_chan->channel));
@@ -462,8 +454,7 @@ void mite_prep_dma(struct mite_channel *mite_chan,
 		dcr |= CR_PSIZE32;
 		break;
 	default:
-		printk(KERN_WARNING "mite: bug! invalid dev bit width for dma "
-		       "transfer\n");
+		pr_warn("bug! invalid dev bit width for dma transfer\n");
 		break;
 	}
 	writel(dcr, mite->mite_io_addr + MITE_DCR(mite_chan->channel));
@@ -483,7 +474,7 @@ void mite_prep_dma(struct mite_channel *mite_chan,
 }
 EXPORT_SYMBOL(mite_prep_dma);
 
-u32 mite_device_bytes_transferred(struct mite_channel *mite_chan)
+static u32 mite_device_bytes_transferred(struct mite_channel *mite_chan)
 {
 	struct mite_struct *mite = mite_chan->mite;
 	return readl(mite->mite_io_addr + MITE_DAR(mite_chan->channel));
@@ -577,7 +568,8 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 	nbytes = mite_bytes_written_to_memory_lb(mite_chan);
 	if ((int)(mite_bytes_written_to_memory_ub(mite_chan) -
 		  old_alloc_count) > 0) {
-		printk("mite: DMA overwrite of free area\n");
+		dev_warn(async->subdevice->device->class_dev,
+			 "mite: DMA overwrite of free area\n");
 		async->events |= COMEDI_CB_OVERFLOW;
 		return -1;
 	}
@@ -621,7 +613,8 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 	    (int)(nbytes_ub - stop_count) > 0)
 		nbytes_ub = stop_count;
 	if ((int)(nbytes_ub - old_alloc_count) > 0) {
-		printk(KERN_ERR "mite: DMA underrun\n");
+		dev_warn(async->subdevice->device->class_dev,
+			 "mite: DMA underrun\n");
 		async->events |= COMEDI_CB_OVERFLOW;
 		return -1;
 	}
@@ -672,8 +665,6 @@ EXPORT_SYMBOL(mite_done);
 
 #ifdef DEBUG_MITE
 
-static void mite_decode(char **bit_str, unsigned int bits);
-
 /* names of bits in mite registers */
 
 static const char *const mite_CHOR_strings[] = {
@@ -743,86 +734,80 @@ static const char *const mite_CHSR_strings[] = {
 	"28", "lpauses", "30", "int",
 };
 
-void mite_dump_regs(struct mite_channel *mite_chan)
-{
-	unsigned long mite_io_addr =
-	    (unsigned long)mite_chan->mite->mite_io_addr;
-	unsigned long addr = 0;
-	unsigned long temp = 0;
-
-	printk(KERN_DEBUG "mite_dump_regs ch%i\n", mite_chan->channel);
-	printk(KERN_DEBUG "mite address is  =0x%08lx\n", mite_io_addr);
-
-	addr = mite_io_addr + MITE_CHOR(channel);
-	printk(KERN_DEBUG "mite status[CHOR]at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_CHOR_strings, temp);
-	addr = mite_io_addr + MITE_CHCR(channel);
-	printk(KERN_DEBUG "mite status[CHCR]at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_CHCR_strings, temp);
-	addr = mite_io_addr + MITE_TCR(channel);
-	printk(KERN_DEBUG "mite status[TCR] at 0x%08lx =0x%08x\n", addr,
-	       readl(addr));
-	addr = mite_io_addr + MITE_MCR(channel);
-	printk(KERN_DEBUG "mite status[MCR] at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_MCR_strings, temp);
-
-	addr = mite_io_addr + MITE_MAR(channel);
-	printk(KERN_DEBUG "mite status[MAR] at 0x%08lx =0x%08x\n", addr,
-	       readl(addr));
-	addr = mite_io_addr + MITE_DCR(channel);
-	printk(KERN_DEBUG "mite status[DCR] at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_DCR_strings, temp);
-	addr = mite_io_addr + MITE_DAR(channel);
-	printk(KERN_DEBUG "mite status[DAR] at 0x%08lx =0x%08x\n", addr,
-	       readl(addr));
-	addr = mite_io_addr + MITE_LKCR(channel);
-	printk(KERN_DEBUG "mite status[LKCR]at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_LKCR_strings, temp);
-	addr = mite_io_addr + MITE_LKAR(channel);
-	printk(KERN_DEBUG "mite status[LKAR]at 0x%08lx =0x%08x\n", addr,
-	       readl(addr));
-	addr = mite_io_addr + MITE_CHSR(channel);
-	printk(KERN_DEBUG "mite status[CHSR]at 0x%08lx =0x%08lx\n", addr,
-	       temp = readl(addr));
-	mite_decode(mite_CHSR_strings, temp);
-	addr = mite_io_addr + MITE_FCR(channel);
-	printk(KERN_DEBUG "mite status[FCR] at 0x%08lx =0x%08x\n\n", addr,
-	       readl(addr));
-}
-EXPORT_SYMBOL(mite_dump_regs);
-
-static void mite_decode(char **bit_str, unsigned int bits)
+static void mite_decode(const char *const *bit_str, unsigned int bits)
 {
 	int i;
 
 	for (i = 31; i >= 0; i--) {
 		if (bits & (1 << i))
-			printk(KERN_DEBUG " %s", bit_str[i]);
+			pr_debug(" %s\n", bit_str[i]);
 	}
-	printk(KERN_DEBUG "\n");
 }
-EXPORT_SYMBOL(mite_decode);
-#endif
 
-#ifdef MODULE
-int __init init_module(void)
+void mite_dump_regs(struct mite_channel *mite_chan)
 {
-	mite_init();
-	mite_list_devices();
+	void __iomem *mite_io_addr = mite_chan->mite->mite_io_addr;
+	unsigned int offset;
+	unsigned int value;
+	int channel = mite_chan->channel;
+
+	pr_debug("mite_dump_regs ch%i\n", channel);
+	pr_debug("mite address is  =%p\n", mite_io_addr);
+
+	offset = MITE_CHOR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[CHOR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_CHOR_strings, value);
+	offset = MITE_CHCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[CHCR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_CHCR_strings, value);
+	offset = MITE_TCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[TCR] at 0x%08x =0x%08x\n", offset, value);
+	offset = MITE_MCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[MCR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_MCR_strings, value);
+	offset = MITE_MAR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[MAR] at 0x%08x =0x%08x\n", offset, value);
+	offset = MITE_DCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[DCR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_DCR_strings, value);
+	offset = MITE_DAR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[DAR] at 0x%08x =0x%08x\n", offset, value);
+	offset = MITE_LKCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[LKCR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_LKCR_strings, value);
+	offset = MITE_LKAR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[LKAR] at 0x%08x =0x%08x\n", offset, value);
+	offset = MITE_CHSR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[CHSR] at 0x%08x =0x%08x\n", offset, value);
+	mite_decode(mite_CHSR_strings, value);
+	offset = MITE_FCR(channel);
+	value = readl(mite_io_addr + offset);
+	pr_debug("mite status[FCR] at 0x%08x =0x%08x\n", offset, value);
+}
+EXPORT_SYMBOL(mite_dump_regs);
+#endif
 
+static int __init mite_module_init(void)
+{
 	return 0;
 }
 
-void __exit cleanup_module(void)
+static void __exit mite_module_exit(void)
 {
-	mite_cleanup();
 }
-#endif
+
+module_init(mite_module_init);
+module_exit(mite_module_exit);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index 83f1b27a4720..255b8ba9c917 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -25,15 +25,16 @@
 #define _MITE_H_
 
 #include <linux/pci.h>
+#include <linux/log2.h>
 #include "../comedidev.h"
 
 /*  #define DEBUG_MITE */
 #define PCIMIO_COMPAT
 
 #ifdef DEBUG_MITE
-#define MDPRINTK(format, args...)	printk(format , ## args)
+#define MDPRINTK(format, args...)	pr_debug(format , ## args)
 #else
-#define MDPRINTK(format, args...)
+#define MDPRINTK(format, args...)	do { } while (0)
 #endif
 
 #define MAX_MITE_DMA_CHANNELS 8
@@ -61,15 +62,11 @@ struct mite_channel {
 };
 
 struct mite_struct {
-	struct mite_struct *next;
-	int used;
-
 	struct pci_dev *pcidev;
 	resource_size_t mite_phys_addr;
 	void __iomem *mite_io_addr;
 	resource_size_t daq_phys_addr;
 	void __iomem *daq_io_addr;
-
 	struct mite_channel channels[MAX_MITE_DMA_CHANNELS];
 	short channel_allocated[MAX_MITE_DMA_CHANNELS];
 	int num_channels;
@@ -77,41 +74,12 @@ struct mite_struct {
 	spinlock_t lock;
 };
 
-static inline 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;
-	ring->hw_dev = get_device(&mite->pcidev->dev);
-	if (ring->hw_dev == NULL) {
-		kfree(ring);
-		return NULL;
-	}
-	ring->n_links = 0;
-	ring->descriptors = NULL;
-	ring->descriptors_dma_addr = 0;
-	return ring;
-};
-
-static inline void mite_free_ring(struct mite_dma_descriptor_ring *ring)
-{
-	if (ring) {
-		if (ring->descriptors) {
-			dma_free_coherent(ring->hw_dev,
-					  ring->n_links *
-					  sizeof(struct mite_dma_descriptor),
-					  ring->descriptors,
-					  ring->descriptors_dma_addr);
-		}
-		put_device(ring->hw_dev);
-		kfree(ring);
-	}
-};
+struct mite_struct *mite_alloc(struct pci_dev *pcidev);
 
-extern struct mite_struct *mite_devices;
+static inline void mite_free(struct mite_struct *mite)
+{
+	kfree(mite);
+}
 
 static inline unsigned int mite_irq(struct mite_struct *mite)
 {
@@ -123,12 +91,11 @@ static inline unsigned int mite_device_id(struct mite_struct *mite)
 	return mite->pcidev->device;
 };
 
-void mite_init(void);
-void mite_cleanup(void);
 int mite_setup(struct mite_struct *mite);
 int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1);
 void mite_unsetup(struct mite_struct *mite);
-void mite_list_devices(void);
+struct mite_dma_descriptor_ring *mite_alloc_ring(struct mite_struct *mite);
+void mite_free_ring(struct mite_dma_descriptor_ring *ring);
 struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
 						   struct
 						   mite_dma_descriptor_ring
@@ -279,8 +246,9 @@ enum MITE_IODWBSR_bits {
 static inline unsigned MITE_IODWBSR_1_WSIZE_bits(unsigned size)
 {
 	unsigned order = 0;
-	while (size >>= 1)
-		++order;
+
+	BUG_ON(size == 0);
+	order = ilog2(size);
 	BUG_ON(order < 1);
 	return (order - 1) & 0x1f;
 }
@@ -427,12 +395,10 @@ static inline int CR_RL(unsigned int retry_limit)
 {
 	int value = 0;
 
-	while (retry_limit) {
-		retry_limit >>= 1;
-		value++;
-	}
+	if (retry_limit)
+		value = 1 + ilog2(retry_limit);
 	if (value > 0x7)
-		printk("comedi: bug! retry_limit too large\n");
+		value = 0x7;
 	return (value & 0x7) << 21;
 }
 
diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index b928b6763cd5..f8b7faefc961 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -353,7 +353,7 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF;
 	s->n_chan = 8;
diff --git a/drivers/staging/comedi/drivers/mpc8260cpm.c b/drivers/staging/comedi/drivers/mpc8260cpm.c
index a7fda8f01e8c..c0c33299b7f1 100644
--- a/drivers/staging/comedi/drivers/mpc8260cpm.c
+++ b/drivers/staging/comedi/drivers/mpc8260cpm.c
@@ -137,7 +137,7 @@ static int mpc8260cpm_attach(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < 4; i++) {
-		s = dev->subdevices + i;
+		s = &dev->subdevices[i];
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 		s->n_chan = 32;
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index eccbe1fb4f2c..4625cb4d07c6 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -204,8 +204,10 @@ static int multiq3_encoder_insn_read(struct comedi_device *dev,
 
 static void encoder_reset(struct comedi_device *dev)
 {
+	struct comedi_subdevice *s = &dev->subdevices[4];
 	int chan;
-	for (chan = 0; chan < dev->subdevices[4].n_chan; chan++) {
+
+	for (chan = 0; chan < s->n_chan; chan++) {
 		int control =
 		    MULTIQ3_CONTROL_MUST | MULTIQ3_AD_MUX_EN | (chan << 3);
 		outw(control, dev->iobase + MULTIQ3_CONTROL);
@@ -258,7 +260,7 @@ static int multiq3_attach(struct comedi_device *dev,
 	if (result < 0)
 		return result;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -267,7 +269,7 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->maxdata = 0x1fff;
 	s->range_table = &range_bipolar5;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -277,7 +279,7 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->maxdata = 0xfff;
 	s->range_table = &range_bipolar5;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* di subdevice */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -286,7 +288,7 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* do subdevice */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -296,7 +298,7 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->state = 0;
 
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	/* encoder (counter) subdevice */
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index a80c52fb2731..51295f32ee89 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -44,8 +44,11 @@ Updated: Sat, 25 Jan 2003 13:24:40 -0800
 #include <linux/interrupt.h>
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "mite.h"
 
+#define DRIVER_NAME "ni_6527"
+
 #define NI6527_DIO_SIZE 4096
 #define NI6527_MITE_SIZE 4096
 
@@ -76,16 +79,6 @@ Updated: Sat, 25 Jan 2003 13:24:40 -0800
 #define Rising_Edge_Detection_Enable(x)		(0x018+(x))
 #define Falling_Edge_Detection_Enable(x)	(0x020+(x))
 
-static int ni6527_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static void ni6527_detach(struct comedi_device *dev);
-static struct comedi_driver driver_ni6527 = {
-	.driver_name = "ni6527",
-	.module = THIS_MODULE,
-	.attach = ni6527_attach,
-	.detach = ni6527_detach,
-};
-
 struct ni6527_board {
 
 	int dev_id;
@@ -103,7 +96,6 @@ static const struct ni6527_board ni6527_boards[] = {
 	 },
 };
 
-#define n_ni6527_boards ARRAY_SIZE(ni6527_boards)
 #define this_board ((const struct ni6527_board *)dev->board_ptr)
 
 static DEFINE_PCI_DEVICE_TABLE(ni6527_pci_table) = {
@@ -122,8 +114,6 @@ struct ni6527_private {
 
 #define devpriv ((struct ni6527_private *)dev->private)
 
-static int ni6527_find_device(struct comedi_device *dev, int bus, int slot);
-
 static int ni6527_di_insn_config(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
@@ -212,7 +202,7 @@ static int ni6527_do_insn_bits(struct comedi_device *dev,
 static irqreturn_t ni6527_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 2;
+	struct comedi_subdevice *s = &dev->subdevices[2];
 	unsigned int status;
 
 	status = readb(devpriv->mite->daq_io_addr + Change_Status);
@@ -235,40 +225,20 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 			       struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_OTHER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and */
-	/*         are mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -364,36 +334,53 @@ static int ni6527_intr_insn_config(struct comedi_device *dev,
 	return 2;
 }
 
-static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static const struct ni6527_board *
+ni6527_find_boardinfo(struct pci_dev *pcidev)
+{
+	unsigned int dev_id = pcidev->device;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(ni6527_boards); n++) {
+		const struct ni6527_board *board = &ni6527_boards[n];
+		if (board->dev_id == dev_id)
+			return board;
+	}
+	return NULL;
+}
+
+static int __devinit ni6527_attach_pci(struct comedi_device *dev,
+				       struct pci_dev *pcidev)
 {
 	struct comedi_subdevice *s;
 	int ret;
 
-	printk(KERN_INFO "comedi%d: ni6527\n", dev->minor);
-
 	ret = alloc_private(dev, sizeof(struct ni6527_private));
 	if (ret < 0)
 		return ret;
 
-	ret = ni6527_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = ni6527_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
 
 	ret = mite_setup(devpriv->mite);
 	if (ret < 0) {
-		printk(KERN_ERR "comedi: error setting up mite\n");
+		dev_err(dev->class_dev, "error setting up mite\n");
 		return ret;
 	}
 
 	dev->board_name = this_board->name;
-	printk(KERN_INFO "comedi board: %s, ID=0x%02x\n", dev->board_name,
-		readb(devpriv->mite->daq_io_addr + ID_Register));
+	dev_info(dev->class_dev, "board: %s, ID=0x%02x\n", dev->board_name,
+		 readb(devpriv->mite->daq_io_addr + ID_Register));
 
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 24;
@@ -402,7 +389,7 @@ static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_config = ni6527_di_insn_config;
 	s->insn_bits = ni6527_di_insn_bits;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 24;
@@ -410,7 +397,7 @@ static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata = 1;
 	s->insn_bits = ni6527_do_insn_bits;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
@@ -432,9 +419,9 @@ static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	writeb(0x00, devpriv->mite->daq_io_addr + Master_Interrupt_Control);
 
 	ret = request_irq(mite_irq(devpriv->mite), ni6527_interrupt,
-			  IRQF_SHARED, "ni6527", dev);
+			  IRQF_SHARED, DRIVER_NAME, dev);
 	if (ret < 0)
-		printk(KERN_WARNING "comedi i6527 irq not available\n");
+		dev_warn(dev->class_dev, "irq not available\n");
 	else
 		dev->irq = mite_irq(devpriv->mite);
 
@@ -448,73 +435,37 @@ static void ni6527_detach(struct comedi_device *dev)
 		       devpriv->mite->daq_io_addr + Master_Interrupt_Control);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (devpriv && devpriv->mite)
+	if (devpriv && devpriv->mite) {
 		mite_unsetup(devpriv->mite);
-}
-
-static int ni6527_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < n_ni6527_boards; i++) {
-			if (mite_device_id(mite) == ni6527_boards[i].dev_id) {
-				dev->board_ptr = ni6527_boards + i;
-				devpriv->mite = mite;
-				return 0;
-			}
-		}
+		mite_free(devpriv->mite);
 	}
-	printk(KERN_ERR "comedi 6527: no device found\n");
-	mite_list_devices();
-	return -EIO;
 }
 
-static int __devinit driver_ni6527_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
+static struct comedi_driver ni6527_driver = {
+	.driver_name = DRIVER_NAME,
+	.module = THIS_MODULE,
+	.attach_pci = ni6527_attach_pci,
+	.detach = ni6527_detach,
+};
+
+static int __devinit ni6527_pci_probe(struct pci_dev *dev,
+				      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_ni6527);
+	return comedi_pci_auto_config(dev, &ni6527_driver);
 }
 
-static void __devexit driver_ni6527_pci_remove(struct pci_dev *dev)
+static void __devexit ni6527_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_ni6527_pci_driver = {
+static struct pci_driver ni6527_pci_driver = {
+	.name = DRIVER_NAME,
 	.id_table = ni6527_pci_table,
-	.probe = &driver_ni6527_pci_probe,
-	.remove = __devexit_p(&driver_ni6527_pci_remove)
+	.probe = ni6527_pci_probe,
+	.remove = __devexit_p(ni6527_pci_remove)
 };
-
-static int __init driver_ni6527_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_ni6527);
-	if (retval < 0)
-		return retval;
-
-	driver_ni6527_pci_driver.name = (char *)driver_ni6527.driver_name;
-	return pci_register_driver(&driver_ni6527_pci_driver);
-}
-
-static void __exit driver_ni6527_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_ni6527_pci_driver);
-	comedi_driver_unregister(&driver_ni6527);
-}
-
-module_init(driver_ni6527_init_module);
-module_exit(driver_ni6527_cleanup_module);
+module_comedi_pci_driver(ni6527_driver, ni6527_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index bce39f1ea36d..2a73ff57a2fb 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -55,6 +55,7 @@ except maybe the 6514.
 #include <linux/slab.h>
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "mite.h"
 
 #define NI6514_DIO_SIZE 4096
@@ -109,18 +110,7 @@ static inline unsigned Filter_Enable(unsigned port)
 #define OverflowIntEnable		0x02
 #define EdgeIntEnable			0x01
 
-static int ni_65xx_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static void ni_65xx_detach(struct comedi_device *dev);
-static struct comedi_driver driver_ni_65xx = {
-	.driver_name = "ni_65xx",
-	.module = THIS_MODULE,
-	.attach = ni_65xx_attach,
-	.detach = ni_65xx_detach,
-};
-
 struct ni_65xx_board {
-
 	int dev_id;
 	const char *name;
 	unsigned num_dio_ports;
@@ -325,8 +315,6 @@ static struct ni_65xx_subdevice_private *ni_65xx_alloc_subdevice_private(void)
 	return subdev_private;
 }
 
-static int ni_65xx_find_device(struct comedi_device *dev, int bus, int slot);
-
 static int ni_65xx_config_filter(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
@@ -453,11 +441,9 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 			writeb(bits,
 			       private(dev)->mite->daq_io_addr +
 			       Port_Data(port));
-/* printk("wrote 0x%x to port %i\n", bits, port); */
 		}
 		port_read_bits =
 		    readb(private(dev)->mite->daq_io_addr + Port_Data(port));
-/* printk("read 0x%x from port %i\n", port_read_bits, port); */
 		if (s->type == COMEDI_SUBD_DO && board(dev)->invert_outputs) {
 			/* Outputs inverted, so invert value read back from
 			 * DO subdevice.  (Does not apply to boards with DIO
@@ -478,7 +464,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 static irqreturn_t ni_65xx_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 2;
+	struct comedi_subdevice *s = &dev->subdevices[2];
 	unsigned int status;
 
 	status = readb(private(dev)->mite->daq_io_addr + Change_Status);
@@ -501,40 +487,20 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 				struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_OTHER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-	compatible */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -644,41 +610,55 @@ static int ni_65xx_intr_insn_config(struct comedi_device *dev,
 	return 2;
 }
 
-static int ni_65xx_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static const struct ni_65xx_board *
+ni_65xx_find_boardinfo(struct pci_dev *pcidev)
+{
+	unsigned int dev_id = pcidev->device;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(ni_65xx_boards); n++) {
+		const struct ni_65xx_board *board = &ni_65xx_boards[n];
+		if (board->dev_id == dev_id)
+			return board;
+	}
+	return NULL;
+}
+
+static int __devinit ni_65xx_attach_pci(struct comedi_device *dev,
+					struct pci_dev *pcidev)
 {
 	struct comedi_subdevice *s;
 	unsigned i;
 	int ret;
 
-	printk(KERN_INFO "comedi%d: ni_65xx:", dev->minor);
-
 	ret = alloc_private(dev, sizeof(struct ni_65xx_private));
 	if (ret < 0)
 		return ret;
 
-	ret = ni_65xx_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = ni_65xx_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+
+	private(dev)->mite = mite_alloc(pcidev);
+	if (!private(dev)->mite)
+		return -ENOMEM;
 
 	ret = mite_setup(private(dev)->mite);
 	if (ret < 0) {
-		printk(KERN_WARNING "error setting up mite\n");
+		dev_warn(dev->class_dev, "error setting up mite\n");
 		return ret;
 	}
 
 	dev->board_name = board(dev)->name;
 	dev->irq = mite_irq(private(dev)->mite);
-	printk(KERN_INFO " %s", dev->board_name);
-
-	printk(KERN_INFO " ID=0x%02x",
+	dev_info(dev->class_dev, "board: %s, ID=0x%02x", dev->board_name,
 	       readb(private(dev)->mite->daq_io_addr + ID_Register));
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	if (board(dev)->num_di_ports) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
@@ -696,7 +676,7 @@ static int ni_65xx_attach(struct comedi_device *dev,
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (board(dev)->num_do_ports) {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -713,7 +693,7 @@ static int ni_65xx_attach(struct comedi_device *dev,
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	if (board(dev)->num_dio_ports) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -737,7 +717,7 @@ static int ni_65xx_attach(struct comedi_device *dev,
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
@@ -772,11 +752,9 @@ static int ni_65xx_attach(struct comedi_device *dev,
 			  "ni_65xx", dev);
 	if (ret < 0) {
 		dev->irq = 0;
-		printk(KERN_WARNING " irq not available");
+		dev_warn(dev->class_dev, "irq not available\n");
 	}
 
-	printk("\n");
-
 	return 0;
 }
 
@@ -791,79 +769,46 @@ static void ni_65xx_detach(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (private(dev)) {
+		struct comedi_subdevice *s;
 		unsigned i;
+
 		for (i = 0; i < dev->n_subdevices; ++i) {
-			kfree(dev->subdevices[i].private);
-			dev->subdevices[i].private = NULL;
+			s = &dev->subdevices[i];
+			kfree(s->private);
+			s->private = NULL;
 		}
-		if (private(dev)->mite)
+		if (private(dev)->mite) {
 			mite_unsetup(private(dev)->mite);
-	}
-}
-
-static int ni_65xx_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < n_ni_65xx_boards; i++) {
-			if (mite_device_id(mite) == ni_65xx_boards[i].dev_id) {
-				dev->board_ptr = ni_65xx_boards + i;
-				private(dev)->mite = mite;
-				return 0;
-			}
+			mite_free(private(dev)->mite);
 		}
 	}
-	printk(KERN_WARNING "no device found\n");
-	mite_list_devices();
-	return -EIO;
 }
 
-static int __devinit driver_ni_65xx_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver ni_65xx_driver = {
+	.driver_name = "ni_65xx",
+	.module = THIS_MODULE,
+	.attach_pci = ni_65xx_attach_pci,
+	.detach = ni_65xx_detach,
+};
+
+static int __devinit ni_65xx_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_ni_65xx);
+	return comedi_pci_auto_config(dev, &ni_65xx_driver);
 }
 
-static void __devexit driver_ni_65xx_pci_remove(struct pci_dev *dev)
+static void __devexit ni_65xx_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_ni_65xx_pci_driver = {
+static struct pci_driver ni_65xx_pci_driver = {
+	.name = "ni_65xx",
 	.id_table = ni_65xx_pci_table,
-	.probe = &driver_ni_65xx_pci_probe,
-	.remove = __devexit_p(&driver_ni_65xx_pci_remove)
+	.probe = ni_65xx_pci_probe,
+	.remove = __devexit_p(ni_65xx_pci_remove)
 };
-
-static int __init driver_ni_65xx_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_ni_65xx);
-	if (retval < 0)
-		return retval;
-
-	driver_ni_65xx_pci_driver.name = (char *)driver_ni_65xx.driver_name;
-	return pci_register_driver(&driver_ni_65xx_pci_driver);
-}
-
-static void __exit driver_ni_65xx_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_ni_65xx_pci_driver);
-	comedi_driver_unregister(&driver_ni_65xx);
-}
-
-module_init(driver_ni_65xx_init_module);
-module_exit(driver_ni_65xx_cleanup_module);
+module_comedi_pci_driver(ni_65xx_driver, ni_65xx_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 5e863ff343dd..df2f3b0bab48 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -448,68 +448,46 @@ static inline struct ni_660x_private *private(struct comedi_device *dev)
 	return dev->private;
 }
 
-/* initialized in ni_660x_find_device() */
+/* initialized in ni_660x_attach_pci() */
 static inline const struct ni_660x_board *board(struct comedi_device *dev)
 {
 	return dev->board_ptr;
 }
 
-#define n_ni_660x_boards ARRAY_SIZE(ni_660x_boards)
-
-static int ni_660x_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
+static int ni_660x_attach_pci(struct comedi_device *dev,
+			      struct pci_dev *pcidev);
 static void ni_660x_detach(struct comedi_device *dev);
 static void init_tio_chip(struct comedi_device *dev, int chipset);
 static void ni_660x_select_pfi_output(struct comedi_device *dev,
 				      unsigned pfi_channel,
 				      unsigned output_select);
 
-static struct comedi_driver driver_ni_660x = {
+static struct comedi_driver ni_660x_driver = {
 	.driver_name = "ni_660x",
 	.module = THIS_MODULE,
-	.attach = ni_660x_attach,
+	.attach_pci = ni_660x_attach_pci,
 	.detach = ni_660x_detach,
 };
 
-static int __devinit driver_ni_660x_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static int __devinit ni_660x_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_ni_660x);
+	return comedi_pci_auto_config(dev, &ni_660x_driver);
 }
 
-static void __devexit driver_ni_660x_pci_remove(struct pci_dev *dev)
+static void __devexit ni_660x_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_ni_660x_pci_driver = {
+static struct pci_driver ni_660x_pci_driver = {
+	.name = "ni_660x",
 	.id_table = ni_660x_pci_table,
-	.probe = &driver_ni_660x_pci_probe,
-	.remove = __devexit_p(&driver_ni_660x_pci_remove)
+	.probe = ni_660x_pci_probe,
+	.remove = __devexit_p(ni_660x_pci_remove)
 };
+module_comedi_pci_driver(ni_660x_driver, ni_660x_pci_driver);
 
-static int __init driver_ni_660x_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_ni_660x);
-	if (retval < 0)
-		return retval;
-
-	driver_ni_660x_pci_driver.name = (char *)driver_ni_660x.driver_name;
-	return pci_register_driver(&driver_ni_660x_pci_driver);
-}
-
-static void __exit driver_ni_660x_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_ni_660x_pci_driver);
-	comedi_driver_unregister(&driver_ni_660x);
-}
-
-module_init(driver_ni_660x_init_module);
-module_exit(driver_ni_660x_cleanup_module);
-
-static int ni_660x_find_device(struct comedi_device *dev, int bus, int slot);
 static int ni_660x_set_pfi_routing(struct comedi_device *dev, unsigned chan,
 				   unsigned source);
 
@@ -748,8 +726,6 @@ static enum NI_660x_Register ni_gpct_to_660x_register(enum ni_gpct_register reg)
 		ni_660x_register = G3InterruptEnable;
 		break;
 	default:
-		printk(KERN_WARNING "%s: unhandled register 0x%x in switch.\n",
-		       __func__, reg);
 		BUG();
 		return 0;
 		break;
@@ -773,8 +749,6 @@ static inline void ni_660x_write_register(struct comedi_device *dev,
 		writel(bits, write_address);
 		break;
 	default:
-		printk(KERN_WARNING "%s: %s: bug! unhandled case (reg=0x%x) in switch.\n",
-		       __FILE__, __func__, reg);
 		BUG();
 		break;
 	}
@@ -796,8 +770,6 @@ static inline unsigned ni_660x_read_register(struct comedi_device *dev,
 		return readl(read_address);
 		break;
 	default:
-		printk(KERN_WARNING "%s: %s: bug! unhandled case (reg=0x%x) in switch.\n",
-		       __FILE__, __func__, reg);
 		BUG();
 		break;
 	}
@@ -893,8 +865,8 @@ static int ni_660x_request_mite_channel(struct comedi_device *dev,
 	return 0;
 }
 
-void ni_660x_release_mite_channel(struct comedi_device *dev,
-				  struct ni_gpct *counter)
+static void ni_660x_release_mite_channel(struct comedi_device *dev,
+					 struct ni_gpct *counter)
 {
 	unsigned long flags;
 
@@ -985,7 +957,7 @@ static irqreturn_t ni_660x_interrupt(int irq, void *d)
 	spin_lock_irqsave(&private(dev)->interrupt_lock, flags);
 	smp_mb();
 	for (i = 0; i < ni_660x_num_counters(dev); ++i) {
-		s = dev->subdevices + NI_660X_GPCT_SUBDEV(i);
+		s = &dev->subdevices[NI_660X_GPCT_SUBDEV(i)];
 		ni_660x_handle_gpct_interrupt(dev, s);
 	}
 	spin_unlock_irqrestore(&private(dev)->interrupt_lock, flags);
@@ -1062,28 +1034,43 @@ static void ni_660x_free_mite_rings(struct comedi_device *dev)
 	}
 }
 
-static int ni_660x_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static const struct ni_660x_board *
+ni_660x_find_boardinfo(struct pci_dev *pcidev)
+{
+	unsigned int dev_id = pcidev->device;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(ni_660x_boards); n++) {
+		const struct ni_660x_board *board = &ni_660x_boards[n];
+		if (board->dev_id == dev_id)
+			return board;
+	}
+	return NULL;
+}
+
+static int __devinit ni_660x_attach_pci(struct comedi_device *dev,
+					struct pci_dev *pcidev)
 {
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned i;
 	unsigned global_interrupt_config_bits;
 
-	printk(KERN_INFO "comedi%d: ni_660x: ", dev->minor);
-
 	ret = ni_660x_allocate_private(dev);
 	if (ret < 0)
 		return ret;
-	ret = ni_660x_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = ni_660x_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+	private(dev)->mite = mite_alloc(pcidev);
+	if (!private(dev)->mite)
+		return -ENOMEM;
 
 	dev->board_name = board(dev)->name;
 
 	ret = mite_setup2(private(dev)->mite, 1);
 	if (ret < 0) {
-		printk(KERN_WARNING "error setting up mite\n");
+		dev_warn(dev->class_dev, "error setting up mite\n");
 		return ret;
 	}
 	comedi_set_hw_dev(dev, &private(dev)->mite->pcidev->dev);
@@ -1091,17 +1078,15 @@ static int ni_660x_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-	printk(KERN_INFO " %s ", dev->board_name);
-
 	ret = comedi_alloc_subdevices(dev, 2 + NI_660X_MAX_NUM_COUNTERS);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* Old GENERAL-PURPOSE COUNTER/TIME (GPCT) subdevice, no longer used */
 	s->type = COMEDI_SUBD_UNUSED;
 
-	s = dev->subdevices + NI_660X_DIO_SUBDEV;
+	s = &dev->subdevices[NI_660X_DIO_SUBDEV];
 	/* DIGITAL I/O SUBDEVICE */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -1124,7 +1109,7 @@ static int ni_660x_attach(struct comedi_device *dev,
 	if (private(dev)->counter_dev == NULL)
 		return -ENOMEM;
 	for (i = 0; i < NI_660X_MAX_NUM_COUNTERS; ++i) {
-		s = dev->subdevices + NI_660X_GPCT_SUBDEV(i);
+		s = &dev->subdevices[NI_660X_GPCT_SUBDEV(i)];
 		if (i < ni_660x_num_counters(dev)) {
 			s->type = COMEDI_SUBD_COUNTER;
 			s->subdev_flags =
@@ -1174,7 +1159,7 @@ static int ni_660x_attach(struct comedi_device *dev,
 	ret = request_irq(mite_irq(private(dev)->mite), ni_660x_interrupt,
 			  IRQF_SHARED, "ni_660x", dev);
 	if (ret < 0) {
-		printk(KERN_WARNING " irq not available\n");
+		dev_warn(dev->class_dev, " irq not available\n");
 		return ret;
 	}
 	dev->irq = mite_irq(private(dev)->mite);
@@ -1183,7 +1168,7 @@ static int ni_660x_attach(struct comedi_device *dev,
 		global_interrupt_config_bits |= Cascade_Int_Enable_Bit;
 	ni_660x_write_register(dev, 0, global_interrupt_config_bits,
 			       GlobalInterruptConfigRegister);
-	printk(KERN_INFO "attached\n");
+	dev_info(dev->class_dev, "ni_660x: %s attached\n", dev->board_name);
 	return 0;
 }
 
@@ -1197,6 +1182,7 @@ static void ni_660x_detach(struct comedi_device *dev)
 		if (private(dev)->mite) {
 			ni_660x_free_mite_rings(dev);
 			mite_unsetup(private(dev)->mite);
+			mite_free(private(dev)->mite);
 		}
 	}
 }
@@ -1240,33 +1226,6 @@ static int ni_660x_GPCT_winsn(struct comedi_device *dev,
 	return ni_tio_winsn(subdev_to_counter(s), insn, data);
 }
 
-static int ni_660x_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-
-		for (i = 0; i < n_ni_660x_boards; i++) {
-			if (mite_device_id(mite) == ni_660x_boards[i].dev_id) {
-				dev->board_ptr = ni_660x_boards + i;
-				private(dev)->mite = mite;
-				return 0;
-			}
-		}
-	}
-	printk(KERN_WARNING "no device found\n");
-	mite_list_devices();
-	return -EIO;
-}
-
 static int ni_660x_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index 9c57618f2c5b..eac6dc047bb0 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -187,37 +187,22 @@ static int ni_670x_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int ni_670x_find_device(struct comedi_device *dev, int bus, int slot)
+static const struct ni_670x_board *
+ni_670x_find_boardinfo(struct pci_dev *pcidev)
 {
-	struct ni_670x_private *devpriv = dev->private;
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number
-			    || slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-
-		for (i = 0; i < ARRAY_SIZE(ni_670x_boards); i++) {
-			if (mite_device_id(mite) == ni_670x_boards[i].dev_id) {
-				dev->board_ptr = ni_670x_boards + i;
-				devpriv->mite = mite;
+	unsigned int dev_id = pcidev->device;
+	unsigned int n;
 
-				return 0;
-			}
-		}
+	for (n = 0; n < ARRAY_SIZE(ni_670x_boards); n++) {
+		const struct ni_670x_board *board = &ni_670x_boards[n];
+		if (board->dev_id == dev_id)
+			return board;
 	}
-	dev_warn(dev->class_dev, "no device found\n");
-	mite_list_devices();
-	return -EIO;
+	return NULL;
 }
 
-static int ni_670x_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static int __devinit ni_670x_attach_pci(struct comedi_device *dev,
+					struct pci_dev *pcidev)
 {
 	const struct ni_670x_board *thisboard;
 	struct ni_670x_private *devpriv;
@@ -229,10 +214,12 @@ static int ni_670x_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 	devpriv = dev->private;
-
-	ret = ni_670x_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = ni_670x_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
 	thisboard = comedi_board(dev);
 
 	ret = mite_setup(devpriv->mite);
@@ -241,13 +228,12 @@ static int ni_670x_attach(struct comedi_device *dev,
 		return ret;
 	}
 	dev->board_name = thisboard->name;
-	dev->irq = mite_irq(devpriv->mite);
 
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -271,7 +257,7 @@ static int ni_670x_attach(struct comedi_device *dev,
 	s->insn_write = &ni_670x_ao_winsn;
 	s->insn_read = &ni_670x_ao_rinsn;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -298,20 +284,20 @@ static void ni_670x_detach(struct comedi_device *dev)
 	struct comedi_subdevice *s;
 
 	if (dev->n_subdevices) {
-		s = dev->subdevices + 0;
+		s = &dev->subdevices[0];
 		if (s)
 			kfree(s->range_table_list);
 	}
-	if (devpriv && devpriv->mite)
+	if (devpriv && devpriv->mite) {
 		mite_unsetup(devpriv->mite);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
+		mite_free(devpriv->mite);
+	}
 }
 
 static struct comedi_driver ni_670x_driver = {
 	.driver_name	= "ni_670x",
 	.module		= THIS_MODULE,
-	.attach		= ni_670x_attach,
+	.attach_pci	= ni_670x_attach_pci,
 	.detach		= ni_670x_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index b53a4286f8cb..83950807b672 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -321,45 +321,23 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 	int startChan;
 	int i;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually
-	 * compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -832,7 +810,7 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_OTHER | SDF_CMD_READ;
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 62c8c44a8d28..93938cec93e7 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -358,7 +358,7 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -371,7 +371,7 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = &atao_ao_winsn;
 	s->insn_read = &atao_ao_rinsn;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -381,7 +381,7 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_bits = atao_dio_insn_bits;
 	s->insn_config = atao_dio_insn_config;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* caldac subdevice */
 	s->type = COMEDI_SUBD_CALIB;
 	s->subdev_flags = SDF_WRITABLE | SDF_INTERNAL;
@@ -390,7 +390,7 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_read = atao_calib_insn_read;
 	s->insn_write = atao_calib_insn_write;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* eeprom subdevice */
 	/* s->type=COMEDI_SUBD_EEPROM; */
 	s->type = COMEDI_SUBD_UNUSED;
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 6448373878ed..cac25572f6bb 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -489,7 +489,7 @@ static int ni_atmio_attach(struct comedi_device *dev,
 
 	/* generic E series stuff in ni_mio_common.c */
 
-	ret = ni_E_init(dev, it);
+	ret = ni_E_init(dev);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 2c78d3dd242a..e91a620f9db3 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -40,6 +40,7 @@ Devices: [National Instruments] AT-MIO-16 (atmio16), AT-MIO-16D (atmio16d)
 
 #include <linux/ioport.h>
 
+#include "comedi_fc.h"
 #include "8255.h"
 
 /* Configuration and Status Registers */
@@ -234,7 +235,7 @@ static void reset_atmio16d(struct comedi_device *dev)
 static irqreturn_t atmio16d_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 
 	comedi_buf_put(s->async, inw(dev->iobase + AD_FIFO_REG));
 
@@ -246,45 +247,26 @@ static int atmio16d_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
 {
-	int err = 0, tmp;
+	int err = 0;
 
-	/* make sure triggers are valid */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique & mutually compatible */
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -724,7 +706,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 	devpriv->dac1_coding = it->options[12];
 
 	/* setup sub-devices */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
@@ -749,7 +731,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 	}
 
 	/* ao subdevice */
-	s++;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 2;
@@ -775,7 +757,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 	}
 
 	/* Digital I/O */
-	s++;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 	s->n_chan = 8;
@@ -785,7 +767,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 
 	/* 8255 subdevice */
-	s++;
+	s = &dev->subdevices[3];
 	if (board->has_8255)
 		subdev_8255_init(dev, s, NULL, dev->iobase);
 	else
@@ -793,7 +775,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 
 /* don't yet know how to deal with counter/timers */
 #if 0
-	s++;
+	s = &dev->subdevices[4];
 	/* do */
 	s->type = COMEDI_SUBD_TIMER;
 	s->n_chan = 0;
@@ -807,9 +789,12 @@ static int atmio16d_attach(struct comedi_device *dev,
 static void atmio16d_detach(struct comedi_device *dev)
 {
 	const struct atmio16_board_t *board = comedi_board(dev);
+	struct comedi_subdevice *s;
 
-	if (dev->subdevices && board->has_8255)
-		subdev_8255_cleanup(dev, dev->subdevices + 3);
+	if (dev->subdevices && board->has_8255) {
+		s = &dev->subdevices[3];
+		subdev_8255_cleanup(dev, s);
+	}
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	reset_atmio16d(dev);
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 83016b411851..2ba0ade45c64 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -1,6 +1,6 @@
 /*
  *     comedi/drivers/ni_daq_700.c
- *     Driver for DAQCard-700 DIO only
+ *     Driver for DAQCard-700 DIO/AI
  *     copied from 8255
  *
  *     COMEDI - Linux Control and Measurement Device Interface
@@ -29,14 +29,25 @@ Author: Fred Brooks <nsaspook@nsaspook.com>,
   based on ni_daq_dio24 by Daniel Vecino Castel <dvecino@able.es>
 Devices: [National Instruments] PCMCIA DAQ-Card-700 (ni_daq_700)
 Status: works
-Updated: Thu, 21 Feb 2008 12:07:20 +0000
+Updated: Wed, 19 Sep 2012 12:07:20 +0000
 
-The daqcard-700 appears in Comedi as a single digital I/O subdevice with
-16 channels.  The channel 0 corresponds to the daqcard-700's output
+The daqcard-700 appears in Comedi as a  digital I/O subdevice (0) with
+16 channels and a analog input subdevice (1) with 16 single-ended channels.
+
+Digital:  The channel 0 corresponds to the daqcard-700's output
 port, bit 0; channel 8 corresponds to the input port, bit 0.
 
-Direction configuration: channels 0-7 output, 8-15 input (8225 device
+Digital direction configuration: channels 0-7 output, 8-15 input (8225 device
 emu as port A output, port B input, port C N/A).
+
+Analog: The input  range is 0 to 4095 for -10 to +10 volts 
+IRQ is assigned but not used.
+
+Version 0.1	Original DIO only driver
+Version 0.2	DIO and basic AI analog input support on 16 se channels
+
+Manuals:	Register level:	http://www.ni.com/pdf/manuals/340698.pdf
+		User Manual:	http://www.ni.com/pdf/manuals/320676d.pdf
 */
 
 #include <linux/interrupt.h>
@@ -51,16 +62,29 @@ emu as port A output, port B input, port C N/A).
 
 static struct pcmcia_device *pcmcia_cur_dev;
 
-struct dio700_board {
+struct daq700_board {
 	const char *name;
 };
 
-#define DIO_W		0x04
-#define DIO_R		0x05
-
-static int subdev_700_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+/* daqcard700 registers */
+#define DIO_W		0x04	/* WO 8bit */
+#define DIO_R		0x05	/* RO 8bit */
+#define CMD_R1		0x00	/* WO 8bit */
+#define CMD_R2		0x07	/* RW 8bit */
+#define CMD_R3		0x05	/* W0 8bit */
+#define STA_R1		0x00	/* RO 8bit */
+#define STA_R2		0x01	/* RO 8bit */
+#define ADFIFO_R	0x02	/* RO 16bit */
+#define ADCLEAR_R	0x01	/* WO 8bit */
+#define CDA_R0		0x08	/* RW 8bit */
+#define CDA_R1		0x09	/* RW 8bit */
+#define CDA_R2		0x0A	/* RW 8bit */
+#define CMO_R		0x0B	/* RO 8bit */
+#define TIC_R		0x06	/* WO 8bit */
+
+static int daq700_dio_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn, unsigned int *data)
 {
 	if (data[0]) {
 		s->state &= ~data[0];
@@ -76,7 +100,7 @@ static int subdev_700_insn(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int subdev_700_insn_config(struct comedi_device *dev,
+static int daq700_dio_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
@@ -97,9 +121,90 @@ static int subdev_700_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int dio700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int daq700_ai_rinsn(struct comedi_device *dev,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn, unsigned int *data)
 {
-	const struct dio700_board *thisboard = comedi_board(dev);
+	int n, i, chan;
+	int d;
+	unsigned int status;
+	enum { TIMEOUT = 100 };
+
+	chan = CR_CHAN(insn->chanspec);
+	/* write channel to multiplexer */
+	/* set mask scan bit high to disable scanning */
+	outb(chan | 0x80, dev->iobase + CMD_R1);
+
+	/* convert n samples */
+	for (n = 0; n < insn->n; n++) {
+		/* trigger conversion with out0 L to H */
+		outb(0x00, dev->iobase + CMD_R2); /* enable ADC conversions */
+		outb(0x30, dev->iobase + CMO_R); /* mode 0 out0 L, from H */
+		/* mode 1 out0 H, L to H, start conversion */
+		outb(0x32, dev->iobase + CMO_R);
+		/* wait for conversion to end */
+		for (i = 0; i < TIMEOUT; i++) {
+			status = inb(dev->iobase + STA_R2);
+			if ((status & 0x03) != 0) {
+				dev_info(dev->class_dev,
+					 "Overflow/run Error\n");
+				return -EOVERFLOW;
+			}
+			status = inb(dev->iobase + STA_R1);
+			if ((status & 0x02) != 0) {
+				dev_info(dev->class_dev, "Data Error\n");
+				return -ENODATA;
+			}
+			if ((status & 0x11) == 0x01) {
+				/* ADC conversion complete */
+				break;
+			}
+			udelay(1);
+		}
+		if (i == TIMEOUT) {
+			dev_info(dev->class_dev,
+				 "timeout during ADC conversion\n");
+			return -ETIMEDOUT;
+		}
+		/* read data */
+		d = inw(dev->iobase + ADFIFO_R);
+		/* mangle the data as necessary */
+		/* Bipolar Offset Binary: 0 to 4095 for -10 to +10 */
+		d &= 0x0fff;
+		d ^= 0x0800;
+		data[n] = d;
+	}
+	return n;
+}
+
+/*
+ * Data acquisition is enabled.
+ * The counter 0 output is high.
+ * The I/O connector pin CLK1 drives counter 1 source.
+ * Multiple-channel scanning is disabled.
+ * All interrupts are disabled.
+ * The analog input range is set to +-10 V
+ * The analog input mode is single-ended.
+ * The analog input circuitry is initialized to channel 0.
+ * The A/D FIFO is cleared.
+ */
+static void daq700_ai_config(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{			
+	unsigned long iobase = dev->iobase;
+
+	outb(0x80, iobase + CMD_R1);	/* disable scanning, ADC to chan 0 */
+	outb(0x00, iobase + CMD_R2);	/* clear all bits */
+	outb(0x00, iobase + CMD_R3);	/* set +-10 range */
+	outb(0x32, iobase + CMO_R);	/* config counter mode1, out0 to H */
+	outb(0x00, iobase + TIC_R);	/* clear counter interrupt */
+	outb(0x00, iobase + ADCLEAR_R);	/* clear the ADC FIFO */
+	inw(iobase + ADFIFO_R);		/* read 16bit junk from FIFO to clear */
+}
+
+static int daq700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	const struct daq700_board *thisboard = comedi_board(dev);
 	struct comedi_subdevice *s;
 	struct pcmcia_device *link;
 	int ret;
@@ -116,23 +221,33 @@ static int dio700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = thisboard->name;
 
-	ret = comedi_alloc_subdevices(dev, 1);
+	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
 		return ret;
 
 	/* DAQCard-700 dio */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type		= COMEDI_SUBD_DIO;
 	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
 	s->n_chan	= 16;
 	s->range_table	= &range_digital;
 	s->maxdata	= 1;
-	s->insn_bits	= subdev_700_insn;
-	s->insn_config	= subdev_700_insn_config;
-
+	s->insn_bits	= daq700_dio_insn_bits;
+	s->insn_config	= daq700_dio_insn_config;
 	s->state	= 0;
 	s->io_bits	= 0x00ff;
 
+	/* DAQCard-700 ai */
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_AI;
+	/* we support single-ended (ground)  */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND;
+	s->n_chan = 16;
+	s->maxdata = (1 << 12) - 1;
+	s->range_table = &range_bipolar10;
+	s->insn_read = daq700_ai_rinsn;
+	daq700_ai_config(dev, s);
+
 	dev_info(dev->class_dev, "%s: %s, io 0x%lx\n",
 		dev->driver->driver_name,
 		dev->board_name,
@@ -141,12 +256,12 @@ static int dio700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void dio700_detach(struct comedi_device *dev)
+static void daq700_detach(struct comedi_device *dev)
 {
 	/* nothing to cleanup */
 }
 
-static const struct dio700_board dio700_boards[] = {
+static const struct daq700_board daq700_boards[] = {
 	{
 		.name		= "daqcard-700",
 	}, {
@@ -154,17 +269,17 @@ static const struct dio700_board dio700_boards[] = {
 	},
 };
 
-static struct comedi_driver driver_dio700 = {
+static struct comedi_driver daq700_driver = {
 	.driver_name	= "ni_daq_700",
 	.module		= THIS_MODULE,
-	.attach		= dio700_attach,
-	.detach		= dio700_detach,
-	.board_name	= &dio700_boards[0].name,
-	.num_names	= ARRAY_SIZE(dio700_boards),
-	.offset		= sizeof(struct dio700_board),
+	.attach		= daq700_attach,
+	.detach		= daq700_detach,
+	.board_name	= &daq700_boards[0].name,
+	.num_names	= ARRAY_SIZE(daq700_boards),
+	.offset		= sizeof(struct daq700_board),
 };
 
-static int dio700_pcmcia_config_loop(struct pcmcia_device *p_dev,
+static int daq700_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
 {
 	if (p_dev->config_index == 0)
@@ -173,14 +288,14 @@ static int dio700_pcmcia_config_loop(struct pcmcia_device *p_dev,
 	return pcmcia_request_io(p_dev);
 }
 
-static int dio700_cs_attach(struct pcmcia_device *link)
+static int daq700_cs_attach(struct pcmcia_device *link)
 {
 	int ret;
 
 	link->config_flags |= CONF_ENABLE_IRQ | CONF_AUTO_AUDIO |
 		CONF_AUTO_SET_IO;
 
-	ret = pcmcia_loop_config(link, dio700_pcmcia_config_loop, NULL);
+	ret = pcmcia_loop_config(link, daq700_pcmcia_config_loop, NULL);
 	if (ret)
 		goto failed;
 
@@ -199,52 +314,53 @@ failed:
 	return ret;
 }
 
-static void dio700_cs_detach(struct pcmcia_device *link)
+static void daq700_cs_detach(struct pcmcia_device *link)
 {
 	pcmcia_disable_device(link);
 	pcmcia_cur_dev = NULL;
 }
 
-static const struct pcmcia_device_id dio700_cs_ids[] = {
+static const struct pcmcia_device_id daq700_cs_ids[] = {
 	PCMCIA_DEVICE_MANF_CARD(0x010b, 0x4743),
 	PCMCIA_DEVICE_NULL
 };
-MODULE_DEVICE_TABLE(pcmcia, dio700_cs_ids);
+MODULE_DEVICE_TABLE(pcmcia, daq700_cs_ids);
 
-static struct pcmcia_driver dio700_cs_driver = {
+static struct pcmcia_driver daq700_cs_driver = {
 	.name		= "ni_daq_700",
 	.owner		= THIS_MODULE,
-	.probe		= dio700_cs_attach,
-	.remove		= dio700_cs_detach,
-	.id_table	= dio700_cs_ids,
+	.probe		= daq700_cs_attach,
+	.remove		= daq700_cs_detach,
+	.id_table	= daq700_cs_ids,
 };
 
-static int __init dio700_cs_init(void)
+static int __init daq700_cs_init(void)
 {
 	int ret;
 
-	ret = comedi_driver_register(&driver_dio700);
+	ret = comedi_driver_register(&daq700_driver);
 	if (ret < 0)
 		return ret;
 
-	ret = pcmcia_register_driver(&dio700_cs_driver);
+	ret = pcmcia_register_driver(&daq700_cs_driver);
 	if (ret < 0) {
-		comedi_driver_unregister(&driver_dio700);
+		comedi_driver_unregister(&daq700_driver);
 		return ret;
 	}
 
 	return 0;
 }
-module_init(dio700_cs_init);
+module_init(daq700_cs_init);
 
-static void __exit dio700_cs_exit(void)
+static void __exit daq700_cs_exit(void)
 {
-	pcmcia_unregister_driver(&dio700_cs_driver);
-	comedi_driver_unregister(&driver_dio700);
+	pcmcia_unregister_driver(&daq700_cs_driver);
+	comedi_driver_unregister(&daq700_driver);
 }
-module_exit(dio700_cs_exit);
+module_exit(daq700_cs_exit);
 
 MODULE_AUTHOR("Fred Brooks <nsaspook@nsaspook.com>");
 MODULE_DESCRIPTION(
-	"Comedi driver for National Instruments PCMCIA DAQCard-700 DIO");
+	"Comedi driver for National Instruments PCMCIA DAQCard-700 DIO/AI");
+MODULE_VERSION("0.2.00");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index e27cae0eb8a2..0ca222bbcbe6 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -164,7 +164,7 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	/* 8255 dio */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	subdev_8255_init(dev, s, NULL, dev->iobase);
 
 	return 0;
@@ -172,8 +172,12 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void dio24_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 0);
+	struct comedi_subdevice *s;
+
+	if (dev->subdevices) {
+		s = &dev->subdevices[0];
+		subdev_8255_cleanup(dev, s);
+	}
 	if (thisboard->bustype != pcmcia_bustype && dev->iobase)
 		release_region(dev->iobase, DIO24_SIZE);
 	if (dev->irq)
@@ -304,7 +308,7 @@ MODULE_DESCRIPTION("Comedi driver for National Instruments "
 		   "PCMCIA DAQ-Card DIO-24");
 MODULE_LICENSE("GPL");
 
-struct pcmcia_driver dio24_cs_driver = {
+static struct pcmcia_driver dio24_cs_driver = {
 	.probe = dio24_cs_attach,
 	.remove = dio24_cs_detach,
 	.suspend = dio24_cs_suspend,
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index ab8b787c78bb..28b91a6c3789 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -73,9 +73,6 @@ NI manuals:
 
 */
 
-#undef LABPC_DEBUG
-/* #define LABPC_DEBUG    enable debugging messages */
-
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/io.h>
@@ -209,7 +206,13 @@ NI manuals:
 #define   INIT_A1_BITS	0x70
 #define COUNTER_B_BASE_REG	0x18
 
-static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it);
+enum scan_mode {
+	MODE_SINGLE_CHAN,
+	MODE_SINGLE_CHAN_INTERVAL,
+	MODE_MULT_CHAN_UP,
+	MODE_MULT_CHAN_DOWN,
+};
+
 static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
 static irqreturn_t labpc_interrupt(int irq, void *d);
 static int labpc_drain_fifo(struct comedi_device *dev);
@@ -240,12 +243,10 @@ static int labpc_eeprom_write_insn(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn,
 				   unsigned int *data);
-static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd);
+static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
+			     enum scan_mode scan_mode);
 #ifdef CONFIG_ISA_DMA_API
-static unsigned int labpc_suggest_transfer_size(struct comedi_cmd cmd);
-#endif
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-static int labpc_find_device(struct comedi_device *dev, int bus, int slot);
+static unsigned int labpc_suggest_transfer_size(const struct comedi_cmd *cmd);
 #endif
 static int labpc_dio_mem_callback(int dir, int port, int data,
 				  unsigned long arg);
@@ -261,13 +262,6 @@ static int labpc_eeprom_write(struct comedi_device *dev,
 static void write_caldac(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value);
 
-enum scan_mode {
-	MODE_SINGLE_CHAN,
-	MODE_SINGLE_CHAN_INTERVAL,
-	MODE_MULT_CHAN_UP,
-	MODE_MULT_CHAN_DOWN,
-};
-
 /* analog input ranges */
 #define NUM_LABPC_PLUS_AI_RANGES 16
 /* indicates unipolar ranges */
@@ -416,12 +410,12 @@ static inline void labpc_outb(unsigned int byte, unsigned long address)
 
 static inline unsigned int labpc_readb(unsigned long address)
 {
-	return readb((void *)address);
+	return readb((void __iomem *)address);
 }
 
 static inline void labpc_writeb(unsigned int byte, unsigned long address)
 {
-	writeb(byte, (void *)address);
+	writeb(byte, (void __iomem *)address);
 }
 
 static const struct labpc_board_struct labpc_boards[] = {
@@ -495,33 +489,14 @@ static const int sample_size = 2;
 
 #define devpriv ((struct labpc_private *)dev->private)
 
-static struct comedi_driver driver_labpc = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = labpc_attach,
-	.detach = labpc_common_detach,
-	.num_names = ARRAY_SIZE(labpc_boards),
-	.board_name = &labpc_boards[0].name,
-	.offset = sizeof(struct labpc_board_struct),
-};
-
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-static DEFINE_PCI_DEVICE_TABLE(labpc_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x161)},
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, labpc_pci_table);
-#endif /* CONFIG_COMEDI_PCI_DRIVERS */
-
 static inline int labpc_counter_load(struct comedi_device *dev,
 				     unsigned long base_address,
 				     unsigned int counter_number,
 				     unsigned int count, unsigned int mode)
 {
 	if (thisboard->memory_mapped_io)
-		return i8254_mm_load((void *)base_address, 0, counter_number,
-				     count, mode);
+		return i8254_mm_load((void __iomem *)base_address, 0,
+				     counter_number, count, mode);
 	else
 		return i8254_load(base_address, 0, counter_number, count, mode);
 }
@@ -538,25 +513,16 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 	short lsb, msb;
 	int ret;
 
-	printk(KERN_ERR "comedi%d: ni_labpc: %s, io 0x%lx", dev->minor,
-								thisboard->name,
-	       iobase);
-	if (irq)
-		printk(", irq %u", irq);
-	if (dma_chan)
-		printk(", dma %u", dma_chan);
-	printk("\n");
-
+	dev_info(dev->class_dev, "ni_labpc: %s\n", thisboard->name);
 	if (iobase == 0) {
-		printk(KERN_ERR "io base address is zero!\n");
+		dev_err(dev->class_dev, "io base address is zero!\n");
 		return -EINVAL;
 	}
 	/*  request io regions for isa boards */
 	if (thisboard->bustype == isa_bustype) {
 		/* check if io addresses are available */
-		if (!request_region(iobase, LABPC_SIZE,
-				    driver_labpc.driver_name)) {
-			printk(KERN_ERR "I/O port conflict\n");
+		if (!request_region(iobase, LABPC_SIZE, DRV_NAME)) {
+			dev_err(dev->class_dev, "I/O port conflict\n");
 			return -EIO;
 		}
 	}
@@ -588,8 +554,9 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 		    || thisboard->bustype == pcmcia_bustype)
 			isr_flags |= IRQF_SHARED;
 		if (request_irq(irq, labpc_interrupt, isr_flags,
-				driver_labpc.driver_name, dev)) {
-			printk(KERN_ERR "unable to allocate irq %u\n", irq);
+				DRV_NAME, dev)) {
+			dev_err(dev->class_dev, "unable to allocate irq %u\n",
+				irq);
 			return -EINVAL;
 		}
 	}
@@ -598,19 +565,21 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 #ifdef CONFIG_ISA_DMA_API
 	/* grab dma channel */
 	if (dma_chan > 3) {
-		printk(KERN_ERR " invalid dma channel %u\n", dma_chan);
+		dev_err(dev->class_dev, "invalid dma channel %u\n", dma_chan);
 		return -EINVAL;
 	} else if (dma_chan) {
 		/* allocate dma buffer */
 		devpriv->dma_buffer =
 		    kmalloc(dma_buffer_size, GFP_KERNEL | GFP_DMA);
 		if (devpriv->dma_buffer == NULL) {
-			printk(KERN_ERR " failed to allocate dma buffer\n");
+			dev_err(dev->class_dev,
+				"failed to allocate dma buffer\n");
 			return -ENOMEM;
 		}
-		if (request_dma(dma_chan, driver_labpc.driver_name)) {
-			printk(KERN_ERR " failed to allocate dma channel %u\n",
-			       dma_chan);
+		if (request_dma(dma_chan, DRV_NAME)) {
+			dev_err(dev->class_dev,
+				"failed to allocate dma channel %u\n",
+				dma_chan);
 			return -EINVAL;
 		}
 		devpriv->dma_chan = dma_chan;
@@ -628,7 +597,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 		return ret;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags =
@@ -643,7 +612,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 	s->cancel = labpc_cancel;
 
 	/* analog output */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (thisboard->has_ao) {
 		/*
 		 * Could provide command support, except it only has a
@@ -670,7 +639,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 	}
 
 	/* 8255 dio */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/*  if board uses io memory we have to give a custom callback
 	 * function to the 8255 driver */
 	if (thisboard->memory_mapped_io)
@@ -680,7 +649,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 		subdev_8255_init(dev, s, NULL, dev->iobase + DIO_BASE_REG);
 
 	/*  calibration subdevices for boards that have one */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	if (thisboard->register_layout == labpc_1200_layout) {
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
@@ -695,7 +664,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 		s->type = COMEDI_SUBD_UNUSED;
 
 	/* EEPROM */
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	if (thisboard->register_layout == labpc_1200_layout) {
 		s->type = COMEDI_SUBD_MEMORY;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
@@ -706,12 +675,6 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 
 		for (i = 0; i < EEPROM_SIZE; i++)
 			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
-#ifdef LABPC_DEBUG
-		printk(KERN_ERR " eeprom:");
-		for (i = 0; i < EEPROM_SIZE; i++)
-			printk(" %i:0x%x ", i, devpriv->eeprom_data[i]);
-		printk("\n");
-#endif
 	} else
 		s->type = COMEDI_SUBD_UNUSED;
 
@@ -719,14 +682,52 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 }
 EXPORT_SYMBOL_GPL(labpc_common_attach);
 
+static const struct labpc_board_struct *
+labpc_pci_find_boardinfo(struct pci_dev *pcidev)
+{
+	unsigned int device_id = pcidev->device;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(labpc_boards); n++) {
+		const struct labpc_board_struct *board = &labpc_boards[n];
+		if (board->bustype == pci_bustype &&
+		    board->device_id == device_id)
+			return board;
+	}
+	return NULL;
+}
+
+static int __devinit labpc_attach_pci(struct comedi_device *dev,
+				      struct pci_dev *pcidev)
+{
+	unsigned long iobase;
+	unsigned int irq;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_COMEDI_PCI_DRIVERS))
+		return -ENODEV;
+	ret = alloc_private(dev, sizeof(struct labpc_private));
+	if (ret < 0)
+		return ret;
+	dev->board_ptr = labpc_pci_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
+	ret = mite_setup(devpriv->mite);
+	if (ret < 0)
+		return ret;
+	iobase = (unsigned long)devpriv->mite->daq_io_addr;
+	irq = mite_irq(devpriv->mite);
+	return labpc_common_attach(dev, iobase, irq, 0);
+}
+
 static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	unsigned long iobase = 0;
 	unsigned int irq = 0;
 	unsigned int dma_chan = 0;
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-	int retval;
-#endif
 
 	/* allocate and initialize dev->private */
 	if (alloc_private(dev, sizeof(struct labpc_private)) < 0)
@@ -740,34 +741,26 @@ static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		irq = it->options[1];
 		dma_chan = it->options[2];
 #else
-		printk(KERN_ERR " this driver has not been built with ISA DMA "
-								"support.\n");
+		dev_err(dev->class_dev,
+			"ni_labpc driver has not been built with ISA DMA support.\n");
 		return -EINVAL;
 #endif
 		break;
 	case pci_bustype:
 #ifdef CONFIG_COMEDI_PCI_DRIVERS
-		retval = labpc_find_device(dev, it->options[0], it->options[1]);
-		if (retval < 0)
-			return retval;
-		retval = mite_setup(devpriv->mite);
-		if (retval < 0)
-			return retval;
-		iobase = (unsigned long)devpriv->mite->daq_io_addr;
-		irq = mite_irq(devpriv->mite);
+		dev_err(dev->class_dev,
+			"manual configuration of PCI board '%s' is not supported\n",
+			thisboard->name);
+		return -EINVAL;
 #else
-		printk(KERN_ERR " this driver has not been built with PCI "
-								"support.\n");
+		dev_err(dev->class_dev,
+			"ni_labpc driver has not been built with PCI support.\n");
 		return -EINVAL;
 #endif
 		break;
-	case pcmcia_bustype:
-		printk
-		    (" this driver does not support pcmcia cards, use ni_labpc_cs.o\n");
-		return -EINVAL;
-		break;
 	default:
-		printk(KERN_ERR "bug! couldn't determine board type\n");
+		dev_err(dev->class_dev,
+			"ni_labpc: bug! couldn't determine board type\n");
 		return -EINVAL;
 		break;
 	}
@@ -775,42 +768,14 @@ static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return labpc_common_attach(dev, iobase, irq, dma_chan);
 }
 
-/* adapted from ni_pcimio for finding mite based boards (pc-1200) */
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-static int labpc_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-/* if bus/slot are specified then make sure we have the right bus/slot */
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number
-			    || slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < driver_labpc.num_names; i++) {
-			if (labpc_boards[i].bustype != pci_bustype)
-				continue;
-			if (mite_device_id(mite) == labpc_boards[i].device_id) {
-				devpriv->mite = mite;
-/* fixup board pointer, in case we were using the dummy "ni_labpc" entry */
-				dev->board_ptr = &labpc_boards[i];
-				return 0;
-			}
-		}
-	}
-	printk(KERN_ERR "no device found\n");
-	mite_list_devices();
-	return -EIO;
-}
-#endif
-
 void labpc_common_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
+	struct comedi_subdevice *s;
+
+	if (dev->subdevices) {
+		s = &dev->subdevices[2];
+		subdev_8255_cleanup(dev, s);
+	}
 #ifdef CONFIG_ISA_DMA_API
 	/* only free stuff if it has been allocated by _attach */
 	kfree(devpriv->dma_buffer);
@@ -822,8 +787,10 @@ void labpc_common_detach(struct comedi_device *dev)
 	if (thisboard->bustype == isa_bustype && dev->iobase)
 		release_region(dev->iobase, LABPC_SIZE);
 #ifdef CONFIG_COMEDI_PCI_DRIVERS
-	if (devpriv->mite)
+	if (devpriv->mite) {
 		mite_unsetup(devpriv->mite);
+		mite_free(devpriv->mite);
+	}
 #endif
 };
 EXPORT_SYMBOL_GPL(labpc_common_detach);
@@ -868,21 +835,19 @@ static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
 	if (CR_CHAN(cmd->chanlist[0]) > CR_CHAN(cmd->chanlist[1]))
 		return MODE_MULT_CHAN_DOWN;
 
-	printk(KERN_ERR "ni_labpc: bug! this should never happen\n");
-
+	pr_err("ni_labpc: bug! cannot determine AI scan mode\n");
 	return 0;
 }
 
 static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
-				     const struct comedi_cmd *cmd)
+				     const struct comedi_cmd *cmd,
+				     enum scan_mode mode)
 {
-	int mode, channel, range, aref, i;
+	int channel, range, aref, i;
 
 	if (cmd->chanlist == NULL)
 		return 0;
 
-	mode = labpc_ai_scan_mode(cmd);
-
 	if (mode == MODE_SINGLE_CHAN)
 		return 0;
 
@@ -924,7 +889,8 @@ static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
 			}
 			break;
 		default:
-			printk(KERN_ERR "ni_labpc: bug! in chanlist check\n");
+			dev_err(dev->class_dev,
+				"ni_labpc: bug! in chanlist check\n");
 			return 1;
 			break;
 		}
@@ -945,9 +911,10 @@ static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
 	return 0;
 }
 
-static int labpc_use_continuous_mode(const struct comedi_cmd *cmd)
+static int labpc_use_continuous_mode(const struct comedi_cmd *cmd,
+				     enum scan_mode mode)
 {
-	if (labpc_ai_scan_mode(cmd) == MODE_SINGLE_CHAN)
+	if (mode == MODE_SINGLE_CHAN)
 		return 1;
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)
@@ -956,24 +923,25 @@ static int labpc_use_continuous_mode(const struct comedi_cmd *cmd)
 	return 0;
 }
 
-static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd)
+static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd,
+					    enum scan_mode mode)
 {
 	if (cmd->convert_src != TRIG_TIMER)
 		return 0;
 
-	if (labpc_ai_scan_mode(cmd) == MODE_SINGLE_CHAN &&
-	    cmd->scan_begin_src == TRIG_TIMER)
+	if (mode == MODE_SINGLE_CHAN && cmd->scan_begin_src == TRIG_TIMER)
 		return cmd->scan_begin_arg;
 
 	return cmd->convert_arg;
 }
 
-static void labpc_set_ai_convert_period(struct comedi_cmd *cmd, unsigned int ns)
+static void labpc_set_ai_convert_period(struct comedi_cmd *cmd,
+					enum scan_mode mode, unsigned int ns)
 {
 	if (cmd->convert_src != TRIG_TIMER)
 		return;
 
-	if (labpc_ai_scan_mode(cmd) == MODE_SINGLE_CHAN &&
+	if (mode == MODE_SINGLE_CHAN &&
 	    cmd->scan_begin_src == TRIG_TIMER) {
 		cmd->scan_begin_arg = ns;
 		if (cmd->convert_arg > cmd->scan_begin_arg)
@@ -982,25 +950,25 @@ static void labpc_set_ai_convert_period(struct comedi_cmd *cmd, unsigned int ns)
 		cmd->convert_arg = ns;
 }
 
-static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd)
+static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd,
+					enum scan_mode mode)
 {
 	if (cmd->scan_begin_src != TRIG_TIMER)
 		return 0;
 
-	if (labpc_ai_scan_mode(cmd) == MODE_SINGLE_CHAN &&
-	    cmd->convert_src == TRIG_TIMER)
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
 		return 0;
 
 	return cmd->scan_begin_arg;
 }
 
-static void labpc_set_ai_scan_period(struct comedi_cmd *cmd, unsigned int ns)
+static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
+				     enum scan_mode mode, unsigned int ns)
 {
 	if (cmd->scan_begin_src != TRIG_TIMER)
 		return;
 
-	if (labpc_ai_scan_mode(cmd) == MODE_SINGLE_CHAN &&
-	    cmd->convert_src == TRIG_TIMER)
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
 		return;
 
 	cmd->scan_begin_arg = ns;
@@ -1011,54 +979,33 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 {
 	int err = 0;
 	int tmp, tmp2;
-	int stop_mask;
+	unsigned int stop_mask;
+	enum scan_mode mode;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
 	stop_mask = TRIG_COUNT | TRIG_NONE;
 	if (thisboard->register_layout == labpc_1200_layout)
 		stop_mask |= TRIG_EXT;
-	cmd->stop_src &= stop_mask;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->stop_src, stop_mask);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_EXT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	/* can't have external stop and start triggers at once */
 	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
@@ -1133,14 +1080,15 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	tmp = cmd->convert_arg;
 	tmp2 = cmd->scan_begin_arg;
-	labpc_adc_timing(dev, cmd);
+	mode = labpc_ai_scan_mode(cmd);
+	labpc_adc_timing(dev, cmd, mode);
 	if (tmp != cmd->convert_arg || tmp2 != cmd->scan_begin_arg)
 		err++;
 
 	if (err)
 		return 4;
 
-	if (labpc_ai_chanlist_invalid(dev, cmd))
+	if (labpc_ai_chanlist_invalid(dev, cmd, mode))
 		return 5;
 
 	return 0;
@@ -1156,6 +1104,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	enum transfer_type xfer;
+	enum scan_mode mode;
 	unsigned long flags;
 
 	if (!dev->irq) {
@@ -1221,6 +1170,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	} else
 		xfer = fifo_not_empty_transfer;
 	devpriv->current_transfer = xfer;
+	mode = labpc_ai_scan_mode(cmd);
 
 	/*  setup command6 register for 1200 boards */
 	if (thisboard->register_layout == labpc_1200_layout) {
@@ -1245,7 +1195,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		else
 			devpriv->command6_bits &= ~A1_INTR_EN_BIT;
 		/*  are we scanning up or down through channels? */
-		if (labpc_ai_scan_mode(cmd) == MODE_MULT_CHAN_UP)
+		if (mode == MODE_MULT_CHAN_UP)
 			devpriv->command6_bits |= ADC_SCAN_UP_BIT;
 		else
 			devpriv->command6_bits &= ~ADC_SCAN_UP_BIT;
@@ -1256,19 +1206,18 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	/* setup channel list, etc (command1 register) */
 	devpriv->command1_bits = 0;
-	if (labpc_ai_scan_mode(cmd) == MODE_MULT_CHAN_UP)
+	if (mode == MODE_MULT_CHAN_UP)
 		channel = CR_CHAN(cmd->chanlist[cmd->chanlist_len - 1]);
 	else
 		channel = CR_CHAN(cmd->chanlist[0]);
 	/* munge channel bits for differential / scan disabled mode */
-	if (labpc_ai_scan_mode(cmd) != MODE_SINGLE_CHAN && aref == AREF_DIFF)
+	if (mode != MODE_SINGLE_CHAN && aref == AREF_DIFF)
 		channel *= 2;
 	devpriv->command1_bits |= ADC_CHAN_BITS(channel);
 	devpriv->command1_bits |= thisboard->ai_range_code[range];
 	devpriv->write_byte(devpriv->command1_bits, dev->iobase + COMMAND1_REG);
 	/* manual says to set scan enable bit on second pass */
-	if (labpc_ai_scan_mode(cmd) == MODE_MULT_CHAN_UP ||
-	    labpc_ai_scan_mode(cmd) == MODE_MULT_CHAN_DOWN) {
+	if (mode == MODE_MULT_CHAN_UP || mode == MODE_MULT_CHAN_DOWN) {
 		devpriv->command1_bits |= ADC_SCAN_EN_BIT;
 		/* need a brief delay before enabling scan, or scan
 		 * list will get screwed when you switch
@@ -1283,7 +1232,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->command4_bits |= EXT_CONVERT_DISABLE_BIT;
 	/* XXX should discard first scan when using interval scanning
 	 * since manual says it is not synced with scan clock */
-	if (labpc_use_continuous_mode(cmd) == 0) {
+	if (labpc_use_continuous_mode(cmd, mode) == 0) {
 		devpriv->command4_bits |= INTERVAL_SCAN_EN_BIT;
 		if (cmd->scan_begin_src == TRIG_EXT)
 			devpriv->command4_bits |= EXT_SCAN_EN_BIT;
@@ -1301,7 +1250,7 @@ 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);
+		labpc_adc_timing(dev, cmd, mode);
 		/*  load counter b0 in mode 3 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
 					 0, devpriv->divisor_b0, 3);
@@ -1311,7 +1260,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		}
 	}
 	/*  set up conversion pacing */
-	if (labpc_ai_convert_period(cmd)) {
+	if (labpc_ai_convert_period(cmd, mode)) {
 		/*  load counter a0 in mode 2 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
 					 0, devpriv->divisor_a0, 2);
@@ -1324,7 +1273,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				    dev->iobase + COUNTER_A_CONTROL_REG);
 
 	/*  set up scan pacing */
-	if (labpc_ai_scan_period(cmd)) {
+	if (labpc_ai_scan_period(cmd, mode)) {
 		/*  load counter b1 in mode 2 */
 		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
 					 1, devpriv->divisor_b1, 2);
@@ -1347,7 +1296,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		set_dma_addr(devpriv->dma_chan,
 			     virt_to_bus(devpriv->dma_buffer));
 		/*  set appropriate size of transfer */
-		devpriv->dma_transfer_size = labpc_suggest_transfer_size(*cmd);
+		devpriv->dma_transfer_size = labpc_suggest_transfer_size(cmd);
 		if (cmd->stop_src == TRIG_COUNT &&
 		    devpriv->count * sample_size < devpriv->dma_transfer_size) {
 			devpriv->dma_transfer_size =
@@ -1786,8 +1735,8 @@ static int labpc_eeprom_write_insn(struct comedi_device *dev,
 
 	/*  only allow writes to user area of eeprom */
 	if (channel < 16 || channel > 127) {
-		printk
-		    ("eeprom writes are only allowed to channels 16 through 127 (the pointer and user areas)");
+		dev_dbg(dev->class_dev,
+			"eeprom writes are only allowed to channels 16 through 127 (the pointer and user areas)\n");
 		return -EINVAL;
 	}
 
@@ -1800,13 +1749,13 @@ static int labpc_eeprom_write_insn(struct comedi_device *dev,
 
 #ifdef CONFIG_ISA_DMA_API
 /* utility function that suggests a dma transfer size in bytes */
-static unsigned int labpc_suggest_transfer_size(struct comedi_cmd cmd)
+static unsigned int labpc_suggest_transfer_size(const struct comedi_cmd *cmd)
 {
 	unsigned int size;
 	unsigned int freq;
 
-	if (cmd.convert_src == TRIG_TIMER)
-		freq = 1000000000 / cmd.convert_arg;
+	if (cmd->convert_src == TRIG_TIMER)
+		freq = 1000000000 / cmd->convert_arg;
 	/* return some default value */
 	else
 		freq = 0xffffffff;
@@ -1825,24 +1774,29 @@ static unsigned int labpc_suggest_transfer_size(struct comedi_cmd cmd)
 #endif
 
 /* figures out what counter values to use based on command */
-static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
+static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
+			     enum scan_mode mode)
 {
 	/* max value for 16 bit counter in mode 2 */
 	const int max_counter_value = 0x10000;
 	/* min value for 16 bit counter in mode 2 */
 	const int min_counter_value = 2;
 	unsigned int base_period;
+	unsigned int scan_period;
+	unsigned int convert_period;
 
 	/*
 	 * if both convert and scan triggers are TRIG_TIMER, then they
 	 * both rely on counter b0
 	 */
-	if (labpc_ai_convert_period(cmd) && labpc_ai_scan_period(cmd)) {
+	convert_period = labpc_ai_convert_period(cmd, mode);
+	scan_period = labpc_ai_scan_period(cmd, mode);
+	if (convert_period && scan_period) {
 		/*
 		 * pick the lowest b0 divisor value we can (for maximum input
 		 * clock speed on convert and scan counters)
 		 */
-		devpriv->divisor_b0 = (labpc_ai_scan_period(cmd) - 1) /
+		devpriv->divisor_b0 = (scan_period - 1) /
 		    (LABPC_TIMER_BASE * max_counter_value) + 1;
 		if (devpriv->divisor_b0 < min_counter_value)
 			devpriv->divisor_b0 = min_counter_value;
@@ -1856,25 +1810,19 @@ static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 		default:
 		case TRIG_ROUND_NEAREST:
 			devpriv->divisor_a0 =
-			    (labpc_ai_convert_period(cmd) +
-			     (base_period / 2)) / base_period;
+			    (convert_period + (base_period / 2)) / base_period;
 			devpriv->divisor_b1 =
-			    (labpc_ai_scan_period(cmd) +
-			     (base_period / 2)) / base_period;
+			    (scan_period + (base_period / 2)) / base_period;
 			break;
 		case TRIG_ROUND_UP:
 			devpriv->divisor_a0 =
-			    (labpc_ai_convert_period(cmd) + (base_period -
-							     1)) / base_period;
+			    (convert_period + (base_period - 1)) / base_period;
 			devpriv->divisor_b1 =
-			    (labpc_ai_scan_period(cmd) + (base_period -
-							  1)) / base_period;
+			    (scan_period + (base_period - 1)) / base_period;
 			break;
 		case TRIG_ROUND_DOWN:
-			devpriv->divisor_a0 =
-			    labpc_ai_convert_period(cmd) / base_period;
-			devpriv->divisor_b1 =
-			    labpc_ai_scan_period(cmd) / base_period;
+			devpriv->divisor_a0 = convert_period / base_period;
+			devpriv->divisor_b1 = scan_period / base_period;
 			break;
 		}
 		/*  make sure a0 and b1 values are acceptable */
@@ -1887,18 +1835,15 @@ static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 		if (devpriv->divisor_b1 > max_counter_value)
 			devpriv->divisor_b1 = max_counter_value;
 		/*  write corrected timings to command */
-		labpc_set_ai_convert_period(cmd,
+		labpc_set_ai_convert_period(cmd, mode,
 					    base_period * devpriv->divisor_a0);
-		labpc_set_ai_scan_period(cmd,
+		labpc_set_ai_scan_period(cmd, mode,
 					 base_period * devpriv->divisor_b1);
 		/*
 		 * if only one TRIG_TIMER is used, we can employ the generic
 		 * cascaded timing functions
 		 */
-	} else if (labpc_ai_scan_period(cmd)) {
-		unsigned int scan_period;
-
-		scan_period = labpc_ai_scan_period(cmd);
+	} else if (scan_period) {
 		/*
 		 * calculate cascaded counter values
 		 * that give desired scan timing
@@ -1908,11 +1853,8 @@ static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 					       &(devpriv->divisor_b0),
 					       &scan_period,
 					       cmd->flags & TRIG_ROUND_MASK);
-		labpc_set_ai_scan_period(cmd, scan_period);
-	} else if (labpc_ai_convert_period(cmd)) {
-		unsigned int convert_period;
-
-		convert_period = labpc_ai_convert_period(cmd);
+		labpc_set_ai_scan_period(cmd, mode, scan_period);
+	} else if (convert_period) {
 		/*
 		 * calculate cascaded counter values
 		 * that give desired conversion timing
@@ -1922,7 +1864,7 @@ static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 					       &(devpriv->divisor_b0),
 					       &convert_period,
 					       cmd->flags & TRIG_ROUND_MASK);
-		labpc_set_ai_convert_period(cmd, convert_period);
+		labpc_set_ai_convert_period(cmd, mode, convert_period);
 	}
 }
 
@@ -1930,10 +1872,10 @@ static int labpc_dio_mem_callback(int dir, int port, int data,
 				  unsigned long iobase)
 {
 	if (dir) {
-		writeb(data, (void *)(iobase + port));
+		writeb(data, (void __iomem *)(iobase + port));
 		return 0;
 	} else {
-		return readb((void *)(iobase + port));
+		return readb((void __iomem *)(iobase + port));
 	}
 }
 
@@ -2136,57 +2078,44 @@ static void write_caldac(struct comedi_device *dev, unsigned int channel,
 	devpriv->write_byte(devpriv->command5_bits, dev->iobase + COMMAND5_REG);
 }
 
+static struct comedi_driver labpc_driver = {
+	.driver_name = DRV_NAME,
+	.module = THIS_MODULE,
+	.attach = labpc_attach,
+	.attach_pci = labpc_attach_pci,
+	.detach = labpc_common_detach,
+	.num_names = ARRAY_SIZE(labpc_boards),
+	.board_name = &labpc_boards[0].name,
+	.offset = sizeof(struct labpc_board_struct),
+};
+
 #ifdef CONFIG_COMEDI_PCI_DRIVERS
-static int __devinit driver_labpc_pci_probe(struct pci_dev *dev,
-					    const struct pci_device_id *ent)
+static DEFINE_PCI_DEVICE_TABLE(labpc_pci_table) = {
+	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x161)},
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, labpc_pci_table);
+
+static int __devinit labpc_pci_probe(struct pci_dev *dev,
+				     const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_labpc);
+	return comedi_pci_auto_config(dev, &labpc_driver);
 }
 
-static void __devexit driver_labpc_pci_remove(struct pci_dev *dev)
+static void __devexit labpc_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_labpc_pci_driver = {
+static struct pci_driver labpc_pci_driver = {
+	.name = DRV_NAME,
 	.id_table = labpc_pci_table,
-	.probe = &driver_labpc_pci_probe,
-	.remove = __devexit_p(&driver_labpc_pci_remove)
+	.probe = labpc_pci_probe,
+	.remove = __devexit_p(labpc_pci_remove)
 };
-
-static int __init driver_labpc_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_labpc);
-	if (retval < 0)
-		return retval;
-
-	driver_labpc_pci_driver.name = (char *)driver_labpc.driver_name;
-	return pci_register_driver(&driver_labpc_pci_driver);
-}
-
-static void __exit driver_labpc_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_labpc_pci_driver);
-	comedi_driver_unregister(&driver_labpc);
-}
-
-module_init(driver_labpc_init_module);
-module_exit(driver_labpc_cleanup_module);
+module_comedi_pci_driver(labpc_driver, labpc_pci_driver);
 #else
-static int __init driver_labpc_init_module(void)
-{
-	return comedi_driver_register(&driver_labpc);
-}
-
-static void __exit driver_labpc_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_labpc);
-}
-
-module_init(driver_labpc_init_module);
-module_exit(driver_labpc_cleanup_module);
+module_comedi_driver(labpc_driver);
 #endif
 
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc_cs.c b/drivers/staging/comedi/drivers/ni_labpc_cs.c
index dbb61b6b3ed1..eb0417eb6d7d 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_cs.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_cs.c
@@ -270,7 +270,7 @@ MODULE_AUTHOR("Frank Mori Hess <fmhess@users.sourceforge.net>");
 MODULE_DESCRIPTION("Comedi driver for National Instruments Lab-PC");
 MODULE_LICENSE("GPL");
 
-struct pcmcia_driver labpc_cs_driver = {
+static struct pcmcia_driver labpc_cs_driver = {
 	.probe = labpc_cs_attach,
 	.remove = labpc_cs_detach,
 	.suspend = labpc_cs_suspend,
@@ -291,7 +291,7 @@ static void __exit exit_labpc_cs(void)
 	pcmcia_unregister_driver(&labpc_cs_driver);
 }
 
-int __init labpc_init_module(void)
+static int __init labpc_init_module(void)
 {
 	int ret;
 
@@ -302,7 +302,7 @@ int __init labpc_init_module(void)
 	return comedi_driver_register(&driver_labpc_cs);
 }
 
-void __exit labpc_exit_module(void)
+static void __exit labpc_exit_module(void)
 {
 	exit_labpc_cs();
 	comedi_driver_unregister(&driver_labpc_cs);
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index cf0e0d147f8c..3e5fdae93163 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -644,10 +644,10 @@ static void ni_release_ao_mite_channel(struct comedi_device *dev)
 #endif /*  PCIDMA */
 }
 
-void ni_release_gpct_mite_channel(struct comedi_device *dev,
-				  unsigned gpct_index)
-{
 #ifdef PCIDMA
+static void ni_release_gpct_mite_channel(struct comedi_device *dev,
+					 unsigned gpct_index)
+{
 	unsigned long flags;
 
 	BUG_ON(gpct_index >= NUM_GPCT);
@@ -663,8 +663,8 @@ void ni_release_gpct_mite_channel(struct comedi_device *dev,
 		mite_release_channel(mite_chan);
 	}
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
-#endif /*  PCIDMA */
 }
+#endif /*  PCIDMA */
 
 static void ni_release_cdo_mite_channel(struct comedi_device *dev)
 {
@@ -872,7 +872,7 @@ static irqreturn_t ni_E_interrupt(int irq, void *d)
 #ifdef PCIDMA
 static void ni_sync_ai_dma(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	unsigned long flags;
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
@@ -884,7 +884,7 @@ static void ni_sync_ai_dma(struct comedi_device *dev)
 static void mite_handle_b_linkc(struct mite_struct *mite,
 				struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AO_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AO_SUBDEV];
 	unsigned long flags;
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
@@ -942,7 +942,7 @@ static void ni_handle_eos(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static void shutdown_ai_command(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 
 #ifdef PCIDMA
 	ni_ai_drain_dma(dev);
@@ -984,8 +984,9 @@ static void handle_gpct_interrupt(struct comedi_device *dev,
 				  unsigned short counter_index)
 {
 #ifdef PCIDMA
-	struct comedi_subdevice *s =
-	    dev->subdevices + NI_GPCT_SUBDEV(counter_index);
+	struct comedi_subdevice *s;
+
+	s = &dev->subdevices[NI_GPCT_SUBDEV(counter_index)];
 
 	ni_tio_handle_interrupt(&devpriv->counter_dev->counters[counter_index],
 				s);
@@ -1018,7 +1019,7 @@ static void ack_a_interrupt(struct comedi_device *dev, unsigned short a_status)
 static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 			       unsigned ai_mite_status)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 
 	/* 67xx boards don't have ai subdevice, but their gpct0 might generate an a interrupt */
 	if (s->type == COMEDI_SUBD_UNUSED)
@@ -1150,7 +1151,7 @@ static void ack_b_interrupt(struct comedi_device *dev, unsigned short b_status)
 static void handle_b_interrupt(struct comedi_device *dev,
 			       unsigned short b_status, unsigned ao_mite_status)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AO_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AO_SUBDEV];
 	/* unsigned short ack=0; */
 #ifdef DEBUG_INTERRUPT
 	printk("ni_mio_common: interrupt: b_status=%04x m1_status=%08x\n",
@@ -1422,7 +1423,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 static void ni_handle_fifo_half_full(struct comedi_device *dev)
 {
 	int n;
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 
 	n = boardtype.ai_fifo_depth / 2;
 
@@ -1470,7 +1471,7 @@ static int ni_ai_drain_dma(struct comedi_device *dev)
 */
 static void ni_handle_fifo_dregs(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data[2];
 	u32 dl;
 	short fifo_empty;
@@ -1534,7 +1535,7 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 
 static void get_last_sample_611x(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data;
 	u32 dl;
 
@@ -1551,7 +1552,7 @@ static void get_last_sample_611x(struct comedi_device *dev)
 
 static void get_last_sample_6143(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	short data;
 	u32 dl;
 
@@ -1598,7 +1599,7 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AI_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AI_SUBDEV];
 	int retval;
 	unsigned long flags;
 
@@ -1637,7 +1638,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 
 static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 {
-	struct comedi_subdevice *s = dev->subdevices + NI_AO_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_AO_SUBDEV];
 	int retval;
 	unsigned long flags;
 
@@ -1765,20 +1766,18 @@ static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int ni_ai_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 	int count;
 
 	/*  lock to avoid race with interrupt handler */
-	if (in_interrupt() == 0)
-		spin_lock_irqsave(&dev->spinlock, flags);
+	spin_lock_irqsave(&dev->spinlock, flags);
 #ifndef PCIDMA
 	ni_handle_fifo_dregs(dev);
 #else
 	ni_sync_ai_dma(dev);
 #endif
 	count = s->async->buf_write_count - s->async->buf_read_count;
-	if (in_interrupt() == 0)
-		spin_unlock_irqrestore(&dev->spinlock, flags);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	return count;
 }
@@ -1880,7 +1879,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 	return insn->n;
 }
 
-void ni_prime_channelgain_list(struct comedi_device *dev)
+static void ni_prime_channelgain_list(struct comedi_device *dev)
 {
 	int i;
 	devpriv->stc_writew(dev, AI_CONVERT_Pulse, AI_Command_1_Register);
@@ -2165,61 +2164,38 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	int err = 0;
 	int tmp;
-	int sources;
+	unsigned int sources;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	if ((cmd->flags & CMDF_WRITE)) {
+	if ((cmd->flags & CMDF_WRITE))
 		cmd->flags &= ~CMDF_WRITE;
-	}
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	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);
 
-	tmp = cmd->convert_src;
 	sources = TRIG_TIMER | TRIG_EXT;
-	if ((boardtype.reg_type == ni_reg_611x)
-	    || (boardtype.reg_type == ni_reg_6143))
+	if (boardtype.reg_type == ni_reg_611x ||
+	    boardtype.reg_type == ni_reg_6143)
 		sources |= TRIG_NOW;
-	cmd->convert_src &= sources;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->convert_src, sources);
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->start_src != TRIG_NOW &&
-	    cmd->start_src != TRIG_INT && cmd->start_src != TRIG_EXT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_OTHER)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -3357,44 +3333,28 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	if ((cmd->flags & CMDF_WRITE) == 0) {
+	if ((cmd->flags & CMDF_WRITE) == 0)
 		cmd->flags |= CMDF_WRITE;
-	}
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_INT | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -3644,51 +3604,21 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 {
 	int err = 0;
 	int tmp;
-	int sources;
 	unsigned i;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	sources = TRIG_INT;
-	cmd->start_src &= sources;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique... */
-
-	if (cmd->start_src != TRIG_INT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_NONE)
-		err++;
-	/* ... and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -3852,7 +3782,7 @@ static int ni_cdio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static void handle_cdio_interrupt(struct comedi_device *dev)
 {
 	unsigned cdio_status;
-	struct comedi_subdevice *s = dev->subdevices + NI_DIO_SUBDEV;
+	struct comedi_subdevice *s = &dev->subdevices[NI_DIO_SUBDEV];
 #ifdef PCIDMA
 	unsigned long flags;
 #endif
@@ -4101,13 +4031,17 @@ static int ni_serial_sw_readwrite8(struct comedi_device *dev,
 
 static void mio_common_detach(struct comedi_device *dev)
 {
+	struct comedi_subdevice *s;
+
 	if (dev->private) {
 		if (devpriv->counter_dev) {
 			ni_gpct_device_destroy(devpriv->counter_dev);
 		}
 	}
-	if (dev->subdevices && boardtype.has_8255)
-		subdev_8255_cleanup(dev, dev->subdevices + NI_8255_DIO_SUBDEV);
+	if (dev->subdevices && boardtype.has_8255) {
+		s = &dev->subdevices[NI_8255_DIO_SUBDEV];
+		subdev_8255_cleanup(dev, s);
+	}
 }
 
 static void init_ao_67xx(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -4399,7 +4333,7 @@ static int ni_alloc_private(struct comedi_device *dev)
 	return 0;
 };
 
-static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
+static int ni_E_init(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
 	unsigned j;
@@ -4417,7 +4351,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* analog input subdevice */
 
-	s = dev->subdevices + NI_AI_SUBDEV;
+	s = &dev->subdevices[NI_AI_SUBDEV];
 	dev->read_subdev = s;
 	if (boardtype.n_adchan) {
 		s->type = COMEDI_SUBD_AI;
@@ -4449,7 +4383,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* analog output subdevice */
 
-	s = dev->subdevices + NI_AO_SUBDEV;
+	s = &dev->subdevices[NI_AO_SUBDEV];
 	if (boardtype.n_aochan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_DEGLITCH | SDF_GROUND;
@@ -4488,7 +4422,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* digital i/o subdevice */
 
-	s = dev->subdevices + NI_DIO_SUBDEV;
+	s = &dev->subdevices[NI_DIO_SUBDEV];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 	s->maxdata = 1;
@@ -4516,7 +4450,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* 8255 device */
-	s = dev->subdevices + NI_8255_DIO_SUBDEV;
+	s = &dev->subdevices[NI_8255_DIO_SUBDEV];
 	if (boardtype.has_8255) {
 		subdev_8255_init(dev, s, ni_8255_callback, (unsigned long)dev);
 	} else {
@@ -4524,11 +4458,11 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* formerly general purpose counter/timer device, but no longer used */
-	s = dev->subdevices + NI_UNUSED_SUBDEV;
+	s = &dev->subdevices[NI_UNUSED_SUBDEV];
 	s->type = COMEDI_SUBD_UNUSED;
 
 	/* calibration subdevice -- ai and ao */
-	s = dev->subdevices + NI_CALIBRATION_SUBDEV;
+	s = &dev->subdevices[NI_CALIBRATION_SUBDEV];
 	s->type = COMEDI_SUBD_CALIB;
 	if (boardtype.reg_type & ni_reg_m_series_mask) {
 		/*  internal PWM analog output used for AI nonlinearity calibration */
@@ -4551,7 +4485,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* EEPROM */
-	s = dev->subdevices + NI_EEPROM_SUBDEV;
+	s = &dev->subdevices[NI_EEPROM_SUBDEV];
 	s->type = COMEDI_SUBD_MEMORY;
 	s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
 	s->maxdata = 0xff;
@@ -4564,7 +4498,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* PFI */
-	s = dev->subdevices + NI_PFI_DIO_SUBDEV;
+	s = &dev->subdevices[NI_PFI_DIO_SUBDEV];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	if (boardtype.reg_type & ni_reg_m_series_mask) {
@@ -4586,7 +4520,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	ni_set_bits(dev, IO_Bidirection_Pin_Register, ~0, 0);
 
 	/* cs5529 calibration adc */
-	s = dev->subdevices + NI_CS5529_CALIBRATION_SUBDEV;
+	s = &dev->subdevices[NI_CS5529_CALIBRATION_SUBDEV];
 	if (boardtype.reg_type & ni_reg_67xx_mask) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_INTERNAL;
@@ -4602,7 +4536,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* Serial */
-	s = dev->subdevices + NI_SERIAL_SUBDEV;
+	s = &dev->subdevices[NI_SERIAL_SUBDEV];
 	s->type = COMEDI_SUBD_SERIAL;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan = 1;
@@ -4612,7 +4546,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->serial_hw_mode = 0;
 
 	/* RTSI */
-	s = dev->subdevices + NI_RTSI_SUBDEV;
+	s = &dev->subdevices[NI_RTSI_SUBDEV];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan = 8;
@@ -4633,7 +4567,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 							NUM_GPCT);
 	/* General purpose counters */
 	for (j = 0; j < NUM_GPCT; ++j) {
-		s = dev->subdevices + NI_GPCT_SUBDEV(j);
+		s = &dev->subdevices[NI_GPCT_SUBDEV(j)];
 		s->type = COMEDI_SUBD_COUNTER;
 		s->subdev_flags =
 		    SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL | SDF_CMD_READ
@@ -4659,7 +4593,7 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/* Frequency output */
-	s = dev->subdevices + NI_FREQ_OUT_SUBDEV;
+	s = &dev->subdevices[NI_FREQ_OUT_SUBDEV];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 1;
@@ -4669,7 +4603,8 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_config = &ni_freq_out_insn_config;
 
 	/* ai configuration */
-	ni_ai_reset(dev, dev->subdevices + NI_AI_SUBDEV);
+	s = &dev->subdevices[NI_AI_SUBDEV];
+	ni_ai_reset(dev, s);
 	if ((boardtype.reg_type & ni_reg_6xxx_mask) == 0) {
 		/*  BEAM is this needed for PCI-6143 ?? */
 		devpriv->clock_and_fout =
@@ -4688,7 +4623,8 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 			    Clock_and_FOUT_Register);
 
 	/* analog output configuration */
-	ni_ao_reset(dev, dev->subdevices + NI_AO_SUBDEV);
+	s = &dev->subdevices[NI_AO_SUBDEV];
+	ni_ao_reset(dev, s);
 
 	if (dev->irq) {
 		devpriv->stc_writew(dev,
diff --git a/drivers/staging/comedi/drivers/ni_mio_cs.c b/drivers/staging/comedi/drivers/ni_mio_cs.c
index b85765d266c2..ca4f8e06e75b 100644
--- a/drivers/staging/comedi/drivers/ni_mio_cs.c
+++ b/drivers/staging/comedi/drivers/ni_mio_cs.c
@@ -382,7 +382,7 @@ static int mio_cs_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->stc_writel = &win_out2;
 	devpriv->stc_readl = &win_in2;
 
-	ret = ni_E_init(dev, it);
+	ret = ni_E_init(dev);
 
 	if (ret < 0)
 		return ret;
@@ -421,7 +421,7 @@ MODULE_AUTHOR("David A. Schleef <ds@schleef.org>");
 MODULE_DESCRIPTION("Comedi driver for National Instruments DAQCard E series");
 MODULE_LICENSE("GPL");
 
-struct pcmcia_driver ni_mio_cs_driver = {
+static struct pcmcia_driver ni_mio_cs_driver = {
 	.probe = &cs_attach,
 	.remove = &cs_detach,
 	.suspend = &mio_cs_suspend,
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 0a55de968039..bc9313ec985c 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -1,8 +1,6 @@
 /*
     comedi/drivers/ni_pcidio.c
-    driver for National Instruments PCI-DIO-96/PCI-6508
-		National Instruments PCI-DIO-32HS
-		National Instruments PCI-6503
+    driver for National Instruments PCI-DIO-32HS
 
     COMEDI - Linux Control and Measurement Device Interface
     Copyright (C) 1999,2002 David A. Schleef <ds@schleef.org>
@@ -24,17 +22,14 @@
 */
 /*
 Driver: ni_pcidio
-Description: National Instruments PCI-DIO32HS, PCI-DIO96, PCI-6533, PCI-6503
+Description: National Instruments PCI-DIO32HS, PCI-6533
 Author: ds
 Status: works
-Devices: [National Instruments] PCI-DIO-32HS (ni_pcidio), PXI-6533,
-  PCI-DIO-96, PCI-DIO-96B, PXI-6508, PCI-6503, PCI-6503B, PCI-6503X,
-  PXI-6503, PCI-6533, PCI-6534
+Devices: [National Instruments] PCI-DIO-32HS (ni_pcidio)
+	 [National Instruments] PXI-6533, PCI-6533 (pxi-6533)
+	 [National Instruments] PCI-6534 (pci-6534)
 Updated: Mon, 09 Jan 2012 14:27:23 +0000
 
-The DIO-96 appears as four 8255 subdevices.  See the 8255
-driver notes for details.
-
 The DIO32HS board appears as one subdevice, with 32 channels.
 Each channel is individually I/O configurable.  The channel order
 is 0=A0, 1=A1, 2=A2, ... 8=B0, 16=C0, 24=D0.  The driver only
@@ -56,49 +51,28 @@ it are contained in the
 comedi_nonfree_firmware tarball available from http://www.comedi.org
 */
 
-/*
-   This driver is for both the NI PCI-DIO-32HS and the PCI-DIO-96,
-   which have very different architectures.  But, since the '96 is
-   so simple, it is included here.
-
-   Manuals (available from ftp://ftp.natinst.com/support/manuals)
-
-	320938c.pdf	PCI-DIO-96/PXI-6508/PCI-6503 User Manual
-	321464b.pdf	AT/PCI-DIO-32HS User Manual
-	341329A.pdf	PCI-6533 Register-Level Programmer Manual
-	341330A.pdf	DAQ-DIO Technical Reference Manual
-
- */
-
 #define USE_DMA
 /* #define DEBUG 1 */
 /* #define DEBUG_FLAGS */
 
 #include <linux/interrupt.h>
 #include <linux/sched.h>
+#include <linux/firmware.h>
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
 #include "mite.h"
-#include "8255.h"
 
 #undef DPRINTK
 #ifdef DEBUG
-#define DPRINTK(format, args...)	printk(format, ## args)
+#define DPRINTK(format, args...) pr_debug(format, ## args)
 #else
-#define DPRINTK(format, args...)
+#define DPRINTK(format, args...) do { } while (0)
 #endif
 
 #define PCI_DIO_SIZE 4096
 #define PCI_MITE_SIZE 4096
 
-/* defines for the PCI-DIO-96 */
-
-#define NIDIO_8255_BASE(x)	((x)*4)
-#define NIDIO_A 0
-#define NIDIO_B 4
-#define NIDIO_C 8
-#define NIDIO_D 12
-
 /* defines for the PCI-DIO-32HS */
 
 #define Window_Address			4	/* W */
@@ -258,6 +232,14 @@ static inline unsigned secondary_DMAChannel_bits(unsigned channel)
 #define Protocol_Register_8		88	/* 32 bit */
 #define StartDelay			Protocol_Register_8
 
+/* Firmware files for PCI-6524 */
+#define FW_PCI_6534_MAIN		"ni6534a.bin"
+#define FW_PCI_6534_SCARAB_DI		"niscrb01.bin"
+#define FW_PCI_6534_SCARAB_DO		"niscrb02.bin"
+MODULE_FIRMWARE(FW_PCI_6534_MAIN);
+MODULE_FIRMWARE(FW_PCI_6534_SCARAB_DI);
+MODULE_FIRMWARE(FW_PCI_6534_SCARAB_DO);
+
 enum pci_6534_firmware_registers {	/* 16 bit */
 	Firmware_Control_Register = 0x100,
 	Firmware_Status_Register = 0x104,
@@ -297,76 +279,23 @@ static int ni_pcidio_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s);
 
 struct nidio_board {
-
 	int dev_id;
 	const char *name;
-	int n_8255;
-	unsigned int is_diodaq:1;
 	unsigned int uses_firmware:1;
 };
 
 static const struct nidio_board nidio_boards[] = {
 	{
-	 .dev_id = 0x1150,
-	 .name = "pci-dio-32hs",
-	 .n_8255 = 0,
-	 .is_diodaq = 1,
-	 },
-	{
-	 .dev_id = 0x1320,
-	 .name = "pxi-6533",
-	 .n_8255 = 0,
-	 .is_diodaq = 1,
-	 },
-	{
-	 .dev_id = 0x12b0,
-	 .name = "pci-6534",
-	 .n_8255 = 0,
-	 .is_diodaq = 1,
-	 .uses_firmware = 1,
-	 },
-	{
-	 .dev_id = 0x0160,
-	 .name = "pci-dio-96",
-	 .n_8255 = 4,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x1630,
-	 .name = "pci-dio-96b",
-	 .n_8255 = 4,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x13c0,
-	 .name = "pxi-6508",
-	 .n_8255 = 4,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x0400,
-	 .name = "pci-6503",
-	 .n_8255 = 1,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x1250,
-	 .name = "pci-6503b",
-	 .n_8255 = 1,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x17d0,
-	 .name = "pci-6503x",
-	 .n_8255 = 1,
-	 .is_diodaq = 0,
-	 },
-	{
-	 .dev_id = 0x1800,
-	 .name = "pxi-6503",
-	 .n_8255 = 1,
-	 .is_diodaq = 0,
-	 },
+		.dev_id		= 0x1150,
+		.name		= "pci-dio-32hs",
+	}, {
+		.dev_id		= 0x1320,
+		.name		= "pxi-6533",
+	}, {
+		.dev_id		= 0x12b0,
+		.name		= "pci-6534",
+		.uses_firmware	= 1,
+	},
 };
 
 #define n_nidio_boards ARRAY_SIZE(nidio_boards)
@@ -442,17 +371,8 @@ static void ni_pcidio_release_di_mite_channel(struct comedi_device *dev)
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 }
 
-static int nidio96_8255_cb(int dir, int port, int data, unsigned long iobase)
-{
-	if (dir) {
-		writeb(data, (void *)(iobase + port));
-		return 0;
-	} else {
-		return readb((void *)(iobase + port));
-	}
-}
-
-void ni_pcidio_event(struct comedi_device *dev, struct comedi_subdevice *s)
+static void ni_pcidio_event(struct comedi_device *dev,
+			    struct comedi_subdevice *s)
 {
 	if (s->
 	    async->events & (COMEDI_CB_EOA | COMEDI_CB_ERROR |
@@ -480,7 +400,7 @@ static int ni_pcidio_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 static irqreturn_t nidio_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	struct comedi_async *async = s->async;
 	struct mite_struct *mite = devpriv->mite;
 
@@ -511,18 +431,12 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 	ni_pcidio_print_flags(flags);
 	ni_pcidio_print_status(status);
 
-	/* printk("buf[0]=%08x\n",*(unsigned int *)async->prealloc_buf); */
-	/* printk("buf[4096]=%08x\n",
-	       *(unsigned int *)(async->prealloc_buf+4096)); */
-
 	spin_lock(&devpriv->mite_channel_lock);
 	if (devpriv->di_mite_chan)
 		m_status = mite_get_status(devpriv->di_mite_chan);
 #ifdef MITE_DEBUG
 	mite_print_chsr(m_status);
 #endif
-	/* printk("mite_bytes_transferred: %d\n",
-	       mite_bytes_transferred(mite,DI_DMA_CHAN)); */
 
 	/* mite_dump_regs(mite); */
 	if (m_status & CHSR_INT) {
@@ -617,7 +531,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 		}
 #if 0
 		else {
-			printk("ni_pcidio: unknown interrupt\n");
+			DPRINTK("ni_pcidio: unknown interrupt\n");
 			async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 			writeb(0x00,
 			       devpriv->mite->daq_io_addr +
@@ -648,38 +562,47 @@ out:
 }
 
 #ifdef DEBUG_FLAGS
+static const char *bit_set_string(unsigned int bits, unsigned int bit,
+				  const char *const strings[])
+{
+	return (bits & (1U << bit)) ? strings[bit] : "";
+}
+
 static const char *const flags_strings[] = {
-	"TransferReady", "CountExpired", "2", "3",
-	"4", "Waited", "PrimaryTC", "SecondaryTC",
+	" TransferReady", " CountExpired", " 2", " 3",
+	" 4", " Waited", " PrimaryTC", " SecondaryTC",
 };
 
+
 static void ni_pcidio_print_flags(unsigned int flags)
 {
-	int i;
-
-	printk(KERN_INFO "group_1_flags:");
-	for (i = 7; i >= 0; i--) {
-		if (flags & (1 << i))
-			printk(" %s", flags_strings[i]);
-	}
-	printk("\n");
+	pr_debug("group_1_flags:%s%s%s%s%s%s%s%s\n",
+		 bit_set_string(flags, 7, flags_strings),
+		 bit_set_string(flags, 6, flags_strings),
+		 bit_set_string(flags, 5, flags_strings),
+		 bit_set_string(flags, 4, flags_strings),
+		 bit_set_string(flags, 3, flags_strings),
+		 bit_set_string(flags, 2, flags_strings),
+		 bit_set_string(flags, 1, flags_strings),
+		 bit_set_string(flags, 0, flags_strings));
 }
 
-static char *status_strings[] = {
-	"DataLeft1", "Reserved1", "Req1", "StopTrig1",
-	"DataLeft2", "Reserved2", "Req2", "StopTrig2",
+static const char *const status_strings[] = {
+	" DataLeft1", " Reserved1", " Req1", " StopTrig1",
+	" DataLeft2", " Reserved2", " Req2", " StopTrig2",
 };
 
 static void ni_pcidio_print_status(unsigned int flags)
 {
-	int i;
-
-	printk(KERN_INFO "group_status:");
-	for (i = 7; i >= 0; i--) {
-		if (flags & (1 << i))
-			printk(" %s", status_strings[i]);
-	}
-	printk("\n");
+	pr_debug("group_status:%s%s%s%s%s%s%s%s\n",
+		 bit_set_string(flags, 7, status_strings),
+		 bit_set_string(flags, 6, status_strings),
+		 bit_set_string(flags, 5, status_strings),
+		 bit_set_string(flags, 4, status_strings),
+		 bit_set_string(flags, 3, status_strings),
+		 bit_set_string(flags, 2, status_strings),
+		 bit_set_string(flags, 1, status_strings),
+		 bit_set_string(flags, 0, status_strings));
 }
 #endif
 
@@ -761,45 +684,25 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
+	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_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-	compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_INT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
+	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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1065,10 +968,12 @@ static int ni_pcidio_change(struct comedi_device *dev,
 }
 
 static int pci_6534_load_fpga(struct comedi_device *dev, int fpga_index,
-			      u8 *data, int data_len)
+			      const u8 *data, size_t data_len)
 {
 	static const int timeout = 1000;
-	int i, j;
+	int i;
+	size_t j;
+
 	writew(0x80 | fpga_index,
 	       devpriv->mite->daq_io_addr + Firmware_Control_Register);
 	writew(0xc0 | fpga_index,
@@ -1079,8 +984,9 @@ static int pci_6534_load_fpga(struct comedi_device *dev, int fpga_index,
 		udelay(1);
 	}
 	if (i == timeout) {
-		printk(KERN_WARNING "ni_pcidio: failed to load fpga %i, "
-		       "waiting for status 0x2\n", fpga_index);
+		dev_warn(dev->class_dev,
+			 "ni_pcidio: failed to load fpga %i, waiting for status 0x2\n",
+			 fpga_index);
 		return -EIO;
 	}
 	writew(0x80 | fpga_index,
@@ -1091,8 +997,9 @@ static int pci_6534_load_fpga(struct comedi_device *dev, int fpga_index,
 		udelay(1);
 	}
 	if (i == timeout) {
-		printk(KERN_WARNING "ni_pcidio: failed to load fpga %i, "
-		       "waiting for status 0x3\n", fpga_index);
+		dev_warn(dev->class_dev,
+			 "ni_pcidio: failed to load fpga %i, waiting for status 0x3\n",
+			 fpga_index);
 		return -EIO;
 	}
 	for (j = 0; j + 1 < data_len;) {
@@ -1107,8 +1014,9 @@ static int pci_6534_load_fpga(struct comedi_device *dev, int fpga_index,
 			udelay(1);
 		}
 		if (i == timeout) {
-			printk("ni_pcidio: failed to load word into fpga %i\n",
-			       fpga_index);
+			dev_warn(dev->class_dev,
+				 "ni_pcidio: failed to load word into fpga %i\n",
+				 fpga_index);
 			return -EIO;
 		}
 		if (need_resched())
@@ -1147,85 +1055,72 @@ static void pci_6534_init_main_fpga(struct comedi_device *dev)
 	writel(0, devpriv->mite->daq_io_addr + FPGA_SCBMS_Counter_Register);
 }
 
-static int pci_6534_upload_firmware(struct comedi_device *dev, int options[])
+static int pci_6534_upload_firmware(struct comedi_device *dev)
 {
 	int ret;
-	void *main_fpga_data, *scarab_a_data, *scarab_b_data;
-	int main_fpga_data_len, scarab_a_data_len, scarab_b_data_len;
+	const struct firmware *fw;
+	static const char *const fw_file[3] = {
+		FW_PCI_6534_SCARAB_DI,	/* loaded into scarab A for DI */
+		FW_PCI_6534_SCARAB_DO,	/* loaded into scarab B for DO */
+		FW_PCI_6534_MAIN,	/* loaded into main FPGA */
+	};
+	int n;
 
-	if (options[COMEDI_DEVCONF_AUX_DATA_LENGTH] == 0)
-		return 0;
 	ret = pci_6534_reset_fpgas(dev);
 	if (ret < 0)
 		return ret;
-	main_fpga_data = comedi_aux_data(options, 0);
-	main_fpga_data_len = options[COMEDI_DEVCONF_AUX_DATA0_LENGTH];
-	ret = pci_6534_load_fpga(dev, 2, main_fpga_data, main_fpga_data_len);
-	if (ret < 0)
-		return ret;
-	pci_6534_init_main_fpga(dev);
-	scarab_a_data = comedi_aux_data(options, 1);
-	scarab_a_data_len = options[COMEDI_DEVCONF_AUX_DATA1_LENGTH];
-	ret = pci_6534_load_fpga(dev, 0, scarab_a_data, scarab_a_data_len);
-	if (ret < 0)
-		return ret;
-	scarab_b_data = comedi_aux_data(options, 2);
-	scarab_b_data_len = options[COMEDI_DEVCONF_AUX_DATA2_LENGTH];
-	ret = pci_6534_load_fpga(dev, 1, scarab_b_data, scarab_b_data_len);
-	if (ret < 0)
-		return ret;
-	return 0;
+	/* load main FPGA first, then the two scarabs */
+	for (n = 2; n >= 0; n--) {
+		ret = request_firmware(&fw, fw_file[n],
+				       &devpriv->mite->pcidev->dev);
+		if (ret == 0) {
+			ret = pci_6534_load_fpga(dev, n, fw->data, fw->size);
+			if (ret == 0 && n == 2)
+				pci_6534_init_main_fpga(dev);
+			release_firmware(fw);
+		}
+		if (ret < 0)
+			break;
+	}
+	return ret;
 }
 
-static int nidio_find_device(struct comedi_device *dev, int bus, int slot)
+static const struct nidio_board *
+nidio_find_boardinfo(struct pci_dev *pcidev)
 {
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < n_nidio_boards; i++) {
-			if (mite_device_id(mite) == nidio_boards[i].dev_id) {
-				dev->board_ptr = nidio_boards + i;
-				devpriv->mite = mite;
+	unsigned int dev_id = pcidev->device;
+	unsigned int n;
 
-				return 0;
-			}
-		}
+	for (n = 0; n < ARRAY_SIZE(nidio_boards); n++) {
+		const struct nidio_board *board = &nidio_boards[n];
+		if (board->dev_id == dev_id)
+			return board;
 	}
-	printk(KERN_WARNING "no device found\n");
-	mite_list_devices();
-	return -EIO;
+	return NULL;
 }
 
-static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int __devinit nidio_attach_pci(struct comedi_device *dev,
+				      struct pci_dev *pcidev)
 {
 	struct comedi_subdevice *s;
-	int i;
 	int ret;
-	int n_subdevices;
 	unsigned int irq;
 
-	printk(KERN_INFO "comedi%d: nidio:", dev->minor);
-
 	ret = alloc_private(dev, sizeof(struct nidio96_private));
 	if (ret < 0)
 		return ret;
 	spin_lock_init(&devpriv->mite_channel_lock);
 
-	ret = nidio_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = nidio_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
 
 	ret = mite_setup(devpriv->mite);
 	if (ret < 0) {
-		printk(KERN_WARNING "error setting up mite\n");
+		dev_warn(dev->class_dev, "error setting up mite\n");
 		return ret;
 	}
 	comedi_set_hw_dev(dev, &devpriv->mite->pcidev->dev);
@@ -1235,84 +1130,60 @@ static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = this_board->name;
 	irq = mite_irq(devpriv->mite);
-	printk(KERN_INFO " %s", dev->board_name);
 	if (this_board->uses_firmware) {
-		ret = pci_6534_upload_firmware(dev, it->options);
+		ret = pci_6534_upload_firmware(dev);
 		if (ret < 0)
 			return ret;
 	}
-	if (!this_board->is_diodaq)
-		n_subdevices = this_board->n_8255;
-	else
-		n_subdevices = 1;
 
-	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
 
-	if (!this_board->is_diodaq) {
-		for (i = 0; i < this_board->n_8255; i++) {
-			subdev_8255_init(dev, dev->subdevices + i,
-					 nidio96_8255_cb,
-					 (unsigned long)(devpriv->mite->
-							 daq_io_addr +
-							 NIDIO_8255_BASE(i)));
-		}
-	} else {
-
-		printk(KERN_INFO " rev=%d",
-		       readb(devpriv->mite->daq_io_addr + Chip_Version));
-
-		s = dev->subdevices + 0;
-
-		dev->read_subdev = s;
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags =
-		    SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL | SDF_PACKED |
-		    SDF_CMD_READ;
-		s->n_chan = 32;
-		s->range_table = &range_digital;
-		s->maxdata = 1;
-		s->insn_config = &ni_pcidio_insn_config;
-		s->insn_bits = &ni_pcidio_insn_bits;
-		s->do_cmd = &ni_pcidio_cmd;
-		s->do_cmdtest = &ni_pcidio_cmdtest;
-		s->cancel = &ni_pcidio_cancel;
-		s->len_chanlist = 32;	/* XXX */
-		s->buf_change = &ni_pcidio_change;
-		s->async_dma_dir = DMA_BIDIRECTIONAL;
-		s->poll = &ni_pcidio_poll;
-
-		writel(0, devpriv->mite->daq_io_addr + Port_IO(0));
-		writel(0, devpriv->mite->daq_io_addr + Port_Pin_Directions(0));
-		writel(0, devpriv->mite->daq_io_addr + Port_Pin_Mask(0));
-
-		/* disable interrupts on board */
-		writeb(0x00,
-		       devpriv->mite->daq_io_addr +
-		       Master_DMA_And_Interrupt_Control);
-
-		ret = request_irq(irq, nidio_interrupt, IRQF_SHARED,
-				  "ni_pcidio", dev);
-		if (ret < 0)
-			printk(KERN_WARNING " irq not available");
+	dev_info(dev->class_dev, "%s rev=%d\n", dev->board_name,
+		 readb(devpriv->mite->daq_io_addr + Chip_Version));
+
+	s = &dev->subdevices[0];
+
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags =
+		SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL | SDF_PACKED |
+		SDF_CMD_READ;
+	s->n_chan = 32;
+	s->range_table = &range_digital;
+	s->maxdata = 1;
+	s->insn_config = &ni_pcidio_insn_config;
+	s->insn_bits = &ni_pcidio_insn_bits;
+	s->do_cmd = &ni_pcidio_cmd;
+	s->do_cmdtest = &ni_pcidio_cmdtest;
+	s->cancel = &ni_pcidio_cancel;
+	s->len_chanlist = 32;	/* XXX */
+	s->buf_change = &ni_pcidio_change;
+	s->async_dma_dir = DMA_BIDIRECTIONAL;
+	s->poll = &ni_pcidio_poll;
+
+	writel(0, devpriv->mite->daq_io_addr + Port_IO(0));
+	writel(0, devpriv->mite->daq_io_addr + Port_Pin_Directions(0));
+	writel(0, devpriv->mite->daq_io_addr + Port_Pin_Mask(0));
+
+	/* disable interrupts on board */
+	writeb(0x00,
+		devpriv->mite->daq_io_addr +
+		Master_DMA_And_Interrupt_Control);
 
-		dev->irq = irq;
-	}
+	ret = request_irq(irq, nidio_interrupt, IRQF_SHARED,
+				"ni_pcidio", dev);
+	if (ret < 0)
+		dev_warn(dev->class_dev, "irq not available\n");
 
-	printk("\n");
+	dev->irq = irq;
 
 	return 0;
 }
 
 static void nidio_detach(struct comedi_device *dev)
 {
-	int i;
-
-	if (this_board && !this_board->is_diodaq) {
-		for (i = 0; i < this_board->n_8255; i++)
-			subdev_8255_cleanup(dev, dev->subdevices + i);
-	}
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
@@ -1320,15 +1191,17 @@ static void nidio_detach(struct comedi_device *dev)
 			mite_free_ring(devpriv->di_mite_ring);
 			devpriv->di_mite_ring = NULL;
 		}
-		if (devpriv->mite)
+		if (devpriv->mite) {
 			mite_unsetup(devpriv->mite);
+			mite_free(devpriv->mite);
+		}
 	}
 }
 
 static struct comedi_driver ni_pcidio_driver = {
 	.driver_name	= "ni_pcidio",
 	.module		= THIS_MODULE,
-	.attach		= nidio_attach,
+	.attach_pci	= nidio_attach_pci,
 	.detach		= nidio_detach,
 };
 
@@ -1347,13 +1220,6 @@ static DEFINE_PCI_DEVICE_TABLE(ni_pcidio_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1150) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1320) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x12b0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0160) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1630) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x13c0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0400) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1250) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x17d0) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1800) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ni_pcidio_pci_table);
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 89f4d43c6d08..f284a90720ec 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1188,8 +1188,6 @@ static const struct ni_board_struct ni_boards[] = {
 	 },
 };
 
-#define n_pcimio_boards ARRAY_SIZE(ni_boards)
-
 struct ni_private {
 NI_PRIVATE_COMMON};
 #define devpriv ((struct ni_private *)dev->private)
@@ -1502,7 +1500,6 @@ static uint32_t m_series_stc_readl(struct comedi_device *dev, int reg)
 
 #include "ni_mio_common.c"
 
-static int pcimio_find_device(struct comedi_device *dev, int bus, int slot);
 static int pcimio_ai_change(struct comedi_device *dev,
 			    struct comedi_subdevice *s, unsigned long new_size);
 static int pcimio_ao_change(struct comedi_device *dev,
@@ -1584,24 +1581,45 @@ static void pcimio_detach(struct comedi_device *dev)
 		mite_free_ring(devpriv->cdo_mite_ring);
 		mite_free_ring(devpriv->gpct_mite_ring[0]);
 		mite_free_ring(devpriv->gpct_mite_ring[1]);
-		if (devpriv->mite)
+		if (devpriv->mite) {
 			mite_unsetup(devpriv->mite);
+			mite_free(devpriv->mite);
+		}
 	}
 }
 
-static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static const struct ni_board_struct *
+pcimio_find_boardinfo(struct pci_dev *pcidev)
+{
+	unsigned int device_id = pcidev->device;
+	unsigned int n;
+
+	for (n = 0; n < ARRAY_SIZE(ni_boards); n++) {
+		const struct ni_board_struct *board = &ni_boards[n];
+		if (board->device_id == device_id)
+			return board;
+	}
+	return NULL;
+}
+
+static int __devinit pcimio_attach_pci(struct comedi_device *dev,
+				       struct pci_dev *pcidev)
 {
 	int ret;
 
-	dev_info(dev->class_dev, "ni_pcimio: attach\n");
+	dev_info(dev->class_dev, "ni_pcimio: attach %s\n", pci_name(pcidev));
 
 	ret = ni_alloc_private(dev);
 	if (ret < 0)
 		return ret;
 
-	ret = pcimio_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
+	dev->board_ptr = pcimio_find_boardinfo(pcidev);
+	if (!dev->board_ptr)
+		return -ENODEV;
+
+	devpriv->mite = mite_alloc(pcidev);
+	if (!devpriv->mite)
+		return -ENOMEM;
 
 	dev_dbg(dev->class_dev, "%s\n", boardtype.name);
 	dev->board_name = boardtype.name;
@@ -1659,7 +1677,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 	}
 
-	ret = ni_E_init(dev, it);
+	ret = ni_E_init(dev);
 	if (ret < 0)
 		return ret;
 
@@ -1672,34 +1690,6 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return ret;
 }
 
-static int pcimio_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-
-		for (i = 0; i < n_pcimio_boards; i++) {
-			if (mite_device_id(mite) == ni_boards[i].device_id) {
-				dev->board_ptr = ni_boards + i;
-				devpriv->mite = mite;
-
-				return 0;
-			}
-		}
-	}
-	pr_warn("no device found\n");
-	mite_list_devices();
-	return -EIO;
-}
-
 static int pcimio_ai_change(struct comedi_device *dev,
 			    struct comedi_subdevice *s, unsigned long new_size)
 {
@@ -1765,7 +1755,7 @@ static int pcimio_dio_change(struct comedi_device *dev,
 static struct comedi_driver ni_pcimio_driver = {
 	.driver_name	= "ni_pcimio",
 	.module		= THIS_MODULE,
-	.attach		= pcimio_attach,
+	.attach_pci	= pcimio_attach_pci,
 	.detach		= pcimio_detach,
 };
 
@@ -1844,7 +1834,7 @@ static struct pci_driver ni_pcimio_pci_driver = {
 	.probe		= ni_pcimio_pci_probe,
 	.remove		= __devexit_p(ni_pcimio_pci_remove)
 };
-module_comedi_pci_driver(ni_pcimio_driver, ni_pcimio_pci_driver)
+module_comedi_pci_driver(ni_pcimio_driver, ni_pcimio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index a9611587460a..8ee93d359bed 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -48,6 +48,7 @@ TODO:
 	Support use of both banks X and Y
 */
 
+#include "comedi_fc.h"
 #include "ni_tio_internal.h"
 #include "mite.h"
 
@@ -237,61 +238,35 @@ EXPORT_SYMBOL_GPL(ni_tio_cmd);
 int ni_tio_cmdtest(struct ni_gpct *counter, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-	int sources;
+	unsigned int sources;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
 	sources = TRIG_NOW | TRIG_INT | TRIG_OTHER;
 	if (ni_tio_counting_mode_registers_present(counter->counter_dev))
 		sources |= TRIG_EXT;
-	cmd->start_src &= sources;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_EXT | TRIG_OTHER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	sources = TRIG_NOW | TRIG_EXT | TRIG_OTHER;
-	cmd->convert_src &= sources;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, sources);
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_FOLLOW | TRIG_EXT | TRIG_OTHER);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique... */
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
-	if (cmd->start_src != TRIG_NOW &&
-	    cmd->start_src != TRIG_INT &&
-	    cmd->start_src != TRIG_EXT && cmd->start_src != TRIG_OTHER)
-		err++;
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_OTHER)
-		err++;
-	if (cmd->convert_src != TRIG_OTHER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_NONE)
-		err++;
-	/* ... and mutually compatible */
 	if (cmd->convert_src != TRIG_NOW && cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index bb72d0bc2975..89305a14eb5c 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -64,6 +64,7 @@ supported.
 #include <linux/ioport.h>
 #include <linux/delay.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 
 #define PCL711_SIZE 16
@@ -168,7 +169,7 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 	int data;
 	struct comedi_device *dev = d;
 	const struct pcl711_board *board = comedi_board(dev);
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 
 	if (!dev->attached) {
 		comedi_error(dev, "spurious interrupt");
@@ -266,42 +267,24 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	int tmp;
 	int err = 0;
 
-	/* step 1 */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -520,7 +503,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* AI subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -536,7 +519,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->do_cmd = pcl711_ai_cmd;
 	}
 
-	s++;
+	s = &dev->subdevices[1];
 	/* AO subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -547,7 +530,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = pcl711_ao_insn;
 	s->insn_read = pcl711_ao_insn_read;
 
-	s++;
+	s = &dev->subdevices[2];
 	/* 16-bit digital input */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -557,7 +540,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->insn_bits = pcl711_di_insn_bits;
 
-	s++;
+	s = &dev->subdevices[3];
 	/* 16-bit digital out */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/pcl724.c b/drivers/staging/comedi/drivers/pcl724.c
index c8fe23ca899d..7b3c4293c01b 100644
--- a/drivers/staging/comedi/drivers/pcl724.c
+++ b/drivers/staging/comedi/drivers/pcl724.c
@@ -99,6 +99,7 @@ static int subdev_8255mapped_cb(int dir, int port, int data,
 static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	const struct pcl724_board *board = comedi_board(dev);
+	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iorange;
 	int ret, i, n_subdevices;
@@ -161,14 +162,13 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
+		s = &dev->subdevices[i];
 		if (board->is_pet48) {
-			subdev_8255_init(dev, dev->subdevices + i,
-					 subdev_8255mapped_cb,
+			subdev_8255_init(dev, s, subdev_8255mapped_cb,
 					 (unsigned long)(dev->iobase +
 							 i * 0x1000));
 		} else
-			subdev_8255_init(dev, dev->subdevices + i,
-					 subdev_8255_cb,
+			subdev_8255_init(dev, s, subdev_8255_cb,
 					 (unsigned long)(dev->iobase +
 							 SIZE_8255 * i));
 	}
@@ -179,10 +179,13 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 static void pcl724_detach(struct comedi_device *dev)
 {
 	const struct pcl724_board *board = comedi_board(dev);
+	struct comedi_subdevice *s;
 	int i;
 
-	for (i = 0; i < dev->n_subdevices; i++)
-		subdev_8255_cleanup(dev, dev->subdevices + i);
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = &dev->subdevices[i];
+		subdev_8255_cleanup(dev, s);
+	}
 #ifdef PCL724_IRQ
 	if (dev->irq)
 		free_irq(dev->irq, dev);
diff --git a/drivers/staging/comedi/drivers/pcl725.c b/drivers/staging/comedi/drivers/pcl725.c
index d5b60cf7c93f..21fbc1ae1e73 100644
--- a/drivers/staging/comedi/drivers/pcl725.c
+++ b/drivers/staging/comedi/drivers/pcl725.c
@@ -62,7 +62,7 @@ static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* do */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -71,7 +71,7 @@ static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_bits = pcl725_do_insn;
 	s->range_table = &range_digital;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* di */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index 2b10f1d83085..07e72de982ac 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -290,7 +290,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ao */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
@@ -316,7 +316,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			devpriv->bipolar[i] = 1;	/* bipolar range */
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* di */
 	if (!board->have_dio) {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -330,7 +330,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->range_table = &range_digital;
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* do */
 	if (!board->have_dio) {
 		s->type = COMEDI_SUBD_UNUSED;
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index 4675ec57082e..e3de49937452 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -84,7 +84,7 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* Isolated do */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -94,7 +94,7 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->private = (void *)PCL730_IDIO_LO;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* Isolated di */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -104,7 +104,7 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->private = (void *)PCL730_IDIO_LO;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* TTL do */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -114,7 +114,7 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_digital;
 	s->private = (void *)PCL730_DIO_LO;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* TTL di */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 578fd8920be1..3cf55ff93083 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -117,6 +117,7 @@
 #include <linux/io.h>
 #include <asm/dma.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 
 /* hardware types of the cards */
@@ -533,49 +534,31 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct pcl812_board *board = comedi_board(dev);
 	int err = 0;
+	unsigned int flags;
 	int tmp, divisor1, divisor2;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
 
-	tmp = cmd->convert_src;
 	if (devpriv->use_ext_trg)
-		cmd->convert_src &= TRIG_EXT;
+		flags = TRIG_EXT;
 	else
-		cmd->convert_src &= TRIG_TIMER;
-
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+		flags = TRIG_TIMER;
+	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are
-	 * unique and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -806,7 +789,7 @@ static irqreturn_t interrupt_pcl812_ai_int(int irq, void *d)
 	char err = 1;
 	unsigned int mask, timeout;
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	unsigned int next_chan;
 
 	s->async->events = 0;
@@ -909,7 +892,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 static irqreturn_t interrupt_pcl812_ai_dma(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	unsigned long dma_flags;
 	int len, bufptr;
 	short *ptr;
@@ -1267,7 +1250,7 @@ no_dma:
 
 	/* analog input */
 	if (board->n_aichan > 0) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE;
 		switch (board->board_type) {
@@ -1409,7 +1392,7 @@ no_dma:
 
 	/* analog output */
 	if (board->n_aochan > 0) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
 		s->n_chan = board->n_aochan;
@@ -1438,7 +1421,7 @@ no_dma:
 
 	/* digital input */
 	if (board->n_dichan > 0) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
 		s->n_chan = board->n_dichan;
@@ -1451,7 +1434,7 @@ no_dma:
 
 	/* digital output */
 	if (board->n_dochan > 0) {
-		s = dev->subdevices + subdev;
+		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = board->n_dochan;
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index ba6911f063cb..0822de058e4d 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -41,6 +41,7 @@ Configuration Options:
 #include <linux/io.h>
 #include <asm/dma.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 
 #define DEBUG(x) x
@@ -258,7 +259,7 @@ static int pcl816_ai_insn_read(struct comedi_device *dev,
 static irqreturn_t interrupt_pcl816_ai_mode13_int(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int low, hi;
 	int timeout = 50;	/* wait max 50us */
 
@@ -349,7 +350,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 static irqreturn_t interrupt_pcl816_ai_mode13_dma(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int len, bufptr, this_dma_buf;
 	unsigned long dma_flags;
 	short *ptr;
@@ -458,48 +459,23 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 	      pcl816_cmdtest_out(-1, cmd);
 	     );
 
-	/* step 1: make sure trigger sources are trivially valid */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_EXT | TRIG_TIMER;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
+	/* Step 2a : make sure trigger sources are unique */
 
-	/*
-	 * step 2: make sure trigger sources
-	 * are unique and mutually compatible
-	 */
-
-	if (cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_TIMER) {
-		cmd->convert_src = TRIG_TIMER;
-		err++;
-	}
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
 
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1183,7 +1159,7 @@ no_dma:
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	if (board->n_aichan > 0) {
 		s->type = COMEDI_SUBD_AI;
 		devpriv->sub_ai = s;
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 34169c16fb92..d4b0859d81f2 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -107,6 +107,7 @@ A word or two about DMA. Driver support DMA operations at two ways:
 #include <linux/io.h>
 #include <asm/dma.h>
 
+#include "comedi_fc.h"
 #include "8253.h"
 
 /* #define PCL818_MODE13_AO 1 */
@@ -477,7 +478,7 @@ static int pcl818_do_insn_bits(struct comedi_device *dev,
 static irqreturn_t interrupt_pcl818_ai_mode13_int(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int low;
 	int timeout = 50;	/* wait max 50us */
 
@@ -536,7 +537,7 @@ conv_finish:
 static irqreturn_t interrupt_pcl818_ai_mode13_dma(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int i, len, bufptr;
 	unsigned long flags;
 	short *ptr;
@@ -615,7 +616,7 @@ static irqreturn_t interrupt_pcl818_ai_mode13_dma(int irq, void *d)
 static irqreturn_t interrupt_pcl818_ai_mode13_dma_rtc(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	unsigned long tmp;
 	unsigned int top1, top2, i, bufptr;
 	long ofs_dats;
@@ -720,7 +721,7 @@ static irqreturn_t interrupt_pcl818_ai_mode13_dma_rtc(int irq, void *d)
 static irqreturn_t interrupt_pcl818_ai_mode13_fifo(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	int i, len, lo;
 
 	outb(0, dev->iobase + PCL818_FI_INTCLR);	/*  clear fifo int request */
@@ -811,7 +812,7 @@ static irqreturn_t interrupt_pcl818(int irq, void *d)
 			   being reprogrammed while a DMA transfer is in
 			   progress.
 			 */
-			struct comedi_subdevice *s = dev->subdevices + 0;
+			struct comedi_subdevice *s = &dev->subdevices[0];
 			devpriv->ai_act_scan = 0;
 			devpriv->neverending_ai = 0;
 			pcl818_ai_cancel(dev, s);
@@ -1261,43 +1262,23 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	int err = 0;
 	int tmp, divisor1 = 0, divisor2 = 0;
 
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
 
-	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
-		err++;
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1763,7 +1744,7 @@ no_dma:
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	if (!board->n_aichan_se) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
@@ -1829,7 +1810,7 @@ no_dma:
 		}
 	}
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	if (!board->n_aochan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
@@ -1862,7 +1843,7 @@ no_dma:
 		}
 	}
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	if (!board->n_dichan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
@@ -1875,7 +1856,7 @@ no_dma:
 		s->insn_bits = pcl818_di_insn_bits;
 	}
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	if (!board->n_dochan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index 62c22ccfb780..4102547dc6a8 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -119,6 +119,8 @@ static int compute_buffer(int config, int devno, struct comedi_subdevice *s)
 static void do_3724_config(struct comedi_device *dev,
 			   struct comedi_subdevice *s, int chanspec)
 {
+	struct comedi_subdevice *s_dio1 = &dev->subdevices[0];
+	struct comedi_subdevice *s_dio2 = &dev->subdevices[1];
 	int config;
 	int buffer_config;
 	unsigned long port_8255_cfg;
@@ -136,10 +138,10 @@ static void do_3724_config(struct comedi_device *dev,
 	if (!(s->io_bits & 0xff0000))
 		config |= CR_C_IO;
 
-	buffer_config = compute_buffer(0, 0, dev->subdevices);
-	buffer_config = compute_buffer(buffer_config, 1, (dev->subdevices) + 1);
+	buffer_config = compute_buffer(0, 0, s_dio1);
+	buffer_config = compute_buffer(buffer_config, 1, s_dio2);
 
-	if (s == dev->subdevices)
+	if (s == s_dio1)
 		port_8255_cfg = dev->iobase + _8255_CR;
 	else
 		port_8255_cfg = dev->iobase + SIZE_8255 + _8255_CR;
@@ -154,6 +156,7 @@ static void do_3724_config(struct comedi_device *dev,
 static void enable_chan(struct comedi_device *dev, struct comedi_subdevice *s,
 			int chanspec)
 {
+	struct comedi_subdevice *s_dio1 = &dev->subdevices[0];
 	unsigned int mask;
 	int gatecfg;
 	struct priv_pcm3724 *priv;
@@ -162,9 +165,9 @@ static void enable_chan(struct comedi_device *dev, struct comedi_subdevice *s,
 	priv = dev->private;
 
 	mask = 1 << CR_CHAN(chanspec);
-	if (s == dev->subdevices)	/*  subdev 0 */
+	if (s == s_dio1)
 		priv->dio_1 |= mask;
-	else		/* subdev 1 */
+	else
 		priv->dio_2 |= mask;
 
 	if (priv->dio_1 & 0xff0000)
@@ -231,6 +234,7 @@ static int pcm3724_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
 	const struct pcm3724_board *board = comedi_board(dev);
+	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iorange;
 	int ret, i, n_subdevices;
@@ -263,9 +267,10 @@ static int pcm3724_attach(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		subdev_8255_init(dev, dev->subdevices + i, subdev_8255_cb,
+		s = &dev->subdevices[i];
+		subdev_8255_init(dev, s, subdev_8255_cb,
 				 (unsigned long)(dev->iobase + SIZE_8255 * i));
-		((dev->subdevices) + i)->insn_config = subdev_3724_insn_config;
+		s->insn_config = subdev_3724_insn_config;
 	}
 	return 0;
 }
@@ -273,11 +278,14 @@ static int pcm3724_attach(struct comedi_device *dev,
 static void pcm3724_detach(struct comedi_device *dev)
 {
 	const struct pcm3724_board *board = comedi_board(dev);
+	struct comedi_subdevice *s;
 	int i;
 
 	if (dev->subdevices) {
-		for (i = 0; i < dev->n_subdevices; i++)
-			subdev_8255_cleanup(dev, dev->subdevices + i);
+		for (i = 0; i < dev->n_subdevices; i++) {
+			s = &dev->subdevices[i];
+			subdev_8255_cleanup(dev, s);
+		}
 	}
 	if (dev->iobase)
 		release_region(dev->iobase, board->io_range);
diff --git a/drivers/staging/comedi/drivers/pcm3730.c b/drivers/staging/comedi/drivers/pcm3730.c
index d65e0bda2c44..067f14d22610 100644
--- a/drivers/staging/comedi/drivers/pcm3730.c
+++ b/drivers/staging/comedi/drivers/pcm3730.c
@@ -72,7 +72,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->maxdata = 1;
@@ -81,7 +81,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)PCM3730_DOA;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->maxdata = 1;
@@ -90,7 +90,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)PCM3730_DOB;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->maxdata = 1;
@@ -99,7 +99,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)PCM3730_DOC;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->maxdata = 1;
@@ -108,7 +108,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)PCM3730_DIA;
 
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->maxdata = 1;
@@ -117,7 +117,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 	s->range_table = &range_digital;
 	s->private = (void *)PCM3730_DIB;
 
-	s = dev->subdevices + 5;
+	s = &dev->subdevices[5];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->maxdata = 1;
diff --git a/drivers/staging/comedi/drivers/pcm_common.c b/drivers/staging/comedi/drivers/pcm_common.c
index 474af7bc6c8b..85ee05ece9c8 100644
--- a/drivers/staging/comedi/drivers/pcm_common.c
+++ b/drivers/staging/comedi/drivers/pcm_common.c
@@ -1,60 +1,30 @@
 #include "../comedidev.h"
+
+#include "comedi_fc.h"
 #include "pcm_common.h"
 
-/*
- * 'do_cmdtest' function for an 'INTERRUPT' subdevice.  This is for
- * the PCM drivers.
- */
 int comedi_pcm_cmdtest(struct comedi_device *dev,
 		       struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	unsigned int tmp;
-
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= (TRIG_NOW | TRIG_INT);
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= (TRIG_COUNT | TRIG_NONE);
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and
-	 * mutually compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* these tests are true if more than one _src bit is set */
-	if ((cmd->start_src & (cmd->start_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_begin_src & (cmd->scan_begin_src - 1)) != 0)
-		err++;
-	if ((cmd->convert_src & (cmd->convert_src - 1)) != 0)
-		err++;
-	if ((cmd->scan_end_src & (cmd->scan_end_src - 1)) != 0)
-		err++;
-	if ((cmd->stop_src & (cmd->stop_src - 1)) != 0)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/pcmad.c b/drivers/staging/comedi/drivers/pcmad.c
index 54d19c943967..5efeb9205c2e 100644
--- a/drivers/staging/comedi/drivers/pcmad.c
+++ b/drivers/staging/comedi/drivers/pcmad.c
@@ -127,7 +127,7 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = board->name;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | AREF_GROUND;
 	s->n_chan = 16;		/* XXX */
diff --git a/drivers/staging/comedi/drivers/pcmda12.c b/drivers/staging/comedi/drivers/pcmda12.c
index 291ce7c1bdb1..28af8f6873eb 100644
--- a/drivers/staging/comedi/drivers/pcmda12.c
+++ b/drivers/staging/comedi/drivers/pcmda12.c
@@ -195,7 +195,7 @@ static int pcmda12_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	s = dev->subdevices;
+	s = &dev->subdevices[0];
 	s->private = NULL;
 	s->maxdata = (0x1 << BITS) - 1;
 	s->range_table = &pcmda12_ranges;
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index 3d2e6f01c4b7..a10bf0a2987f 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -526,6 +526,7 @@ static irqreturn_t interrupt_pcmmio(int irq, void *d)
 {
 	int asic, got1 = 0;
 	struct comedi_device *dev = (struct comedi_device *)d;
+	int i;
 
 	for (asic = 0; asic < MAX_ASICS; ++asic) {
 		if (irq == devpriv->asics[asic].irq) {
@@ -583,9 +584,8 @@ static irqreturn_t interrupt_pcmmio(int irq, void *d)
 				printk
 				    (KERN_DEBUG "got edge detect interrupt %d asic %d which_chans: %06x\n",
 				     irq, asic, triggered);
-				for (s = dev->subdevices + 2;
-				     s < dev->subdevices + dev->n_subdevices;
-				     ++s) {
+				for (i = 2; i < dev->n_subdevices; i++) {
+					s = &dev->subdevices[i];
 					/*
 					 * this is an interrupt subdev,
 					 * and it matches this asic!
@@ -1076,9 +1076,8 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	/* First, AI */
-	sdev_no = 0;
-	s = dev->subdevices + sdev_no;
-	s->private = devpriv->sprivs + sdev_no;
+	s = &dev->subdevices[0];
+	s->private = &devpriv->sprivs[0];
 	s->maxdata = (1 << board->ai_bits) - 1;
 	s->range_table = board->ai_range_table;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
@@ -1092,9 +1091,8 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	outb(0, subpriv->iobase + 4 + 3);
 
 	/* Next, AO */
-	++sdev_no;
-	s = dev->subdevices + sdev_no;
-	s->private = devpriv->sprivs + sdev_no;
+	s = &dev->subdevices[1];
+	s->private = &devpriv->sprivs[1];
 	s->maxdata = (1 << board->ao_bits) - 1;
 	s->range_table = board->ao_range_table;
 	s->subdev_flags = SDF_READABLE;
@@ -1108,14 +1106,13 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	outb(0, subpriv->iobase + 3);
 	outb(0, subpriv->iobase + 4 + 3);
 
-	++sdev_no;
 	port = 0;
 	asic = 0;
-	for (; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
+	for (sdev_no = 2; sdev_no < dev->n_subdevices; ++sdev_no) {
 		int byte_no;
 
-		s = dev->subdevices + sdev_no;
-		s->private = devpriv->sprivs + sdev_no;
+		s = &dev->subdevices[sdev_no];
+		s->private = &devpriv->sprivs[sdev_no];
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -1200,15 +1197,6 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				 * multiple irqs..
 				 */
 
-	if (irq[0]) {
-		printk(KERN_DEBUG "comedi%d: irq: %u\n", dev->minor, irq[0]);
-		if (board->dio_num_asics == 2 && irq[1])
-			printk(KERN_DEBUG "comedi%d: second ASIC irq: %u\n",
-					dev->minor, irq[1]);
-	} else {
-		printk(KERN_INFO "comedi%d: (IRQ mode disabled)\n", dev->minor);
-	}
-
 	printk(KERN_INFO "comedi%d: attached\n", dev->minor);
 
 	return 1;
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index feef3d02f35a..0e32119bc3f9 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -442,7 +442,7 @@ static void pcmuio_stop_intr(struct comedi_device *dev,
 
 	subpriv->intr.enabled_mask = 0;
 	subpriv->intr.active = 0;
-	s->async->inttrig = 0;
+	s->async->inttrig = NULL;
 	nports = subpriv->intr.num_asic_chans / CHANS_PER_PORT;
 	firstport = subpriv->intr.asic_chan / CHANS_PER_PORT;
 	switch_page(dev, asic, PAGE_ENAB);
@@ -456,6 +456,7 @@ static irqreturn_t interrupt_pcmuio(int irq, void *d)
 {
 	int asic, got1 = 0;
 	struct comedi_device *dev = (struct comedi_device *)d;
+	int i;
 
 	for (asic = 0; asic < MAX_ASICS; ++asic) {
 		if (irq == devpriv->asics[asic].irq) {
@@ -507,9 +508,8 @@ static irqreturn_t interrupt_pcmuio(int irq, void *d)
 				printk
 				    ("PCMUIO DEBUG: got edge detect interrupt %d asic %d which_chans: %06x\n",
 				     irq, asic, triggered);
-				for (s = dev->subdevices;
-				     s < dev->subdevices + dev->n_subdevices;
-				     ++s) {
+				for (i = 0; i < dev->n_subdevices; i++) {
+					s = &dev->subdevices[i];
 					if (subpriv->intr.asic == asic) {	/* this is an interrupt subdev, and it matches this asic! */
 						unsigned long flags;
 						unsigned oldevents;
@@ -683,7 +683,7 @@ pcmuio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
 		return -EINVAL;
 
 	spin_lock_irqsave(&subpriv->intr.spinlock, flags);
-	s->async->inttrig = 0;
+	s->async->inttrig = NULL;
 	if (subpriv->intr.active)
 		event = pcmuio_start_intr(dev, s);
 
@@ -811,8 +811,8 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	for (sdev_no = 0; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
 		int byte_no;
 
-		s = dev->subdevices + sdev_no;
-		s->private = devpriv->sprivs + sdev_no;
+		s = &dev->subdevices[sdev_no];
+		s->private = &devpriv->sprivs[sdev_no];
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/poc.c b/drivers/staging/comedi/drivers/poc.c
index c253bb9ef335..78dfe167b147 100644
--- a/drivers/staging/comedi/drivers/poc.c
+++ b/drivers/staging/comedi/drivers/poc.c
@@ -164,7 +164,7 @@ static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -ENOMEM;
 
 	/* analog output subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = board->type;
 	s->n_chan = board->n_chan;
 	s->maxdata = (1 << board->n_bits) - 1;
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index a029147c9b69..3e276f7a3380 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -56,6 +56,8 @@ Devices: [Quatech] DAQP-208 (daqp), DAQP-308
 
 #include <linux/completion.h>
 
+#include "comedi_fc.h"
+
 /* Maximum number of separate DAQP devices we'll allow */
 #define MAX_DEV         4
 
@@ -456,51 +458,26 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW);
+	err |= cfc_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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources
-	 * are unique and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-	if (cmd->convert_src != TRIG_NOW && cmd->convert_src != TRIG_TIMER)
-		err++;
-	if (cmd->scan_begin_src == TRIG_FOLLOW && cmd->convert_src == TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -878,7 +855,7 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_INFO "comedi%d: attaching daqp%d (io 0x%04lx)\n",
 	       dev->minor, it->options[0], dev->iobase);
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->private = local;
 	s->type = COMEDI_SUBD_AI;
@@ -892,7 +869,7 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->do_cmd = daqp_ai_cmd;
 	s->cancel = daqp_ai_cancel;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	dev->write_subdev = s;
 	s->private = local;
 	s->type = COMEDI_SUBD_AO;
@@ -903,7 +880,7 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->range_table = &range_daqp_ao;
 	s->insn_write = daqp_ao_insn_write;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->private = local;
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -911,7 +888,7 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->len_chanlist = 1;
 	s->insn_read = daqp_di_insn_read;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->private = local;
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITEABLE;
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 5aa8be1e7b92..41d24b08913b 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -106,6 +106,8 @@ Configuration options:
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
+
 #define DRV_NAME "rtd520"
 
 /*======================================================================
@@ -783,7 +785,7 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 				 void *d)
 {				/* our data *//* cpu context (ignored) */
 	struct comedi_device *dev = d;
-	struct comedi_subdevice *s = dev->subdevices + 0;	/* analog in subdevice */
+	struct comedi_subdevice *s = &dev->subdevices[0];
 	struct rtdPrivate *devpriv = dev->private;
 	u32 overrun;
 	u16 status;
@@ -964,7 +966,7 @@ static int rtd_ai_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 /*
   cmdtest tests a particular command to see if it is valid.
   Using the cmdtest ioctl, a user can create a valid cmd
-  and then have it executed by the cmd ioctl (asyncronously).
+  and then have it executed by the cmd ioctl (asynchronously).
 
   cmdtest returns 1,2,3,4 or 0, depending on which tests
   the command passes.
@@ -976,52 +978,25 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique
-	   and mutually compatible */
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT) {
-		err++;
-	}
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1706,7 +1681,7 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
@@ -1726,7 +1701,7 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->cancel = rtd_ai_cancel;
 	/* s->poll = rtd_ai_poll; *//* not ready yet */
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -1736,7 +1711,7 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = rtd_ao_winsn;
 	s->insn_read = rtd_ao_rinsn;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -1748,7 +1723,7 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_config = rtd_dio_insn_config;
 
 	/* timer/counter subdevices (not currently supported) */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 3;
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index f7fa940d9783..137885b1681a 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -360,7 +360,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->dac1_coding = it->options[8];
 	devpriv->muxgain_bits = -1;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
@@ -379,7 +379,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		break;
 	}
 
-	s++;
+	s = &dev->subdevices[1];
 	if (board->has_ao) {
 		/* ao subdevice (only on rti815) */
 		s->type = COMEDI_SUBD_AO;
@@ -409,7 +409,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	s++;
+	s = &dev->subdevices[2];
 	/* di */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
@@ -418,7 +418,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 
-	s++;
+	s = &dev->subdevices[3];
 	/* do */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -429,7 +429,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 /* don't yet know how to deal with counter/timers */
 #if 0
-	s++;
+	s = &dev->subdevices[4];
 	/* do */
 	s->type = COMEDI_SUBD_TIMER;
 #endif
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index fc16508181d4..3f9d0278be50 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -111,7 +111,7 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices;
+	s = &dev->subdevices[0];
 	/* ao subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 737a194dfce3..a1e256293bd6 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -83,36 +83,6 @@ comedi_config /dev/comedi0 s526 0x2C0,0x3
 #define REG_EED 0x32
 #define REG_EEC 0x34
 
-static const int s526_ports[] = {
-	REG_TCR,
-	REG_WDC,
-	REG_DAC,
-	REG_ADC,
-	REG_ADD,
-	REG_DIO,
-	REG_IER,
-	REG_ISR,
-	REG_MSC,
-	REG_C0L,
-	REG_C0H,
-	REG_C0M,
-	REG_C0C,
-	REG_C1L,
-	REG_C1H,
-	REG_C1M,
-	REG_C1C,
-	REG_C2L,
-	REG_C2H,
-	REG_C2M,
-	REG_C2C,
-	REG_C3L,
-	REG_C3H,
-	REG_C3M,
-	REG_C3C,
-	REG_EED,
-	REG_EEC
-};
-
 struct counter_mode_register_t {
 #if defined(__LITTLE_ENDIAN_BITFIELD)
 	unsigned short coutSource:1;
@@ -148,122 +118,48 @@ union cmReg {
 	unsigned short value;
 };
 
-#define MAX_GPCT_CONFIG_DATA 6
-
-/* Different Application Classes for GPCT Subdevices */
-/* The list is not exhaustive and needs discussion! */
-enum S526_GPCT_APP_CLASS {
-	CountingAndTimeMeasurement,
-	SinglePulseGeneration,
-	PulseTrainGeneration,
-	PositionMeasurement,
-	Miscellaneous
-};
-
-/* Config struct for different GPCT subdevice Application Classes and
-   their options
-*/
-struct s526GPCTConfig {
-	enum S526_GPCT_APP_CLASS app;
-	int data[MAX_GPCT_CONFIG_DATA];
-};
-
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
-struct s526_board {
-	const char *name;
-	int gpct_chans;
-	int gpct_bits;
-	int ad_chans;
-	int ad_bits;
-	int da_chans;
-	int da_bits;
-	int have_dio;
-};
-
-static const struct s526_board s526_boards[] = {
-	{
-	 .name = "s526",
-	 .gpct_chans = 4,
-	 .gpct_bits = 24,
-	 .ad_chans = 8,
-	 .ad_bits = 16,
-	 .da_chans = 4,
-	 .da_bits = 16,
-	 .have_dio = 1,
-	 }
-};
-
-#define ADDR_REG(reg) (dev->iobase + (reg))
-#define ADDR_CHAN_REG(reg, chan) (dev->iobase + (reg) + (chan) * 8)
-
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device
-   struct.
-*/
 struct s526_private {
 	unsigned int ao_readback[2];
-	struct s526GPCTConfig s526_gpct_config[4];
-	unsigned short s526_ai_config;
+	unsigned int gpct_config[4];
+	unsigned short ai_config;
 };
 
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct s526_private *)dev->private)
-
 static int s526_gpct_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn,
 			   unsigned int *data)
 {
-	int i;			/*  counts the Data */
-	int counter_channel = CR_CHAN(insn->chanspec);
-	unsigned short datalow;
-	unsigned short datahigh;
-
-	/*  Check if (n > 0) */
-	if (insn->n <= 0) {
-		printk(KERN_ERR "s526: INSN_READ: n should be > 0\n");
-		return -EINVAL;
-	}
-	/*  Read the low word first */
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long chan_iobase = dev->iobase + chan * 8;
+	unsigned int lo;
+	unsigned int hi;
+	int i;
+
 	for (i = 0; i < insn->n; i++) {
-		datalow = inw(ADDR_CHAN_REG(REG_C0L, counter_channel));
-		datahigh = inw(ADDR_CHAN_REG(REG_C0H, counter_channel));
-		data[i] = (int)(datahigh & 0x00FF);
-		data[i] = (data[i] << 16) | (datalow & 0xFFFF);
-		/* printk("s526 GPCT[%d]: %x(0x%04x, 0x%04x)\n",
-		   counter_channel, data[i], datahigh, datalow); */
+		/* Read the low word first */
+		lo = inw(chan_iobase + REG_C0L) & 0xffff;
+		hi = inw(chan_iobase + REG_C0H) & 0xff;
+
+		data[i] = (hi << 16) | lo;
 	}
-	return i;
+
+	return insn->n;
 }
 
 static int s526_gpct_insn_config(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	int subdev_channel = CR_CHAN(insn->chanspec);	/*  Unpack chanspec */
-	int i;
-	short value;
+	struct s526_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long chan_iobase = dev->iobase + chan * 8;
+	unsigned int val;
 	union cmReg cmReg;
 
-	/* printk("s526: GPCT_INSN_CONFIG: Configuring Channel %d\n",
-						subdev_channel); */
-
-	for (i = 0; i < MAX_GPCT_CONFIG_DATA; i++) {
-		devpriv->s526_gpct_config[subdev_channel].data[i] =
-		    insn->data[i];
-/* printk("data[%d]=%x\n", i, insn->data[i]); */
-	}
-
 	/*  Check what type of Counter the user requested, data[0] contains */
 	/*  the Application type */
-	switch (insn->data[0]) {
+	switch (data[0]) {
 	case INSN_CONFIG_GPCT_QUADRATURE_ENCODER:
 		/*
 		   data[0]: Application Type
@@ -271,9 +167,7 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		   data[2]: Pre-load Register Value
 		   data[3]: Conter Control Register
 		 */
-		printk(KERN_INFO "s526: GPCT_INSN_CONFIG: Configuring Encoder\n");
-		devpriv->s526_gpct_config[subdev_channel].app =
-		    PositionMeasurement;
+		devpriv->gpct_config[chan] = data[0];
 
 #if 0
 		/*  Example of Counter Application */
@@ -290,34 +184,32 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		cmReg.reg.preloadRegSel = 0;	/*  PR0 */
 		cmReg.reg.reserved = 0;
 
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
-		outw(0x0001, ADDR_CHAN_REG(REG_C0H, subdev_channel));
-		outw(0x3C68, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		outw(0x0001, chan_iobase + REG_C0H);
+		outw(0x3C68, chan_iobase + REG_C0L);
 
 		/*  Reset the counter */
-		outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		outw(0x8000, chan_iobase + REG_C0C);
 		/*  Load the counter from PR0 */
-		outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		outw(0x4000, chan_iobase + REG_C0C);
 
 		/*  Reset RCAP (fires one-shot) */
-		outw(0x0008, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		outw(0x0008, chan_iobase + REG_C0C);
 
 #endif
 
 #if 1
 		/*  Set Counter Mode Register */
-		cmReg.value = insn->data[1] & 0xFFFF;
-
-/* printk("s526: Counter Mode register=%x\n", cmReg.value); */
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		cmReg.value = data[1] & 0xffff;
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Reset the counter if it is software preload */
 		if (cmReg.reg.autoLoadResetRcap == 0) {
 			/*  Reset the counter */
-			outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+			outw(0x8000, chan_iobase + REG_C0C);
 			/* Load the counter from PR0
-			 * outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+			 * outw(0x4000, chan_iobase + REG_C0C);
 			 */
 		}
 #else
@@ -325,47 +217,47 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		cmReg.reg.countDirCtrl = 0;
 
 		/*  data[1] contains GPCT_X1, GPCT_X2 or GPCT_X4 */
-		if (insn->data[1] == GPCT_X2)
+		if (data[1] == GPCT_X2)
 			cmReg.reg.clockSource = 1;
-		else if (insn->data[1] == GPCT_X4)
+		else if (data[1] == GPCT_X4)
 			cmReg.reg.clockSource = 2;
 		else
 			cmReg.reg.clockSource = 0;
 
 		/*  When to take into account the indexpulse: */
-		/*if (insn->data[2] == GPCT_IndexPhaseLowLow) {
-		} else if (insn->data[2] == GPCT_IndexPhaseLowHigh) {
-		} else if (insn->data[2] == GPCT_IndexPhaseHighLow) {
-		} else if (insn->data[2] == GPCT_IndexPhaseHighHigh) {
+		/*if (data[2] == GPCT_IndexPhaseLowLow) {
+		} else if (data[2] == GPCT_IndexPhaseLowHigh) {
+		} else if (data[2] == GPCT_IndexPhaseHighLow) {
+		} else if (data[2] == GPCT_IndexPhaseHighHigh) {
 		}*/
 		/*  Take into account the index pulse? */
-		if (insn->data[3] == GPCT_RESET_COUNTER_ON_INDEX)
+		if (data[3] == GPCT_RESET_COUNTER_ON_INDEX)
 			/*  Auto load with INDEX^ */
 			cmReg.reg.autoLoadResetRcap = 4;
 
 		/*  Set Counter Mode Register */
-		cmReg.value = (short)(insn->data[1] & 0xFFFF);
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		cmReg.value = data[1] & 0xffff;
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Load the pre-load register high word */
-		value = (short)((insn->data[2] >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+		val = (data[2] >> 16) & 0xffff;
+		outw(val, chan_iobase + REG_C0H);
 
 		/*  Load the pre-load register low word */
-		value = (short)(insn->data[2] & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		val = data[2] & 0xffff;
+		outw(val, chan_iobase + REG_C0L);
 
 		/*  Write the Counter Control Register */
-		if (insn->data[3] != 0) {
-			value = (short)(insn->data[3] & 0xFFFF);
-			outw(value, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		if (data[3]) {
+			val = data[3] & 0xffff;
+			outw(val, chan_iobase + REG_C0C);
 		}
 		/*  Reset the counter if it is software preload */
 		if (cmReg.reg.autoLoadResetRcap == 0) {
 			/*  Reset the counter */
-			outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+			outw(0x8000, chan_iobase + REG_C0C);
 			/*  Load the counter from PR0 */
-			outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+			outw(0x4000, chan_iobase + REG_C0C);
 		}
 #endif
 		break;
@@ -378,40 +270,38 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		   data[3]: Pre-load Register 1 Value
 		   data[4]: Conter Control Register
 		 */
-		printk(KERN_INFO "s526: GPCT_INSN_CONFIG: Configuring SPG\n");
-		devpriv->s526_gpct_config[subdev_channel].app =
-		    SinglePulseGeneration;
+		devpriv->gpct_config[chan] = data[0];
 
 		/*  Set Counter Mode Register */
-		cmReg.value = (short)(insn->data[1] & 0xFFFF);
+		cmReg.value = data[1] & 0xffff;
 		cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Load the pre-load register 0 high word */
-		value = (short)((insn->data[2] >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+		val = (data[2] >> 16) & 0xffff;
+		outw(val, chan_iobase + REG_C0H);
 
 		/*  Load the pre-load register 0 low word */
-		value = (short)(insn->data[2] & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		val = data[2] & 0xffff;
+		outw(val, chan_iobase + REG_C0L);
 
 		/*  Set Counter Mode Register */
-		cmReg.value = (short)(insn->data[1] & 0xFFFF);
+		cmReg.value = data[1] & 0xffff;
 		cmReg.reg.preloadRegSel = 1;	/*  PR1 */
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Load the pre-load register 1 high word */
-		value = (short)((insn->data[3] >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+		val = (data[3] >> 16) & 0xffff;
+		outw(val, chan_iobase + REG_C0H);
 
 		/*  Load the pre-load register 1 low word */
-		value = (short)(insn->data[3] & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		val = data[3] & 0xffff;
+		outw(val, chan_iobase + REG_C0L);
 
 		/*  Write the Counter Control Register */
-		if (insn->data[4] != 0) {
-			value = (short)(insn->data[4] & 0xFFFF);
-			outw(value, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		if (data[4]) {
+			val = data[4] & 0xffff;
+			outw(val, chan_iobase + REG_C0C);
 		}
 		break;
 
@@ -423,45 +313,42 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		   data[3]: Pre-load Register 1 Value
 		   data[4]: Conter Control Register
 		 */
-		printk(KERN_INFO "s526: GPCT_INSN_CONFIG: Configuring PTG\n");
-		devpriv->s526_gpct_config[subdev_channel].app =
-		    PulseTrainGeneration;
+		devpriv->gpct_config[chan] = data[0];
 
 		/*  Set Counter Mode Register */
-		cmReg.value = (short)(insn->data[1] & 0xFFFF);
+		cmReg.value = data[1] & 0xffff;
 		cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Load the pre-load register 0 high word */
-		value = (short)((insn->data[2] >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+		val = (data[2] >> 16) & 0xffff;
+		outw(val, chan_iobase + REG_C0H);
 
 		/*  Load the pre-load register 0 low word */
-		value = (short)(insn->data[2] & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		val = data[2] & 0xffff;
+		outw(val, chan_iobase + REG_C0L);
 
 		/*  Set Counter Mode Register */
-		cmReg.value = (short)(insn->data[1] & 0xFFFF);
+		cmReg.value = data[1] & 0xffff;
 		cmReg.reg.preloadRegSel = 1;	/*  PR1 */
-		outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+		outw(cmReg.value, chan_iobase + REG_C0M);
 
 		/*  Load the pre-load register 1 high word */
-		value = (short)((insn->data[3] >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+		val = (data[3] >> 16) & 0xffff;
+		outw(val, chan_iobase + REG_C0H);
 
 		/*  Load the pre-load register 1 low word */
-		value = (short)(insn->data[3] & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		val = data[3] & 0xffff;
+		outw(val, chan_iobase + REG_C0L);
 
 		/*  Write the Counter Control Register */
-		if (insn->data[4] != 0) {
-			value = (short)(insn->data[4] & 0xFFFF);
-			outw(value, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+		if (data[4]) {
+			val = data[4] & 0xffff;
+			outw(val, chan_iobase + REG_C0C);
 		}
 		break;
 
 	default:
-		printk(KERN_ERR "s526: unsupported GPCT_insn_config\n");
 		return -EINVAL;
 		break;
 	}
@@ -470,65 +357,40 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 }
 
 static int s526_gpct_winsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn,
 			   unsigned int *data)
 {
-	int subdev_channel = CR_CHAN(insn->chanspec);	/*  Unpack chanspec */
-	short value;
-	union cmReg cmReg;
-
-	printk(KERN_INFO "s526: GPCT_INSN_WRITE on channel %d\n",
-					subdev_channel);
-	cmReg.value = inw(ADDR_CHAN_REG(REG_C0M, subdev_channel));
-	printk(KERN_INFO "s526: Counter Mode Register: %x\n", cmReg.value);
-	/*  Check what Application of Counter this channel is configured for */
-	switch (devpriv->s526_gpct_config[subdev_channel].app) {
-	case PositionMeasurement:
-		printk(KERN_INFO "S526: INSN_WRITE: PM\n");
-		outw(0xFFFF & ((*data) >> 16), ADDR_CHAN_REG(REG_C0H,
-							     subdev_channel));
-		outw(0xFFFF & (*data), ADDR_CHAN_REG(REG_C0L, subdev_channel));
-		break;
+	struct s526_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long chan_iobase = dev->iobase + chan * 8;
 
-	case SinglePulseGeneration:
-		printk(KERN_INFO "S526: INSN_WRITE: SPG\n");
-		outw(0xFFFF & ((*data) >> 16), ADDR_CHAN_REG(REG_C0H,
-							     subdev_channel));
-		outw(0xFFFF & (*data), ADDR_CHAN_REG(REG_C0L, subdev_channel));
-		break;
+	inw(chan_iobase + REG_C0M);	/* Is this read required? */
 
-	case PulseTrainGeneration:
+	/*  Check what Application of Counter this channel is configured for */
+	switch (devpriv->gpct_config[chan]) {
+	case INSN_CONFIG_GPCT_PULSE_TRAIN_GENERATOR:
 		/* data[0] contains the PULSE_WIDTH
 		   data[1] contains the PULSE_PERIOD
 		   @pre PULSE_PERIOD > PULSE_WIDTH > 0
 		   The above periods must be expressed as a multiple of the
 		   pulse frequency on the selected source
 		 */
-		printk(KERN_INFO "S526: INSN_WRITE: PTG\n");
-		if ((insn->data[1] > insn->data[0]) && (insn->data[0] > 0)) {
-			(devpriv->s526_gpct_config[subdev_channel]).data[0] =
-			    insn->data[0];
-			(devpriv->s526_gpct_config[subdev_channel]).data[1] =
-			    insn->data[1];
-		} else {
-			printk(KERN_ERR "s526: INSN_WRITE: PTG: Problem with Pulse params -> %d %d\n",
-				insn->data[0], insn->data[1]);
+		if ((data[1] <= data[0]) || !data[0])
 			return -EINVAL;
-		}
 
-		value = (short)((*data >> 16) & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0H, subdev_channel));
-		value = (short)(*data & 0xFFFF);
-		outw(value, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+		/* Fall thru to write the PULSE_WIDTH */
+
+	case INSN_CONFIG_GPCT_QUADRATURE_ENCODER:
+	case INSN_CONFIG_GPCT_SINGLE_PULSE_GENERATOR:
+		outw((data[0] >> 16) & 0xffff, chan_iobase + REG_C0H);
+		outw(data[0] & 0xffff, chan_iobase + REG_C0L);
 		break;
-	default:		/*  Impossible */
-		printk
-		    ("s526: INSN_WRITE: Functionality %d not implemented yet\n",
-		     devpriv->s526_gpct_config[subdev_channel].app);
+
+	default:
 		return -EINVAL;
-		break;
 	}
-	/*  return the number of samples written */
+
 	return insn->n;
 }
 
@@ -537,6 +399,7 @@ static int s526_ai_insn_config(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct s526_private *devpriv = dev->private;
 	int result = -EINVAL;
 
 	if (insn->n < 1)
@@ -552,62 +415,50 @@ static int s526_ai_insn_config(struct comedi_device *dev,
 	 * INSN_READ handler. */
 
 	/*  Enable ADC interrupt */
-	outw(ISR_ADC_DONE, ADDR_REG(REG_IER));
-/* printk("s526: ADC current value: 0x%04x\n", inw(ADDR_REG(REG_ADC))); */
-	devpriv->s526_ai_config = (data[0] & 0x3FF) << 5;
+	outw(ISR_ADC_DONE, dev->iobase + REG_IER);
+	devpriv->ai_config = (data[0] & 0x3ff) << 5;
 	if (data[1] > 0)
-		devpriv->s526_ai_config |= 0x8000;	/* set the delay */
+		devpriv->ai_config |= 0x8000;	/* set the delay */
 
-	devpriv->s526_ai_config |= 0x0001;	/*  ADC start bit. */
+	devpriv->ai_config |= 0x0001;		/* ADC start bit */
 
 	return result;
 }
 
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
 static int s526_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
+	struct s526_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int n, i;
-	int chan = CR_CHAN(insn->chanspec);
 	unsigned short value;
 	unsigned int d;
 	unsigned int status;
 
 	/* Set configured delay, enable channel for this channel only,
 	 * select "ADC read" channel, set "ADC start" bit. */
-	value = (devpriv->s526_ai_config & 0x8000) |
-	    ((1 << 5) << chan) | (chan << 1) | 0x0001;
+	value = (devpriv->ai_config & 0x8000) |
+		((1 << 5) << chan) | (chan << 1) | 0x0001;
 
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
-		outw(value, ADDR_REG(REG_ADC));
-/* printk("s526: Wrote 0x%04x to ADC\n", value); */
-/* printk("s526: ADC reg=0x%04x\n", inw(ADDR_REG(REG_ADC))); */
+		outw(value, dev->iobase + REG_ADC);
 
 #define TIMEOUT 100
 		/* wait for conversion to end */
 		for (i = 0; i < TIMEOUT; i++) {
-			status = inw(ADDR_REG(REG_ISR));
+			status = inw(dev->iobase + REG_ISR);
 			if (status & ISR_ADC_DONE) {
-				outw(ISR_ADC_DONE, ADDR_REG(REG_ISR));
+				outw(ISR_ADC_DONE, dev->iobase + REG_ISR);
 				break;
 			}
 		}
-		if (i == TIMEOUT) {
-			/* printk() should be used instead of printk()
-			 * whenever the code can be called from real-time. */
-			printk(KERN_ERR "s526: ADC(0x%04x) timeout\n",
-			       inw(ADDR_REG(REG_ISR)));
+		if (i == TIMEOUT)
 			return -ETIMEDOUT;
-		}
 
 		/* read data */
-		d = inw(ADDR_REG(REG_ADD));
-/* printk("AI[%d]=0x%04x\n", n, (unsigned short)(d & 0xFFFF)); */
+		d = inw(dev->iobase + REG_ADD);
 
 		/* munge data */
 		data[n] = d ^ 0x8000;
@@ -620,40 +471,30 @@ static int s526_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int s526_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
+	struct s526_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned short val;
+	int i;
 
-/* printk("s526_ao_winsn\n"); */
 	val = chan << 1;
-/* outw(val, dev->iobase + REG_DAC); */
-	outw(val, ADDR_REG(REG_DAC));
+	outw(val, dev->iobase + REG_DAC);
 
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
 	for (i = 0; i < insn->n; i++) {
-		/* a typical programming sequence */
-		/* write the data to preload register
-		 * outw(data[i], dev->iobase + REG_ADD);
-		 */
-		/* write the data to preload register */
-		outw(data[i], ADDR_REG(REG_ADD));
+		outw(data[i], dev->iobase + REG_ADD);
 		devpriv->ao_readback[chan] = data[i];
-/* outw(val + 1, dev->iobase + REG_DAC);  starts the D/A conversion. */
-		outw(val + 1, ADDR_REG(REG_DAC)); /*starts the D/A conversion.*/
+		/* starts the D/A conversion */
+		outw(val + 1, dev->iobase + REG_DAC);
 	}
 
-	/* return the number of samples read/written */
 	return i;
 }
 
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
 static int s526_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
+	struct s526_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[chan];
@@ -661,30 +502,18 @@ static int s526_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return i;
 }
 
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write */
 static int s526_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
-		/* Write out the new digital output lines */
-		outw(s->state, ADDR_REG(REG_DIO));
+
+		outw(s->state, dev->iobase + REG_DIO);
 	}
 
-	/* on return, data[1] contains the value of the digital
-	 * input and output lines. */
-	data[1] = inw(ADDR_REG(REG_DIO)) & 0xFF; /* low 8 bits are the data */
-	/* or we could just return the software copy of the output values if
-	 * it was a purely digital output subdevice */
-	/* data[1]=s->state & 0xFF; */
+	data[1] = inw(dev->iobase + REG_DIO) & 0xff;
 
 	return insn->n;
 }
@@ -693,16 +522,9 @@ static int s526_dio_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	int chan = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int group, mask;
 
-	printk(KERN_INFO "S526 DIO insn_config\n");
-
-	/* The input or output configuration of each digital line is
-	 * configured by a special insn_config instruction.  chanspec
-	 * contains the channel to be changed, and data[0] contains the
-	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
-
 	group = chan >> 2;
 	mask = 0xF << (group << 2);
 	switch (data[0]) {
@@ -721,88 +543,60 @@ static int s526_dio_insn_config(struct comedi_device *dev,
 	default:
 		return -EINVAL;
 	}
-	outw(s->state, ADDR_REG(REG_DIO));
+	outw(s->state, dev->iobase + REG_DIO);
 
 	return 1;
 }
 
 static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct s526_board *board = comedi_board(dev);
+	struct s526_private *devpriv;
 	struct comedi_subdevice *s;
 	int iobase;
-	int i, n;
 	int ret;
-/* short value; */
-/* int subdev_channel = 0; */
-	union cmReg cmReg;
 
-	printk(KERN_INFO "comedi%d: s526: ", dev->minor);
+	dev->board_name = dev->driver->driver_name;
 
 	iobase = it->options[0];
-	if (!iobase || !request_region(iobase, S526_IOSIZE, board->name)) {
+	if (!iobase || !request_region(iobase, S526_IOSIZE, dev->board_name)) {
 		comedi_error(dev, "I/O port conflict");
 		return -EIO;
 	}
 	dev->iobase = iobase;
 
-	printk("iobase=0x%lx\n", dev->iobase);
-
-	/*** make it a little quieter, exw, 8/29/06
-	for (i = 0; i < S526_NUM_PORTS; i++) {
-		printk("0x%02x: 0x%04x\n", ADDR_REG(s526_ports[i]),
-				inw(ADDR_REG(s526_ports[i])));
-	}
-	***/
-
-	dev->board_name = board->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct s526_private)) < 0)
-		return -ENOMEM;
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret)
+		return ret;
+	devpriv = dev->private;
 
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* GENERAL-PURPOSE COUNTER/TIME (GPCT) */
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL;
-	/* KG: What does SDF_LSAMPL (see multiq3.c) mean? */
-	s->n_chan = board->gpct_chans;
+	s->n_chan = 4;
 	s->maxdata = 0x00ffffff;	/* 24 bit counter */
 	s->insn_read = s526_gpct_rinsn;
 	s->insn_config = s526_gpct_insn_config;
 	s->insn_write = s526_gpct_winsn;
 
-	/* Command are not implemented yet, however they are necessary to
-	   allocate the necessary memory for the comedi_async struct (used
-	   to trigger the GPCT in case of pulsegenerator function */
-	/* s->do_cmd = s526_gpct_cmd; */
-	/* s->do_cmdtest = s526_gpct_cmdtest; */
-	/* s->cancel = s526_gpct_cancel; */
-
-	s = dev->subdevices + 1;
-	/* dev->read_subdev=s; */
+	s = &dev->subdevices[1];
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
-	/* we support differential */
 	s->subdev_flags = SDF_READABLE | SDF_DIFF;
 	/* channels 0 to 7 are the regular differential inputs */
 	/* channel 8 is "reference 0" (+10V), channel 9 is "reference 1" (0V) */
 	s->n_chan = 10;
 	s->maxdata = 0xffff;
 	s->range_table = &range_bipolar10;
-	s->len_chanlist = 16;	/* This is the maximum chanlist length that
-				   the board can handle */
+	s->len_chanlist = 16;
 	s->insn_read = s526_ai_rinsn;
 	s->insn_config = s526_ai_insn_config;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -812,105 +606,18 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = s526_ao_winsn;
 	s->insn_read = s526_ao_rinsn;
 
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	/* digital i/o subdevice */
-	if (board->have_dio) {
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 8;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_bits = s526_dio_insn_bits;
-		s->insn_config = s526_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-
-#if 0
-	/*  Example of Counter Application */
-	/* One-shot (software trigger) */
-	cmReg.reg.coutSource = 0;	/*  out RCAP */
-	cmReg.reg.coutPolarity = 1;	/*  Polarity inverted */
-	cmReg.reg.autoLoadResetRcap = 1;/*  Auto load 0:disabled, 1:enabled */
-	cmReg.reg.hwCtEnableSource = 3;	/*  NOT RCAP */
-	cmReg.reg.ctEnableCtrl = 2;	/*  Hardware */
-	cmReg.reg.clockSource = 2;	/*  Internal */
-	cmReg.reg.countDir = 1;	/*  Down */
-	cmReg.reg.countDirCtrl = 1;	/*  Software */
-	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
-	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-	cmReg.reg.reserved = 0;
-
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
-
-	outw(0x0001, ADDR_CHAN_REG(REG_C0H, subdev_channel));
-	outw(0x3C68, ADDR_CHAN_REG(REG_C0L, subdev_channel));
-
-	/*  Reset the counter */
-	outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-	/*  Load the counter from PR0 */
-	outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-	/*  Reset RCAP (fires one-shot) */
-	outw(0x0008, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-
-#else
-
-	/*  Set Counter Mode Register */
-	cmReg.reg.coutSource = 0;	/*  out RCAP */
-	cmReg.reg.coutPolarity = 0;	/*  Polarity inverted */
-	cmReg.reg.autoLoadResetRcap = 0;	/*  Auto load disabled */
-	cmReg.reg.hwCtEnableSource = 2;	/*  NOT RCAP */
-	cmReg.reg.ctEnableCtrl = 1;	/*  1: Software,  >1 : Hardware */
-	cmReg.reg.clockSource = 3;	/*  x4 */
-	cmReg.reg.countDir = 0;	/*  up */
-	cmReg.reg.countDirCtrl = 0;	/*  quadrature */
-	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
-	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-	cmReg.reg.reserved = 0;
-
-	n = 0;
-	printk(KERN_INFO "Mode reg=0x%04x, 0x%04lx\n",
-		cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	udelay(1000);
-	printk(KERN_INFO "Read back mode reg=0x%04x\n",
-		inw(ADDR_CHAN_REG(REG_C0M, n)));
-
-	/*  Load the pre-load register high word */
-/* value = (short) (0x55); */
-/* outw(value, ADDR_CHAN_REG(REG_C0H, n)); */
-
-	/*  Load the pre-load register low word */
-/* value = (short)(0xaa55); */
-/* outw(value, ADDR_CHAN_REG(REG_C0L, n)); */
-
-	/*  Write the Counter Control Register */
-/* outw(value, ADDR_CHAN_REG(REG_C0C, 0)); */
-
-	/*  Reset the counter if it is software preload */
-	if (cmReg.reg.autoLoadResetRcap == 0) {
-		/*  Reset the counter */
-		outw(0x8000, ADDR_CHAN_REG(REG_C0C, n));
-		/*  Load the counter from PR0 */
-		outw(0x4000, ADDR_CHAN_REG(REG_C0C, n));
-	}
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = s526_dio_insn_bits;
+	s->insn_config = s526_dio_insn_config;
 
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	udelay(1000);
-	printk(KERN_INFO "Read back mode reg=0x%04x\n",
-			inw(ADDR_CHAN_REG(REG_C0M, n)));
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
-#endif
-	printk(KERN_INFO "Current registres:\n");
-
-	for (i = 0; i < S526_NUM_PORTS; i++) {
-		printk(KERN_INFO "0x%02lx: 0x%04x\n",
-			ADDR_REG(s526_ports[i]), inw(ADDR_REG(s526_ports[i])));
-	}
 	return 1;
 }
 
@@ -925,9 +632,6 @@ static struct comedi_driver s526_driver = {
 	.module		= THIS_MODULE,
 	.attach		= s526_attach,
 	.detach		= s526_detach,
-	.board_name	= &s526_boards[0].name,
-	.offset		= sizeof(struct s526_board),
-	.num_names	= ARRAY_SIZE(s526_boards),
 };
 module_comedi_driver(s526_driver);
 
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index f90578e5e727..551d68b7837c 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -32,11 +32,7 @@ Authors: Gianluca Palli <gpalli@deis.unibo.it>,
 Updated: Fri, 15 Feb 2008 10:28:42 +0000
 Status: experimental
 
-Configuration options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
+Configuration options: not applicable, uses PCI auto config
 
 INSN_CONFIG instructions:
   analog input:
@@ -82,45 +78,8 @@ INSN_CONFIG instructions:
 #define PCI_SUBVENDOR_ID_S626 0x6000
 #define PCI_SUBDEVICE_ID_S626 0x0272
 
-struct s626_board {
-	const char *name;
-	int vendor_id;
-	int device_id;
-	int subvendor_id;
-	int subdevice_id;
-	int ai_chans;
-	int ai_bits;
-	int ao_chans;
-	int ao_bits;
-	int dio_chans;
-	int dio_banks;
-	int enc_chans;
-};
-
-static const struct s626_board s626_boards[] = {
-	{
-	 .name = "s626",
-	 .vendor_id = PCI_VENDOR_ID_S626,
-	 .device_id = PCI_DEVICE_ID_S626,
-	 .subvendor_id = PCI_SUBVENDOR_ID_S626,
-	 .subdevice_id = PCI_SUBDEVICE_ID_S626,
-	 .ai_chans = S626_ADC_CHANNELS,
-	 .ai_bits = 14,
-	 .ao_chans = S626_DAC_CHANNELS,
-	 .ao_bits = 13,
-	 .dio_chans = S626_DIO_CHANNELS,
-	 .dio_banks = S626_DIO_BANKS,
-	 .enc_chans = S626_ENCODER_CHANNELS,
-	 }
-};
-
-#define thisboard ((const struct s626_board *)dev->board_ptr)
-
 struct s626_private {
-	struct pci_dev *pdev;
 	void __iomem *base_addr;
-	int got_regions;
-	short allocatedBuf;
 	uint8_t ai_cmd_running;	/*  ai_cmd is running */
 	uint8_t ai_continous;	/*  continous acquisition */
 	int ai_sample_count;	/*  number of samples to acquire */
@@ -139,9 +98,7 @@ struct s626_private {
 	/* Pointer to logical adrs of DMA buffer used to hold DAC  data. */
 	uint16_t Dacpol;	/* Image of DAC polarity register. */
 	uint8_t TrimSetpoint[12];	/* Images of TrimDAC setpoints */
-	uint16_t ChargeEnabled;	/* Image of MISC2 Battery */
 	/* Charge Enabled (0 or WRMISC2_CHARGE_ENABLE). */
-	uint16_t WDInterval;	/* Image of MISC2 watchdog interval control bits. */
 	uint32_t I2CAdrs;
 	/* I2C device address for onboard EEPROM (board rev dependent). */
 	/*   short         I2Cards; */
@@ -1548,56 +1505,28 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.  Using
-	 * the cmdtest ioctl, a user can create a valid cmd and then have it
-	 * executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests the
-	 * command passes. */
-
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT | TRIG_EXT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT | TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually
-	   compatible */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT
-	    && cmd->scan_begin_src != TRIG_FOLLOW)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER &&
-	    cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1847,6 +1776,9 @@ static int s626_dio_insn_config(struct comedi_device *dev,
 /* Now this function initializes the value of the counter (data[0])
    and set the subdevice. To complete with trigger and interrupt
    configuration */
+/* FIXME: data[0] is supposed to be an INSN_CONFIG_xxx constant indicating
+ * what is being configured, but this function appears to be using data[0]
+ * as a variable. */
 static int s626_enc_insn_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
@@ -1868,7 +1800,7 @@ static int s626_enc_insn_config(struct comedi_device *dev,
 	/*   (data==NULL) ? (Preloadvalue=0) : (Preloadvalue=data[0]); */
 
 	k->SetMode(dev, k, Setup, TRUE);
-	Preload(dev, k, *(insn->data));
+	Preload(dev, k, data[0]);
 	k->PulseIndex(dev, k);
 	SetLatchSource(dev, k, valueSrclatch);
 	k->SetEnable(dev, k, (uint16_t) (enab != 0));
@@ -1920,6 +1852,7 @@ static void WriteMISC2(struct comedi_device *dev, uint16_t NewImage)
 static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
 		      size_t bsize)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	void *vbptr;
 	dma_addr_t vpptr;
 
@@ -1930,7 +1863,7 @@ static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
 	vbptr = pdma->LogicalBase;
 	vpptr = pdma->PhysicalBase;
 	if (vbptr) {
-		pci_free_consistent(devpriv->pdev, bsize, vbptr, vpptr);
+		pci_free_consistent(pcidev, bsize, vbptr, vpptr);
 		pdma->LogicalBase = NULL;
 		pdma->PhysicalBase = 0;
 	}
@@ -2476,146 +2409,317 @@ static void CountersInit(struct comedi_device *dev)
 	}
 }
 
-static struct pci_dev *s626_find_pci(struct comedi_device *dev,
-				     struct comedi_devconfig *it)
+static int s626_allocate_dma_buffers(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-	int i;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	void *addr;
+	dma_addr_t appdma;
 
-	for (i = 0; i < ARRAY_SIZE(s626_boards) && !pcidev; i++) {
-		do {
-			pcidev = pci_get_subsys(s626_boards[i].vendor_id,
-						s626_boards[i].device_id,
-						s626_boards[i].subvendor_id,
-						s626_boards[i].subdevice_id,
-						pcidev);
-
-			if ((bus || slot) && pcidev) {
-				/* matches requested bus/slot */
-				if (pcidev->bus->number == bus &&
-				    PCI_SLOT(pcidev->devfn) == slot)
-					break;
-			} else {
-				break;
-			}
-		} while (1);
-	}
-	return pcidev;
+	addr = pci_alloc_consistent(pcidev, DMABUF_SIZE, &appdma);
+	if (!addr)
+		return -ENOMEM;
+	devpriv->ANABuf.LogicalBase = addr;
+	devpriv->ANABuf.PhysicalBase = appdma;
+
+	addr = pci_alloc_consistent(pcidev, DMABUF_SIZE, &appdma);
+	if (!addr)
+		return -ENOMEM;
+	devpriv->RPSBuf.LogicalBase = addr;
+	devpriv->RPSBuf.PhysicalBase = appdma;
+
+	return 0;
 }
 
-static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static void s626_initialize(struct comedi_device *dev)
 {
-/*   uint8_t	PollList; */
-/*   uint16_t	AdcData; */
-/*   uint16_t	StartVal; */
-/*   uint16_t	index; */
-/*   unsigned int data[16]; */
-	int result;
+	dma_addr_t pPhysBuf;
+	uint16_t chan;
 	int i;
-	int ret;
-	resource_size_t resourceStart;
-	dma_addr_t appdma;
-	struct comedi_subdevice *s;
 
-	if (alloc_private(dev, sizeof(struct s626_private)) < 0)
-		return -ENOMEM;
+	/* Enable DEBI and audio pins, enable I2C interface */
+	MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
 
-	devpriv->pdev = s626_find_pci(dev, it);
-	if (!devpriv->pdev) {
-		printk(KERN_ERR "s626_attach: Board not present!!!\n");
-		return -ENODEV;
-	}
+	/*
+	 *  Configure DEBI operating mode
+	 *
+	 *   Local bus is 16 bits wide
+	 *   Declare DEBI transfer timeout interval
+	 *   Set up byte lane steering
+	 *   Intel-compatible local bus (DEBI never times out)
+	 */
+	WR7146(P_DEBICFG, DEBI_CFG_SLAVE16 |
+			  (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
+			  DEBI_SWAP | DEBI_CFG_INTEL);
+
+	/* Disable MMU paging */
+	WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);
 
-	result = comedi_pci_enable(devpriv->pdev, "s626");
-	if (result < 0) {
-		printk(KERN_ERR "s626_attach: comedi_pci_enable fails\n");
-		return -ENODEV;
+	/* Init GPIO so that ADC Start* is negated */
+	WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+
+	/* I2C device address for onboard eeprom (revb) */
+	devpriv->I2CAdrs = 0xA0;
+
+	/*
+	 * Issue an I2C ABORT command to halt any I2C
+	 * operation in progress and reset BUSY flag.
+	 */
+	WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);
+	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+	while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0)
+		;
+
+	/*
+	 * Per SAA7146 data sheet, write to STATUS
+	 * reg twice to reset all  I2C error flags.
+	 */
+	for (i = 0; i < 2; i++) {
+		WR7146(P_I2CSTAT, I2C_CLKSEL);
+		MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+		while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
+			;
 	}
-	devpriv->got_regions = 1;
 
-	resourceStart = pci_resource_start(devpriv->pdev, 0);
+	/*
+	 * Init audio interface functional attributes: set DAC/ADC
+	 * serial clock rates, invert DAC serial clock so that
+	 * DAC data setup times are satisfied, enable DAC serial
+	 * clock out.
+	 */
+	WR7146(P_ACON2, ACON2_INIT);
+
+	/*
+	 * Set up TSL1 slot list, which is used to control the
+	 * accumulation of ADC data: RSD1 = shift data in on SD1.
+	 * SIB_A1  = store data uint8_t at next available location
+	 * in FB BUFFER1 register.
+	 */
+	WR7146(P_TSL1, RSD1 | SIB_A1);
+	WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);
+
+	/* Enable TSL1 slot list so that it executes all the time */
+	WR7146(P_ACON1, ACON1_ADCSTART);
+
+	/*
+	 * Initialize RPS registers used for ADC
+	 */
+
+	/* Physical start of RPS program */
+	WR7146(P_RPSADDR1, (uint32_t)devpriv->RPSBuf.PhysicalBase);
+	/* RPS program performs no explicit mem writes */
+	WR7146(P_RPSPAGE1, 0);
+	/* Disable RPS timeouts */
+	WR7146(P_RPS1_TOUT, 0);
+
+#if 0
+	/*
+	 * SAA7146 BUG WORKAROUND
+	 *
+	 * Initialize SAA7146 ADC interface to a known state by
+	 * invoking ADCs until FB BUFFER 1 register shows that it
+	 * is correctly receiving ADC data. This is necessary
+	 * because the SAA7146 ADC interface does not start up in
+	 * a defined state after a PCI reset.
+	 */
+
+	{
+	uint8_t PollList;
+	uint16_t AdcData;
+	uint16_t StartVal;
+	uint16_t index;
+	unsigned int data[16];
+
+	/* Create a simple polling list for analog input channel 0 */
+	PollList = EOPL;
+	ResetADC(dev, &PollList);
 
-	devpriv->base_addr = ioremap(resourceStart, SIZEOF_ADDRESS_SPACE);
-	if (devpriv->base_addr == NULL) {
-		printk(KERN_ERR "s626_attach: IOREMAP failed\n");
-		return -ENODEV;
+	/* Get initial ADC value */
+	s626_ai_rinsn(dev, dev->subdevices, NULL, data);
+	StartVal = data[0];
+
+	/*
+	 * VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION.
+	 *
+	 * Invoke ADCs until the new ADC value differs from the initial
+	 * value or a timeout occurs.  The timeout protects against the
+	 * possibility that the driver is restarting and the ADC data is a
+	 * fixed value resulting from the applied ADC analog input being
+	 * unusually quiet or at the rail.
+	 */
+	for (index = 0; index < 500; index++) {
+		s626_ai_rinsn(dev, dev->subdevices, NULL, data);
+		AdcData = data[0];
+		if (AdcData != StartVal)
+			break;
 	}
 
-	if (devpriv->base_addr) {
-		/* disable master interrupt */
-		writel(0, devpriv->base_addr + P_IER);
+	}
+#endif	/* SAA7146 BUG WORKAROUND */
 
-		/* soft reset */
-		writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+	/*
+	 * Initialize the DAC interface
+	 */
 
-		/* DMA FIXME DMA// */
+	/*
+	 * Init Audio2's output DMAC attributes:
+	 *   burst length = 1 DWORD
+	 *   threshold = 1 DWORD.
+	 */
+	WR7146(P_PCI_BT_A, 0);
 
-		/* adc buffer allocation */
-		devpriv->allocatedBuf = 0;
+	/*
+	 * Init Audio2's output DMA physical addresses.  The protection
+	 * address is set to 1 DWORD past the base address so that a
+	 * single DWORD will be transferred each time a DMA transfer is
+	 * enabled.
+	 */
+	pPhysBuf = devpriv->ANABuf.PhysicalBase +
+		   (DAC_WDMABUF_OS * sizeof(uint32_t));
+	WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);
+	WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));
 
-		devpriv->ANABuf.LogicalBase =
-		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+	/*
+	 * Cache Audio2's output DMA buffer logical address.  This is
+	 * where DAC data is buffered for A2 output DMA transfers.
+	 */
+	devpriv->pDacWBuf = (uint32_t *)devpriv->ANABuf.LogicalBase +
+			    DAC_WDMABUF_OS;
 
-		if (devpriv->ANABuf.LogicalBase == NULL) {
-			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
-			return -ENOMEM;
-		}
+	/*
+	 * Audio2's output channels does not use paging.  The
+	 * protection violation handling bit is set so that the
+	 * DMAC will automatically halt and its PCI address pointer
+	 * will be reset when the protection address is reached.
+	 */
+	WR7146(P_PAGEA2_OUT, 8);
 
-		devpriv->ANABuf.PhysicalBase = appdma;
+	/*
+	 * Initialize time slot list 2 (TSL2), which is used to control
+	 * the clock generation for and serialization of data to be sent
+	 * to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
+	 * execution; this permits other slots to be safely modified
+	 * without first turning off the TSL sequencer (which is
+	 * apparently impossible to do).  Also, SD3 (which is driven by a
+	 * pull-up resistor) is shifted in and stored to the MSB of
+	 * FB_BUFFER2 to be used as evidence that the slot sequence has
+	 * not yet finished executing.
+	 */
 
-		devpriv->allocatedBuf++;
+	/* Slot 0: Trap TSL execution, shift 0xFF into FB_BUFFER2 */
+	SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);
 
-		devpriv->RPSBuf.LogicalBase =
-		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+	/*
+	 * Initialize slot 1, which is constant.  Slot 1 causes a
+	 * DWORD to be transferred from audio channel 2's output FIFO
+	 * to the FIFO's output buffer so that it can be serialized
+	 * and sent to the DAC during subsequent slots.  All remaining
+	 * slots are dynamically populated as required by the target
+	 * DAC device.
+	 */
 
-		if (devpriv->RPSBuf.LogicalBase == NULL) {
-			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
-			return -ENOMEM;
-		}
+	/* Slot 1: Fetch DWORD from Audio2's output FIFO */
+	SETVECT(1, LF_A2);
+
+	/* Start DAC's audio interface (TSL2) running */
+	WR7146(P_ACON1, ACON1_DACSTART);
+
+	/*
+	 * Init Trim DACs to calibrated values.  Do it twice because the
+	 * SAA7146 audio channel does not always reset properly and
+	 * sometimes causes the first few TrimDAC writes to malfunction.
+	 */
+	LoadTrimDACs(dev);
+	LoadTrimDACs(dev);
+
+	/*
+	 * Manually init all gate array hardware in case this is a soft
+	 * reset (we have no way of determining whether this is a warm
+	 * or cold start).  This is necessary because the gate array will
+	 * reset only in response to a PCI hard reset; there is no soft
+	 * reset function.
+	 */
 
-		devpriv->RPSBuf.PhysicalBase = appdma;
+	/*
+	 * Init all DAC outputs to 0V and init all DAC setpoint and
+	 * polarity images.
+	 */
+	for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
+		SetDAC(dev, chan, 0);
 
-		devpriv->allocatedBuf++;
+	/* Init counters */
+	CountersInit(dev);
 
-	}
+	/*
+	 * Without modifying the state of the Battery Backup enab, disable
+	 * the watchdog timer, set DIO channels 0-5 to operate in the
+	 * standard DIO (vs. counter overflow) mode, disable the battery
+	 * charger, and reset the watchdog interval selector to zero.
+	 */
+	WriteMISC2(dev, (uint16_t)(DEBIread(dev, LP_RDMISC2) &
+				   MISC2_BATT_ENABLE));
 
-	dev->board_ptr = s626_boards;
-	dev->board_name = thisboard->name;
+	/* Initialize the digital I/O subsystem */
+	s626_dio_init(dev);
 
-	ret = comedi_alloc_subdevices(dev, 6);
+	/* enable interrupt test */
+	/* writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER); */
+}
+
+static int s626_attach_pci(struct comedi_device *dev, struct pci_dev *pcidev)
+{
+	struct comedi_subdevice *s;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	dev->board_name = dev->driver->driver_name;
+
+	if (alloc_private(dev, sizeof(struct s626_private)) < 0)
+		return -ENOMEM;
+
+	ret = comedi_pci_enable(pcidev, dev->board_name);
 	if (ret)
 		return ret;
+	dev->iobase = 1;	/* detach needs this */
 
-	dev->iobase = (unsigned long)devpriv->base_addr;
-	dev->irq = devpriv->pdev->irq;
+	devpriv->base_addr = ioremap(pci_resource_start(pcidev, 0),
+				     pci_resource_len(pcidev, 0));
+	if (!devpriv->base_addr)
+		return -ENOMEM;
 
-	/* set up interrupt handler */
-	if (dev->irq == 0) {
-		printk(KERN_ERR " unknown irq (bad)\n");
-	} else {
-		ret = request_irq(dev->irq, s626_irq_handler, IRQF_SHARED,
-				  "s626", dev);
+	/* disable master interrupt */
+	writel(0, devpriv->base_addr + P_IER);
 
-		if (ret < 0) {
-			printk(KERN_ERR " irq not available\n");
-			dev->irq = 0;
-		}
+	/* soft reset */
+	writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+
+	/* DMA FIXME DMA// */
+
+	ret = s626_allocate_dma_buffers(dev);
+	if (ret)
+		return ret;
+
+	if (pcidev->irq) {
+		ret = request_irq(pcidev->irq, s626_irq_handler, IRQF_SHARED,
+				  dev->board_name, dev);
+
+		if (ret == 0)
+			dev->irq = pcidev->irq;
 	}
 
+	ret = comedi_alloc_subdevices(dev, 6);
+	if (ret)
+		return ret;
+
 	s = dev->subdevices + 0;
 	/* analog input subdevice */
 	dev->read_subdev = s;
 	/* we support single-ended (ground) and differential */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = thisboard->ai_chans;
+	s->n_chan = S626_ADC_CHANNELS;
 	s->maxdata = (0xffff >> 2);
 	s->range_table = &s626_range_table;
-	s->len_chanlist = thisboard->ai_chans;	/* This is the maximum chanlist
-						   length that the board can
-						   handle */
+	s->len_chanlist = S626_ADC_CHANNELS;
 	s->insn_config = s626_ai_insn_config;
 	s->insn_read = s626_ai_insn_read;
 	s->do_cmd = s626_ai_cmd;
@@ -2626,7 +2730,7 @@ static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = thisboard->ao_chans;
+	s->n_chan = S626_DAC_CHANNELS;
 	s->maxdata = (0x3fff);
 	s->range_table = &range_bipolar10;
 	s->insn_write = s626_ao_winsn;
@@ -2672,7 +2776,7 @@ static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* encoder (counter) subdevice */
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = thisboard->enc_chans;
+	s->n_chan = S626_ENCODER_CHANNELS;
 	s->private = enc_private_data;
 	s->insn_config = s626_enc_insn_config;
 	s->insn_read = s626_enc_insn_read;
@@ -2680,269 +2784,17 @@ static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata = 0xffffff;
 	s->range_table = &range_unknown;
 
-	/* stop ai_command */
-	devpriv->ai_cmd_running = 0;
-
-	if (devpriv->base_addr && (devpriv->allocatedBuf == 2)) {
-		dma_addr_t pPhysBuf;
-		uint16_t chan;
-
-		/*  enab DEBI and audio pins, enable I2C interface. */
-		MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
-		/*  Configure DEBI operating mode. */
-		WR7146(P_DEBICFG, DEBI_CFG_SLAVE16	/*  Local bus is 16 */
-		       /*  bits wide. */
-		       | (DEBI_TOUT << DEBI_CFG_TOUT_BIT)
-
-		       /*  Declare DEBI */
-		       /*  transfer timeout */
-		       /*  interval. */
-		       |DEBI_SWAP	/*  Set up byte lane */
-		       /*  steering. */
-		       | DEBI_CFG_INTEL);	/*  Intel-compatible */
-		/*  local bus (DEBI */
-		/*  never times out). */
-
-		/* DEBI INIT S626 WR7146( P_DEBICFG, DEBI_CFG_INTEL | DEBI_CFG_TOQ */
-		/* | DEBI_CFG_INCQ| DEBI_CFG_16Q); //end */
-
-		/*  Paging is disabled. */
-		WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);	/*  Disable MMU paging. */
-
-		/*  Init GPIO so that ADC Start* is negated. */
-		WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
-
-		/* IsBoardRevA is a boolean that indicates whether the board is RevA.
-		 *
-		 * VERSION 2.01 CHANGE: REV A & B BOARDS NOW SUPPORTED BY DYNAMIC
-		 * EEPROM ADDRESS SELECTION.  Initialize the I2C interface, which
-		 * is used to access the onboard serial EEPROM.  The EEPROM's I2C
-		 * DeviceAddress is hardwired to a value that is dependent on the
-		 * 626 board revision.  On all board revisions, the EEPROM stores
-		 * TrimDAC calibration constants for analog I/O.  On RevB and
-		 * higher boards, the DeviceAddress is hardwired to 0 to enable
-		 * the EEPROM to also store the PCI SubVendorID and SubDeviceID;
-		 * this is the address at which the SAA7146 expects a
-		 * configuration EEPROM to reside.  On RevA boards, the EEPROM
-		 * device address, which is hardwired to 4, prevents the SAA7146
-		 * from retrieving PCI sub-IDs, so the SAA7146 uses its built-in
-		 * default values, instead.
-		 */
-
-		/*     devpriv->I2Cards= IsBoardRevA ? 0xA8 : 0xA0; // Set I2C EEPROM */
-		/*  DeviceType (0xA0) */
-		/*  and DeviceAddress<<1. */
-
-		devpriv->I2CAdrs = 0xA0;	/*  I2C device address for onboard */
-		/*  eeprom(revb) */
+	s626_initialize(dev);
 
-		/*  Issue an I2C ABORT command to halt any I2C operation in */
-		/* progress and reset BUSY flag. */
-		WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);
-		/*  Write I2C control: abort any I2C activity. */
-		MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-		/*  Invoke command  upload */
-		while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0)
-			;
-		/*  and wait for upload to complete. */
-
-		/* Per SAA7146 data sheet, write to STATUS reg twice to
-		 * reset all  I2C error flags. */
-		for (i = 0; i < 2; i++) {
-			WR7146(P_I2CSTAT, I2C_CLKSEL);
-			/*  Write I2C control: reset  error flags. */
-			MC_ENABLE(P_MC2, MC2_UPLD_IIC);	/*  Invoke command upload */
-			while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
-				;
-			/* and wait for upload to complete. */
-		}
-
-		/* Init audio interface functional attributes: set DAC/ADC
-		 * serial clock rates, invert DAC serial clock so that
-		 * DAC data setup times are satisfied, enable DAC serial
-		 * clock out.
-		 */
+	dev_info(dev->class_dev, "%s attached\n", dev->board_name);
 
-		WR7146(P_ACON2, ACON2_INIT);
-
-		/* Set up TSL1 slot list, which is used to control the
-		 * accumulation of ADC data: RSD1 = shift data in on SD1.
-		 * SIB_A1  = store data uint8_t at next available location in
-		 * FB BUFFER1  register. */
-		WR7146(P_TSL1, RSD1 | SIB_A1);
-		/*  Fetch ADC high data uint8_t. */
-		WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);
-		/*  Fetch ADC low data uint8_t; end of TSL1. */
-
-		/*  enab TSL1 slot list so that it executes all the time. */
-		WR7146(P_ACON1, ACON1_ADCSTART);
-
-		/*  Initialize RPS registers used for ADC. */
-
-		/* Physical start of RPS program. */
-		WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
-
-		WR7146(P_RPSPAGE1, 0);
-		/*  RPS program performs no explicit mem writes. */
-		WR7146(P_RPS1_TOUT, 0);	/*  Disable RPS timeouts. */
-
-		/* SAA7146 BUG WORKAROUND.  Initialize SAA7146 ADC interface
-		 * to a known state by invoking ADCs until FB BUFFER 1
-		 * register shows that it is correctly receiving ADC data.
-		 * This is necessary because the SAA7146 ADC interface does
-		 * not start up in a defined state after a PCI reset.
-		 */
-
-/*     PollList = EOPL;		// Create a simple polling */
-/*				// list for analog input */
-/*				// channel 0. */
-/*     ResetADC( dev, &PollList ); */
-
-/*     s626_ai_rinsn(dev,dev->subdevices,NULL,data); //( &AdcData ); // */
-/*							//Get initial ADC */
-/*							//value. */
-
-/*     StartVal = data[0]; */
-
-/*     // VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION. */
-/*     // Invoke ADCs until the new ADC value differs from the initial */
-/*     // value or a timeout occurs.  The timeout protects against the */
-/*     // possibility that the driver is restarting and the ADC data is a */
-/*     // fixed value resulting from the applied ADC analog input being */
-/*     // unusually quiet or at the rail. */
-
-/*     for ( index = 0; index < 500; index++ ) */
-/*       { */
-/*	s626_ai_rinsn(dev,dev->subdevices,NULL,data); */
-/*	AdcData = data[0];	//ReadADC(  &AdcData ); */
-/*	if ( AdcData != StartVal ) */
-/*		break; */
-/*       } */
-
-		/*  end initADC */
-
-		/*  init the DAC interface */
-
-		/* Init Audio2's output DMAC attributes: burst length = 1
-		 * DWORD,  threshold = 1 DWORD.
-		 */
-		WR7146(P_PCI_BT_A, 0);
-
-		/* Init Audio2's output DMA physical addresses.  The protection
-		 * address is set to 1 DWORD past the base address so that a
-		 * single DWORD will be transferred each time a DMA transfer is
-		 * enabled. */
-
-		pPhysBuf =
-		    devpriv->ANABuf.PhysicalBase +
-		    (DAC_WDMABUF_OS * sizeof(uint32_t));
-
-		WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);	/*  Buffer base adrs. */
-		WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));	/*  Protection address. */
-
-		/* Cache Audio2's output DMA buffer logical address.  This is
-		 * where DAC data is buffered for A2 output DMA transfers. */
-		devpriv->pDacWBuf =
-		    (uint32_t *) devpriv->ANABuf.LogicalBase + DAC_WDMABUF_OS;
-
-		/* Audio2's output channels does not use paging.  The protection
-		 * violation handling bit is set so that the DMAC will
-		 * automatically halt and its PCI address pointer will be reset
-		 * when the protection address is reached. */
-
-		WR7146(P_PAGEA2_OUT, 8);
-
-		/* Initialize time slot list 2 (TSL2), which is used to control
-		 * the clock generation for and serialization of data to be sent
-		 * to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
-		 * execution; this permits other slots to be safely modified
-		 * without first turning off the TSL sequencer (which is
-		 * apparently impossible to do).  Also, SD3 (which is driven by a
-		 * pull-up resistor) is shifted in and stored to the MSB of
-		 * FB_BUFFER2 to be used as evidence that the slot sequence has
-		 * not yet finished executing.
-		 */
-
-		SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);
-		/*  Slot 0: Trap TSL execution, shift 0xFF into FB_BUFFER2. */
-
-		/* Initialize slot 1, which is constant.  Slot 1 causes a
-		 * DWORD to be transferred from audio channel 2's output FIFO
-		 * to the FIFO's output buffer so that it can be serialized
-		 * and sent to the DAC during subsequent slots.  All remaining
-		 * slots are dynamically populated as required by the target
-		 * DAC device.
-		 */
-		SETVECT(1, LF_A2);
-		/*  Slot 1: Fetch DWORD from Audio2's output FIFO. */
-
-		/*  Start DAC's audio interface (TSL2) running. */
-		WR7146(P_ACON1, ACON1_DACSTART);
-
-		/* end init DAC interface */
-
-		/* Init Trim DACs to calibrated values.  Do it twice because the
-		 * SAA7146 audio channel does not always reset properly and
-		 * sometimes causes the first few TrimDAC writes to malfunction.
-		 */
-
-		LoadTrimDACs(dev);
-		LoadTrimDACs(dev);	/*  Insurance. */
-
-		/* Manually init all gate array hardware in case this is a soft
-		 * reset (we have no way of determining whether this is a warm
-		 * or cold start).  This is necessary because the gate array will
-		 * reset only in response to a PCI hard reset; there is no soft
-		 * reset function. */
-
-		/* Init all DAC outputs to 0V and init all DAC setpoint and
-		 * polarity images.
-		 */
-		for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
-			SetDAC(dev, chan, 0);
-
-		/* Init image of WRMISC2 Battery Charger Enabled control bit.
-		 * This image is used when the state of the charger control bit,
-		 * which has no direct hardware readback mechanism, is queried.
-		 */
-		devpriv->ChargeEnabled = 0;
-
-		/* Init image of watchdog timer interval in WRMISC2.  This image
-		 * maintains the value of the control bits of MISC2 are
-		 * continuously reset to zero as long as the WD timer is disabled.
-		 */
-		devpriv->WDInterval = 0;
-
-		/* Init Counter Interrupt enab mask for RDMISC2.  This mask is
-		 * applied against MISC2 when testing to determine which timer
-		 * events are requesting interrupt service.
-		 */
-		devpriv->CounterIntEnabs = 0;
-
-		/*  Init counters. */
-		CountersInit(dev);
-
-		/* Without modifying the state of the Battery Backup enab, disable
-		 * the watchdog timer, set DIO channels 0-5 to operate in the
-		 * standard DIO (vs. counter overflow) mode, disable the battery
-		 * charger, and reset the watchdog interval selector to zero.
-		 */
-		WriteMISC2(dev, (uint16_t) (DEBIread(dev,
-						     LP_RDMISC2) &
-					    MISC2_BATT_ENABLE));
-
-		/*  Initialize the digital I/O subsystem. */
-		s626_dio_init(dev);
-
-		/* enable interrupt test */
-		/*  writel(IRQ_GPIO3 | IRQ_RPS1,devpriv->base_addr+P_IER); */
-	}
-
-	return 1;
+	return 0;
 }
 
 static void s626_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (devpriv) {
 		/* stop ai_command */
 		devpriv->ai_cmd_running = 0;
@@ -2967,18 +2819,17 @@ static void s626_detach(struct comedi_device *dev)
 			free_irq(dev->irq, dev);
 		if (devpriv->base_addr)
 			iounmap(devpriv->base_addr);
-		if (devpriv->pdev) {
-			if (devpriv->got_regions)
-				comedi_pci_disable(devpriv->pdev);
-			pci_dev_put(devpriv->pdev);
-		}
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
 	}
 }
 
 static struct comedi_driver s626_driver = {
 	.driver_name	= "s626",
 	.module		= THIS_MODULE,
-	.attach		= s626_attach,
+	.attach_pci	= s626_attach_pci,
 	.detach		= s626_detach,
 };
 
diff --git a/drivers/staging/comedi/drivers/s626.h b/drivers/staging/comedi/drivers/s626.h
index 8a8f196cf153..ff4b3a5e4e5f 100644
--- a/drivers/staging/comedi/drivers/s626.h
+++ b/drivers/staging/comedi/drivers/s626.h
@@ -73,7 +73,6 @@
 #include <linux/slab.h>
 
 #define S626_SIZE 0x0200
-#define SIZEOF_ADDRESS_SPACE		0x0200
 #define DMABUF_SIZE			4096	/*  4k pages */
 
 #define S626_ADC_CHANNELS       16
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index c18314be8c82..5bf84cfbdceb 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -588,7 +588,9 @@ static int serial_2002_open(struct comedi_device *dev)
 				kfree(s->range_table_list);
 				s->range_table = NULL;
 				s->range_table_list = NULL;
-				if (range) {
+				if (kind == 1 || kind == 2) {
+					s->range_table = &range_digital;
+				} else if (range) {
 					s->range_table_list = range_table_list =
 					    kmalloc(sizeof
 						    (struct
@@ -798,7 +800,7 @@ static int serial2002_attach(struct comedi_device *dev,
 		return ret;
 
 	/* digital input subdevice */
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 0;
@@ -807,7 +809,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->insn_read = &serial2002_di_rinsn;
 
 	/* digital output subdevice */
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITEABLE;
 	s->n_chan = 0;
@@ -816,7 +818,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->insn_write = &serial2002_do_winsn;
 
 	/* analog input subdevice */
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = 0;
@@ -825,7 +827,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->insn_read = &serial2002_ai_rinsn;
 
 	/* analog output subdevice */
-	s = dev->subdevices + 3;
+	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITEABLE;
 	s->n_chan = 0;
@@ -835,7 +837,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->insn_read = &serial2002_ao_rinsn;
 
 	/* encoder input subdevice */
-	s = dev->subdevices + 4;
+	s = &dev->subdevices[4];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
 	s->n_chan = 0;
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index 9a68eebefca0..b70cdf300bbd 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -76,6 +76,8 @@ Configuration Options:
 
 #include <linux/pci.h>		/* for PCI devices */
 
+#include "comedi_fc.h"
+
 /* Imaginary registers for the imaginary board */
 
 #define SKEL_SIZE 0
@@ -238,7 +240,7 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* dev->read_subdev=s; */
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
@@ -256,7 +258,7 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 */
 	s->do_cmdtest = skel_ai_cmdtest;
 
-	s = dev->subdevices + 1;
+	s = &dev->subdevices[1];
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -266,7 +268,7 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_write = skel_ao_winsn;
 	s->insn_read = skel_ao_rinsn;
 
-	s = dev->subdevices + 2;
+	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 	if (thisboard->have_dio) {
 		s->type = COMEDI_SUBD_DIO;
@@ -349,60 +351,40 @@ static int skel_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return n;
 }
 
+/*
+ * cmdtest tests a particular command to see if it is valid.
+ * Using the cmdtest ioctl, a user can create a valid cmd
+ * and then have it executes by the cmd ioctl.
+ *
+ * cmdtest returns 1,2,3,4 or 0, depending on which tests
+ * the command passes.
+ */
 static int skel_ai_cmdtest(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
+			   struct comedi_subdevice *s,
+			   struct comedi_cmd *cmd)
 {
 	int err = 0;
 	int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
-	/* step 1: make sure trigger sources are trivially valid */
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible
-     */
+	/* Step 2a : make sure trigger sources are unique */
 
-	/* note that mutual compatibility is not an issue here */
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c
index 84b9f2a4280b..ae3aa1c5caef 100644
--- a/drivers/staging/comedi/drivers/ssv_dnp.c
+++ b/drivers/staging/comedi/drivers/ssv_dnp.c
@@ -177,15 +177,13 @@ static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int ret;
 
-	printk(KERN_INFO "comedi%d: dnp: ", dev->minor);
-
 	dev->board_name = board->name;
 
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret)
 		return ret;
 
-	s = dev->subdevices + 0;
+	s = &dev->subdevices[0];
 	/* digital i/o subdevice                                             */
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
@@ -195,8 +193,6 @@ static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_bits = dnp_dio_insn_bits;
 	s->insn_config = dnp_dio_insn_config;
 
-	printk("attached\n");
-
 	/* We use the I/O ports 0x22,0x23 and 0xa3-0xa9, which are always
 	 * allocated for the primary 8259, so we don't need to allocate them
 	 * ourselves. */
@@ -209,6 +205,7 @@ static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	outb(PCMR, CSCIR);
 	outb((inb(CSCDR) & 0xAA), CSCDR);
 
+	dev_info(dev->class_dev, "%s: attached\n", dev->board_name);
 	return 1;
 }
 
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 11ee83681da7..b536bba74351 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -98,6 +98,8 @@ sampling rate. If you sample two channels you get 4kHz and so on.
 
 #include "../comedidev.h"
 
+#include "comedi_fc.h"
+
 /* timeout for the USB-transfer in ms*/
 #define BULK_TIMEOUT 1000
 
@@ -404,7 +406,7 @@ static void usbduxsub_ai_IsocIrq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 	/* subdevice which is the AD converter */
-	s = this_comedidev->subdevices + SUBDEV_AD;
+	s = &this_comedidev->subdevices[SUBDEV_AD];
 
 	/* first we test if something unusual has just happened */
 	switch (urb->status) {
@@ -604,7 +606,7 @@ static void usbduxsub_ao_IsocIrq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 
-	s = this_comedidev->subdevices + SUBDEV_DA;
+	s = &this_comedidev->subdevices[SUBDEV_DA];
 
 	switch (urb->status) {
 	case 0:
@@ -929,9 +931,9 @@ static int usbduxsub_submit_OutURBs(struct usbduxsub *usbduxsub)
 static int usbdux_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	int err = 0, tmp, i;
-	unsigned int tmpTimer;
 	struct usbduxsub *this_usbduxsub = dev->private;
+	int err = 0, i;
+	unsigned int tmpTimer;
 
 	if (!(this_usbduxsub->probed))
 		return -ENODEV;
@@ -939,51 +941,23 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 	dev_dbg(&this_usbduxsub->interface->dev,
 		"comedi%d: usbdux_ai_cmdtest\n", dev->minor);
 
-	/* make sure triggers are valid */
-	/* Only immediate triggers are allowed */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	/* trigger should happen timed */
-	tmp = cmd->scan_begin_src;
-	/* start a new _scan_ with a timer */
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	/* scanning is continuous */
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	/* issue a trigger when scan is finished and start a new scan */
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	/* trigger at the end of count events or not, stop condition or not */
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually compatible
-	 * note that mutual compatibility is not an issue here
-	 */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1488,8 +1462,9 @@ static int usbdux_ao_inttrig(struct comedi_device *dev,
 static int usbdux_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	int err = 0, tmp;
 	struct usbduxsub *this_usbduxsub = dev->private;
+	int err = 0;
+	unsigned int flags;
 
 	if (!this_usbduxsub)
 		return -EFAULT;
@@ -1500,69 +1475,46 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 	dev_dbg(&this_usbduxsub->interface->dev,
 		"comedi%d: usbdux_ao_cmdtest\n", dev->minor);
 
-	/* make sure triggers are valid */
-	/* Only immediate triggers are allowed */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
-	/* trigger should happen timed */
-	tmp = cmd->scan_begin_src;
-	/* just now we scan also in the high speed mode every frame */
-	/* this is due to ehci driver limitations */
 	if (0) {		/* (this_usbduxsub->high_speed) */
-		/* start immediately a new scan */
 		/* the sampling rate is set by the coversion rate */
-		cmd->scan_begin_src &= TRIG_FOLLOW;
+		flags = TRIG_FOLLOW;
 	} else {
 		/* start a new scan (output at once) with a timer */
-		cmd->scan_begin_src &= TRIG_TIMER;
+		flags = TRIG_TIMER;
 	}
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
 
-	/* scanning is continuous */
-	tmp = cmd->convert_src;
-	/* we always output at 1kHz just now all channels at once */
 	if (0) {		/* (this_usbduxsub->high_speed) */
 		/*
-		 * in usb-2.0 only one conversion it transmitted but with 8kHz/n
+		 * in usb-2.0 only one conversion it transmitted
+		 * but with 8kHz/n
 		 */
-		cmd->convert_src &= TRIG_TIMER;
+		flags = TRIG_TIMER;
 	} else {
-		/* all conversion events happen simultaneously with a rate of
-		 * 1kHz/n */
-		cmd->convert_src &= TRIG_NOW;
+		/*
+		 * all conversion events happen simultaneously with
+		 * a rate of 1kHz/n
+		 */
+		flags = TRIG_NOW;
 	}
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	/* issue a trigger when scan is finished and start a new scan */
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
 
-	/* trigger at the end of count events or not, stop condition or not */
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually compatible
-	 * note that mutual compatibility is not an issue here
-	 */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1947,7 +1899,7 @@ static void usbduxsub_pwm_irq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 
-	s = this_comedidev->subdevices + SUBDEV_DA;
+	s = &this_comedidev->subdevices[SUBDEV_DA];
 
 	switch (urb->status) {
 	case 0:
@@ -2293,10 +2245,8 @@ static void tidy_up(struct usbduxsub *usbduxsub_tmp)
 	usbduxsub_tmp->pwm_cmd_running = 0;
 }
 
-/* common part of attach and attach_usb */
 static int usbdux_attach_common(struct comedi_device *dev,
-				struct usbduxsub *udev,
-				void *aux_data, int aux_len)
+				struct usbduxsub *udev)
 {
 	int ret;
 	struct comedi_subdevice *s = NULL;
@@ -2306,10 +2256,6 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	/* pointer back to the corresponding comedi device */
 	udev->comedidev = dev;
 
-	/* trying to upload the firmware into the chip */
-	if (aux_data)
-		firmwareUpload(udev, aux_data, aux_len);
-
 	dev->board_name = "usbdux";
 
 	/* set number of subdevices */
@@ -2331,7 +2277,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	dev->private = udev;
 
 	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
+	s = &dev->subdevices[SUBDEV_AD];
 	/* the URBs get the comedi subdevice */
 	/* which is responsible for reading */
 	/* this is the subdevice which reads data */
@@ -2358,7 +2304,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	s->range_table = (&range_usbdux_ai_range);
 
 	/* analog out */
-	s = dev->subdevices + SUBDEV_DA;
+	s = &dev->subdevices[SUBDEV_DA];
 	/* analog out */
 	s->type = COMEDI_SUBD_AO;
 	/* backward pointer */
@@ -2384,7 +2330,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	s->insn_write = usbdux_ao_insn_write;
 
 	/* digital I/O */
-	s = dev->subdevices + SUBDEV_DIO;
+	s = &dev->subdevices[SUBDEV_DIO];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 8;
@@ -2396,7 +2342,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	s->private = NULL;
 
 	/* counter */
-	s = dev->subdevices + SUBDEV_COUNTER;
+	s = &dev->subdevices[SUBDEV_COUNTER];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 	s->n_chan = 4;
@@ -2407,7 +2353,7 @@ static int usbdux_attach_common(struct comedi_device *dev,
 
 	if (udev->high_speed) {
 		/* timer / pwm */
-		s = dev->subdevices + SUBDEV_PWM;
+		s = &dev->subdevices[SUBDEV_PWM];
 		s->type = COMEDI_SUBD_PWM;
 		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
 		s->n_chan = 8;
@@ -2429,48 +2375,6 @@ static int usbdux_attach_common(struct comedi_device *dev,
 	return 0;
 }
 
-/* is called when comedi-config is called */
-static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	void *aux_data;
-	int aux_len;
-
-	dev->private = NULL;
-
-	aux_data = comedi_aux_data(it->options, 0);
-	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-	if (aux_data == NULL)
-		aux_len = 0;
-	else if (aux_len == 0)
-		aux_data = NULL;
-
-	down(&start_stop_sem);
-	/* find a valid device which has been detected by the probe function of
-	 * the usb */
-	index = -1;
-	for (i = 0; i < NUMUSBDUX; i++) {
-		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
-			index = i;
-			break;
-		}
-	}
-
-	if (index < 0) {
-		printk(KERN_ERR
-		       "comedi%d: usbdux: error: attach failed, no usbdux devs connected to the usb bus.\n",
-		       dev->minor);
-		ret = -ENODEV;
-	} else
-		ret = usbdux_attach_common(dev, &usbduxsub[index],
-					   aux_data, aux_len);
-	up(&start_stop_sem);
-	return ret;
-}
-
-/* is called from comedi_usb_auto_config() */
 static int usbdux_attach_usb(struct comedi_device *dev,
 			     struct usb_interface *uinterf)
 {
@@ -2492,7 +2396,7 @@ static int usbdux_attach_usb(struct comedi_device *dev,
 		       dev->minor);
 		ret = -ENODEV;
 	} else
-		ret = usbdux_attach_common(dev, this_usbduxsub, NULL, 0);
+		ret = usbdux_attach_common(dev, this_usbduxsub);
 	up(&start_stop_sem);
 	return ret;
 }
@@ -2513,9 +2417,8 @@ static void usbdux_detach(struct comedi_device *dev)
 static struct comedi_driver usbdux_driver = {
 	.driver_name	= "usbdux",
 	.module		= THIS_MODULE,
-	.attach		= usbdux_attach,
-	.detach		= usbdux_detach,
 	.attach_usb	= usbdux_attach_usb,
+	.detach		= usbdux_detach,
 };
 
 static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index 8eb41257c6ce..1154a7e2895d 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -346,7 +346,7 @@ static void usbduxfastsub_ai_Irq(struct urb *urb)
 		return;
 	}
 	/* subdevice which is the AD converter */
-	s = this_comedidev->subdevices + SUBDEV_AD;
+	s = &this_comedidev->subdevices[SUBDEV_AD];
 
 	/* first we test if something unusual has just happened */
 	switch (urb->status) {
@@ -549,10 +549,10 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
 {
-	int err = 0, stop_mask = 0;
+	struct usbduxfastsub_s *udfs = dev->private;
+	int err = 0;
 	long int steps, tmp;
 	int minSamplPer;
-	struct usbduxfastsub_s *udfs = dev->private;
 
 	if (!udfs->probed)
 		return -ENODEV;
@@ -563,57 +563,31 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 	       "scan_begin_arg=%u\n",
 	       dev->minor, cmd->convert_arg, cmd->scan_begin_arg);
 #endif
-	/* step 1: make sure trigger sources are trivially valid */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_EXT | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	stop_mask = TRIG_COUNT | TRIG_NONE;
-	cmd->stop_src &= stop_mask;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	err |= cfc_check_trigger_src(&cmd->start_src,
+					TRIG_NOW | TRIG_EXT | TRIG_INT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually compatible
-	 */
+	/* Step 2a : make sure trigger sources are unique */
 
-	if (cmd->start_src != TRIG_NOW &&
-	    cmd->start_src != TRIG_EXT && cmd->start_src != TRIG_INT)
-		err++;
-	if (cmd->scan_begin_src != TRIG_TIMER &&
-	    cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT)
-		err++;
-	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_EXT && cmd->stop_src != TRIG_NONE)
-		err++;
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
 
 	/* can't have external stop and start triggers at once */
 	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
-		err++;
+		err |= -EINVAL;
 
 	if (err)
 		return 2;
@@ -1430,10 +1404,8 @@ static void tidy_up(struct usbduxfastsub_s *udfs)
 	udfs->ai_cmd_running = 0;
 }
 
-/* common part of attach and attach_usb */
 static int usbduxfast_attach_common(struct comedi_device *dev,
-				    struct usbduxfastsub_s *udfs,
-				    void *aux_data, int aux_len)
+				    struct usbduxfastsub_s *udfs)
 {
 	int ret;
 	struct comedi_subdevice *s;
@@ -1441,9 +1413,6 @@ static int usbduxfast_attach_common(struct comedi_device *dev,
 	down(&udfs->sem);
 	/* pointer back to the corresponding comedi device */
 	udfs->comedidev = dev;
-	/* trying to upload the firmware into the chip */
-	if (aux_data)
-		firmwareUpload(udfs, aux_data, aux_len);
 	dev->board_name = "usbduxfast";
 	ret = comedi_alloc_subdevices(dev, 1);
 	if (ret) {
@@ -1453,7 +1422,7 @@ static int usbduxfast_attach_common(struct comedi_device *dev,
 	/* private structure is also simply the usb-structure */
 	dev->private = udfs;
 	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
+	s = &dev->subdevices[SUBDEV_AD];
 	/*
 	 * the URBs get the comedi subdevice which is responsible for reading
 	 * this is the subdevice which reads data
@@ -1485,48 +1454,6 @@ static int usbduxfast_attach_common(struct comedi_device *dev,
 	return 0;
 }
 
-/* is called for COMEDI_DEVCONFIG ioctl (when comedi_config is run) */
-static int usbduxfast_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	void *aux_data;
-	int aux_len;
-
-	dev->private = NULL;
-
-	aux_data = comedi_aux_data(it->options, 0);
-	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-	if (aux_data == NULL)
-		aux_len = 0;
-	else if (aux_len == 0)
-		aux_data = NULL;
-	down(&start_stop_sem);
-	/*
-	 * find a valid device which has been detected by the
-	 * probe function of the usb
-	 */
-	index = -1;
-	for (i = 0; i < NUMUSBDUXFAST; i++) {
-		if (usbduxfastsub[i].probed && !usbduxfastsub[i].attached) {
-			index = i;
-			break;
-		}
-	}
-	if (index < 0) {
-		dev_err(dev->class_dev,
-			"usbduxfast: error: attach failed, no usbduxfast devs connected to the usb bus.\n");
-		ret = -ENODEV;
-	} else
-		ret = usbduxfast_attach_common(dev, &usbduxfastsub[index],
-					       aux_data, aux_len);
-	up(&start_stop_sem);
-	return ret;
-}
-
-/* is called from comedi_usb_auto_config() */
 static int usbduxfast_attach_usb(struct comedi_device *dev,
 				 struct usb_interface *uinterf)
 {
@@ -1545,7 +1472,7 @@ static int usbduxfast_attach_usb(struct comedi_device *dev,
 		       "usbduxfast: error: attach_usb failed, already attached\n");
 		ret = -ENODEV;
 	} else
-		ret = usbduxfast_attach_common(dev, udfs, NULL, 0);
+		ret = usbduxfast_attach_common(dev, udfs);
 	up(&start_stop_sem);
 	return ret;
 }
@@ -1568,9 +1495,8 @@ static void usbduxfast_detach(struct comedi_device *dev)
 static struct comedi_driver usbduxfast_driver = {
 	.driver_name	= "usbduxfast",
 	.module		= THIS_MODULE,
-	.attach		= usbduxfast_attach,
-	.detach		= usbduxfast_detach,
 	.attach_usb	= usbduxfast_attach_usb,
+	.detach		= usbduxfast_detach,
 };
 
 static void usbduxfast_firmware_request_complete_handler(const struct firmware
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index f54ab8c2fcfd..b1694121f845 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -356,7 +356,7 @@ static void usbduxsub_ai_IsocIrq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 	/* subdevice which is the AD converter */
-	s = this_comedidev->subdevices + SUBDEV_AD;
+	s = &this_comedidev->subdevices[SUBDEV_AD];
 
 	/* first we test if something unusual has just happened */
 	switch (urb->status) {
@@ -558,7 +558,7 @@ static void usbduxsub_ao_IsocIrq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 
-	s = this_comedidev->subdevices + SUBDEV_DA;
+	s = &this_comedidev->subdevices[SUBDEV_DA];
 
 	switch (urb->status) {
 	case 0:
@@ -897,9 +897,9 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd)
 {
-	int err = 0, tmp, i;
-	unsigned int tmpTimer;
 	struct usbduxsub *this_usbduxsub = dev->private;
+	int err = 0, i;
+	unsigned int tmpTimer;
 
 	if (!(this_usbduxsub->probed))
 		return -ENODEV;
@@ -907,51 +907,23 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 	dev_dbg(&this_usbduxsub->interface->dev,
 		"comedi%d: usbdux_ai_cmdtest\n", dev->minor);
 
-	/* make sure triggers are valid */
-	/* Only immediate triggers are allowed */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	/* trigger should happen timed */
-	tmp = cmd->scan_begin_src;
-	/* start a new _scan_ with a timer */
-	cmd->scan_begin_src &= TRIG_TIMER;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	/* scanning is continuous */
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_NOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
 
-	/* issue a trigger when scan is finished and start a new scan */
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	/* trigger at the end of count events or not, stop condition or not */
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources are unique and mutually compatible
-	 * note that mutual compatibility is not an issue here
-	 */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1558,8 +1530,9 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd)
 {
-	int err = 0, tmp;
 	struct usbduxsub *this_usbduxsub = dev->private;
+	int err = 0;
+	unsigned int flags;
 
 	if (!this_usbduxsub)
 		return -EFAULT;
@@ -1570,63 +1543,35 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 	dev_dbg(&this_usbduxsub->interface->dev,
 		"comedi%d: usbdux_ao_cmdtest\n", dev->minor);
 
-	/* make sure triggers are valid */
-	/* Only immediate triggers are allowed */
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW | TRIG_INT;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
-	/* trigger should happen timed */
-	tmp = cmd->scan_begin_src;
-	/* just now we scan also in the high speed mode every frame */
-	/* this is due to ehci driver limitations */
 	if (0) {		/* (this_usbduxsub->high_speed) */
-		/* start immediately a new scan */
-		/* the sampling rate is set by the coversion rate */
-		cmd->scan_begin_src &= TRIG_FOLLOW;
+		/*
+		 * start immediately a new scan
+		 * the sampling rate is set by the coversion rate
+		 */
+		flags = TRIG_FOLLOW;
 	} else {
 		/* start a new scan (output at once) with a timer */
-		cmd->scan_begin_src &= TRIG_TIMER;
+		flags = TRIG_TIMER;
 	}
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	/* scanning is continuous */
-	tmp = cmd->convert_src;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
 
-	/* all conversion events happen simultaneously */
-	cmd->convert_src &= TRIG_NOW;
-
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	/* issue a trigger when scan is finished and start a new scan */
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	/* trigger at the end of count events or not, stop condition or not */
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
+	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);
 
 	if (err)
 		return 1;
 
-	/*
-	 * step 2: make sure trigger sources
-	 * are unique and mutually compatible
-	 * note that mutual compatibility is not an issue here
-	 */
-	if (cmd->scan_begin_src != TRIG_FOLLOW &&
-	    cmd->scan_begin_src != TRIG_EXT &&
-	    cmd->scan_begin_src != TRIG_TIMER)
-		err++;
-	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
-		err++;
+	/* 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);
+
+	/* Step 2b : and mutually compatible */
 
 	if (err)
 		return 2;
@@ -1950,7 +1895,7 @@ static void usbduxsub_pwm_irq(struct urb *urb)
 	/* the private structure of the subdevice is struct usbduxsub */
 	this_usbduxsub = this_comedidev->private;
 
-	s = this_comedidev->subdevices + SUBDEV_DA;
+	s = &this_comedidev->subdevices[SUBDEV_DA];
 
 	switch (urb->status) {
 	case 0:
@@ -2301,10 +2246,8 @@ static void tidy_up(struct usbduxsub *usbduxsub_tmp)
 	usbduxsub_tmp->pwm_cmd_running = 0;
 }
 
-/* common part of attach and attach_usb */
 static int usbduxsigma_attach_common(struct comedi_device *dev,
-				     struct usbduxsub *uds,
-				     void *aux_data, int aux_len)
+				     struct usbduxsub *uds)
 {
 	int ret;
 	struct comedi_subdevice *s;
@@ -2314,9 +2257,6 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	down(&uds->sem);
 	/* pointer back to the corresponding comedi device */
 	uds->comedidev = dev;
-	/* trying to upload the firmware into the FX2 */
-	if (aux_data)
-		firmwareUpload(uds, aux_data, aux_len);
 	dev->board_name = "usbduxsigma";
 	/* set number of subdevices */
 	if (uds->high_speed)
@@ -2331,7 +2271,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	/* private structure is also simply the usb-structure */
 	dev->private = uds;
 	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
+	s = &dev->subdevices[SUBDEV_AD];
 	/* the URBs get the comedi subdevice */
 	/* which is responsible for reading */
 	/* this is the subdevice which reads data */
@@ -2358,7 +2298,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	/* range table to convert to physical units */
 	s->range_table = (&range_usbdux_ai_range);
 	/* analog output subdevice */
-	s = dev->subdevices + SUBDEV_DA;
+	s = &dev->subdevices[SUBDEV_DA];
 	/* analog out */
 	s->type = COMEDI_SUBD_AO;
 	/* backward pointer */
@@ -2383,7 +2323,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	s->insn_read = usbdux_ao_insn_read;
 	s->insn_write = usbdux_ao_insn_write;
 	/* digital I/O subdevice */
-	s = dev->subdevices + SUBDEV_DIO;
+	s = &dev->subdevices[SUBDEV_DIO];
 	s->type = COMEDI_SUBD_DIO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	/* 8 external and 16 internal channels */
@@ -2396,7 +2336,7 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	s->private = NULL;
 	if (uds->high_speed) {
 		/* timer / pwm subdevice */
-		s = dev->subdevices + SUBDEV_PWM;
+		s = &dev->subdevices[SUBDEV_PWM];
 		s->type = COMEDI_SUBD_PWM;
 		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
 		s->n_chan = 8;
@@ -2419,47 +2359,6 @@ static int usbduxsigma_attach_common(struct comedi_device *dev,
 	return 0;
 }
 
-/* is called for COMEDI_DEVCONFIG ioctl (when comedi_config is run) */
-static int usbduxsigma_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	void *aux_data;
-	int aux_len;
-
-	dev->private = NULL;
-
-	aux_data = comedi_aux_data(it->options, 0);
-	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-	if (aux_data == NULL)
-		aux_len = 0;
-	else if (aux_len == 0)
-		aux_data = NULL;
-
-	down(&start_stop_sem);
-	/* find a valid device which has been detected by the probe function of
-	 * the usb */
-	index = -1;
-	for (i = 0; i < NUMUSBDUX; i++) {
-		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
-			index = i;
-			break;
-		}
-	}
-	if (index < 0) {
-		dev_err(dev->class_dev,
-			"usbduxsigma: error: attach failed, dev not connected to the usb bus.\n");
-		ret = -ENODEV;
-	} else
-		ret = usbduxsigma_attach_common(dev, &usbduxsub[index],
-						aux_data, aux_len);
-	up(&start_stop_sem);
-	return ret;
-}
-
-/* is called from comedi_usb_auto_config() */
 static int usbduxsigma_attach_usb(struct comedi_device *dev,
 				  struct usb_interface *uinterf)
 {
@@ -2478,7 +2377,7 @@ static int usbduxsigma_attach_usb(struct comedi_device *dev,
 		       "usbduxsigma: error: attach_usb failed, already attached\n");
 		ret = -ENODEV;
 	} else
-		ret = usbduxsigma_attach_common(dev, uds, NULL, 0);
+		ret = usbduxsigma_attach_common(dev, uds);
 	up(&start_stop_sem);
 	return ret;
 }
@@ -2499,9 +2398,8 @@ static void usbduxsigma_detach(struct comedi_device *dev)
 static struct comedi_driver usbduxsigma_driver = {
 	.driver_name	= "usbduxsigma",
 	.module		= THIS_MODULE,
-	.attach		= usbduxsigma_attach,
-	.detach		= usbduxsigma_detach,
 	.attach_usb	= usbduxsigma_attach_usb,
+	.detach		= usbduxsigma_detach,
 };
 
 static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index 94010fc05905..df277aa591bb 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -546,6 +546,7 @@ static int vmk80xx_ai_rinsn(struct comedi_device *cdev,
 			reg[0] = VMK8055_AI2_REG;
 		break;
 	case VMK8061_MODEL:
+	default:
 		reg[0] = VMK8061_AI_REG1;
 		reg[1] = VMK8061_AI_REG2;
 		dev->usb_tx_buf[0] = VMK8061_CMD_RD_AI;
@@ -904,6 +905,7 @@ static int vmk80xx_cnt_rinsn(struct comedi_device *cdev,
 			reg[0] = VMK8055_CNT2_REG;
 		break;
 	case VMK8061_MODEL:
+	default:
 		reg[0] = VMK8061_CNT_REG;
 		reg[1] = VMK8061_CNT_REG;
 		dev->usb_tx_buf[0] = VMK8061_CMD_RD_CNT;
@@ -1119,7 +1121,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 		return ret;
 	}
 	/* Analog input subdevice */
-	s = cdev->subdevices + VMK80XX_SUBD_AI;
+	s = &cdev->subdevices[VMK80XX_SUBD_AI];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = dev->board.ai_chans;
@@ -1127,7 +1129,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 	s->range_table = dev->board.range;
 	s->insn_read = vmk80xx_ai_rinsn;
 	/* Analog output subdevice */
-	s = cdev->subdevices + VMK80XX_SUBD_AO;
+	s = &cdev->subdevices[VMK80XX_SUBD_AO];
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
 	s->n_chan = dev->board.ao_chans;
@@ -1139,7 +1141,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 		s->insn_read = vmk80xx_ao_rinsn;
 	}
 	/* Digital input subdevice */
-	s = cdev->subdevices + VMK80XX_SUBD_DI;
+	s = &cdev->subdevices[VMK80XX_SUBD_DI];
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = dev->board.di_chans;
@@ -1147,7 +1149,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 	s->insn_read = vmk80xx_di_rinsn;
 	s->insn_bits = vmk80xx_di_bits;
 	/* Digital output subdevice */
-	s = cdev->subdevices + VMK80XX_SUBD_DO;
+	s = &cdev->subdevices[VMK80XX_SUBD_DO];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
 	s->n_chan = dev->board.do_chans;
@@ -1159,7 +1161,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 		s->insn_read = vmk80xx_do_rinsn;
 	}
 	/* Counter subdevice */
-	s = cdev->subdevices + VMK80XX_SUBD_CNT;
+	s = &cdev->subdevices[VMK80XX_SUBD_CNT];
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE;
 	s->n_chan = dev->board.cnt_chans;
@@ -1172,7 +1174,7 @@ static int vmk80xx_attach_common(struct comedi_device *cdev,
 	}
 	/* PWM subdevice */
 	if (dev->board.model == VMK8061_MODEL) {
-		s = cdev->subdevices + VMK80XX_SUBD_PWM;
+		s = &cdev->subdevices[VMK80XX_SUBD_PWM];
 		s->type = COMEDI_SUBD_PWM;
 		s->subdev_flags = SDF_READABLE | SDF_WRITEABLE;
 		s->n_chan = dev->board.pwm_chans;
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
index 0252b4408851..3f20ea55b8d0 100644
--- a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -80,7 +80,9 @@ int comedi_close(struct comedi_device *d)
 }
 EXPORT_SYMBOL(comedi_close);
 
-static int comedi_do_insn(struct comedi_device *dev, struct comedi_insn *insn)
+static int comedi_do_insn(struct comedi_device *dev,
+			  struct comedi_insn *insn,
+			  unsigned int *data)
 {
 	struct comedi_subdevice *s;
 	int ret = 0;
@@ -90,7 +92,7 @@ static int comedi_do_insn(struct comedi_device *dev, struct comedi_insn *insn)
 		ret = -EINVAL;
 		goto error;
 	}
-	s = dev->subdevices + insn->subdev;
+	s = &dev->subdevices[insn->subdev];
 
 	if (s->type == COMEDI_SUBD_UNUSED) {
 		printk(KERN_ERR "%d not useable subdevice\n", insn->subdev);
@@ -115,11 +117,11 @@ static int comedi_do_insn(struct comedi_device *dev, struct comedi_insn *insn)
 
 	switch (insn->insn) {
 	case INSN_BITS:
-		ret = s->insn_bits(dev, s, insn, insn->data);
+		ret = s->insn_bits(dev, s, insn, data);
 		break;
 	case INSN_CONFIG:
 		/* XXX should check instruction length */
-		ret = s->insn_config(dev, s, insn, insn->data);
+		ret = s->insn_config(dev, s, insn, data);
 		break;
 	default:
 		ret = -EINVAL;
@@ -140,11 +142,10 @@ int comedi_dio_config(struct comedi_device *dev, unsigned int subdev,
 	memset(&insn, 0, sizeof(insn));
 	insn.insn = INSN_CONFIG;
 	insn.n = 1;
-	insn.data = &io;
 	insn.subdev = subdev;
 	insn.chanspec = CR_PACK(chan, 0, 0);
 
-	return comedi_do_insn(dev, &insn);
+	return comedi_do_insn(dev, &insn, &io);
 }
 EXPORT_SYMBOL(comedi_dio_config);
 
@@ -158,13 +159,12 @@ int comedi_dio_bitfield(struct comedi_device *dev, unsigned int subdev,
 	memset(&insn, 0, sizeof(insn));
 	insn.insn = INSN_BITS;
 	insn.n = 2;
-	insn.data = data;
 	insn.subdev = subdev;
 
 	data[0] = mask;
 	data[1] = *bits;
 
-	ret = comedi_do_insn(dev, &insn);
+	ret = comedi_do_insn(dev, &insn, data);
 
 	*bits = data[1];
 
@@ -175,11 +175,14 @@ EXPORT_SYMBOL(comedi_dio_bitfield);
 int comedi_find_subdevice_by_type(struct comedi_device *dev, int type,
 				  unsigned int subd)
 {
+	struct comedi_subdevice *s;
+
 	if (subd > dev->n_subdevices)
 		return -ENODEV;
 
 	for (; subd < dev->n_subdevices; subd++) {
-		if (dev->subdevices[subd].type == type)
+		s = &dev->subdevices[subd];
+		if (s->type == type)
 			return subd;
 	}
 	return -1;
@@ -188,7 +191,7 @@ EXPORT_SYMBOL(comedi_find_subdevice_by_type);
 
 int comedi_get_n_channels(struct comedi_device *dev, unsigned int subdevice)
 {
-	struct comedi_subdevice *s = dev->subdevices + subdevice;
+	struct comedi_subdevice *s = &dev->subdevices[subdevice];
 
 	return s->n_chan;
 }
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
index 41f95237789d..59ff0cf73381 100644
--- a/drivers/staging/comedi/range.c
+++ b/drivers/staging/comedi/range.c
@@ -68,7 +68,7 @@ int do_rangeinfo_ioctl(struct comedi_device *dev,
 		return -EINVAL;
 	if (subd >= dev->n_subdevices)
 		return -EINVAL;
-	s = dev->subdevices + subd;
+	s = &dev->subdevices[subd];
 	if (s->range_table) {
 		lr = s->range_table;
 	} else if (s->range_table_list) {
@@ -131,6 +131,7 @@ static int aref_invalid(struct comedi_subdevice *s, unsigned int chanspec)
 int comedi_check_chanlist(struct comedi_subdevice *s, int n,
 			  unsigned int *chanlist)
 {
+	struct comedi_device *dev = s->device;
 	int i;
 	int chan;
 
@@ -139,10 +140,10 @@ int comedi_check_chanlist(struct comedi_subdevice *s, int n,
 			if (CR_CHAN(chanlist[i]) >= s->n_chan ||
 			    CR_RANGE(chanlist[i]) >= s->range_table->length
 			    || aref_invalid(s, chanlist[i])) {
-				printk(KERN_ERR "bad chanlist[%d]=0x%08x "
-				       "in_chan=%d range length=%d\n", i,
-				       chanlist[i], s->n_chan,
-				       s->range_table->length);
+				dev_warn(dev->class_dev,
+					 "bad chanlist[%d]=0x%08x in_chan=%d range length=%d\n",
+					 i, chanlist[i], s->n_chan,
+					 s->range_table->length);
 				return -EINVAL;
 			}
 	} else if (s->range_table_list) {
@@ -152,13 +153,14 @@ int comedi_check_chanlist(struct comedi_subdevice *s, int n,
 			    CR_RANGE(chanlist[i]) >=
 			    s->range_table_list[chan]->length
 			    || aref_invalid(s, chanlist[i])) {
-				printk(KERN_ERR "bad chanlist[%d]=0x%08x\n",
-				       i, chanlist[i]);
+				dev_warn(dev->class_dev,
+					 "bad chanlist[%d]=0x%08x\n",
+					 i, chanlist[i]);
 				return -EINVAL;
 			}
 		}
 	} else {
-		printk(KERN_ERR "comedi: (bug) no range type list!\n");
+		dev_err(dev->class_dev, "(bug) no range type list!\n");
 		return -EINVAL;
 	}
 	return 0;
diff --git a/drivers/staging/cptm1217/clearpad_tm1217.c b/drivers/staging/cptm1217/clearpad_tm1217.c
index 0d924d3a2ab1..a49b0da60049 100644
--- a/drivers/staging/cptm1217/clearpad_tm1217.c
+++ b/drivers/staging/cptm1217/clearpad_tm1217.c
@@ -658,18 +658,7 @@ static struct i2c_driver cp_tm1217_driver = {
 	.resume     = cp_tm1217_resume,
 };
 
-static int __init clearpad_tm1217_init(void)
-{
-	return i2c_add_driver(&cp_tm1217_driver);
-}
-
-static void __exit clearpad_tm1217_exit(void)
-{
-	i2c_del_driver(&cp_tm1217_driver);
-}
-
-module_init(clearpad_tm1217_init);
-module_exit(clearpad_tm1217_exit);
+module_i2c_driver(cp_tm1217_driver);
 
 MODULE_AUTHOR("Ramesh Agarwal <ramesh.agarwal@intel.com>");
 MODULE_DESCRIPTION("Synaptics TM1217 TouchScreen Driver");
diff --git a/drivers/staging/crystalhd/crystalhd_lnx.c b/drivers/staging/crystalhd/crystalhd_lnx.c
index d9e3d618f7f4..166203aeb7b4 100644
--- a/drivers/staging/crystalhd/crystalhd_lnx.c
+++ b/drivers/staging/crystalhd/crystalhd_lnx.c
@@ -373,13 +373,15 @@ static int __devinit chd_dec_init_chdev(struct crystalhd_adp *adp)
 	/* register crystalhd class */
 	crystalhd_class = class_create(THIS_MODULE, "crystalhd");
 	if (IS_ERR(crystalhd_class)) {
+		rc = PTR_ERR(crystalhd_class);
 		BCMLOG_ERR("failed to create class\n");
-		goto fail;
+		goto class_create_fail;
 	}
 
 	dev = device_create(crystalhd_class, NULL, MKDEV(adp->chd_dec_major, 0),
 			    NULL, "crystalhd");
 	if (IS_ERR(dev)) {
+		rc = PTR_ERR(dev);
 		BCMLOG_ERR("failed to create device\n");
 		goto device_create_fail;
 	}
@@ -410,6 +412,8 @@ elem_pool_fail:
 	device_destroy(crystalhd_class, MKDEV(adp->chd_dec_major, 0));
 device_create_fail:
 	class_destroy(crystalhd_class);
+class_create_fail:
+	unregister_chrdev(adp->chd_dec_major, CRYSTALHD_API_NAME);
 fail:
 	return rc;
 }
diff --git a/drivers/staging/csr/Makefile b/drivers/staging/csr/Makefile
index afda44b0a925..ab626edc5ba4 100644
--- a/drivers/staging/csr/Makefile
+++ b/drivers/staging/csr/Makefile
@@ -25,7 +25,6 @@ csr_wifi-y :=	bh.o				\
 		unifi_event.o			\
 		unifi_pdu_processing.o		\
 		unifi_sme.o			\
-		csr_formatted_io.o		\
 		csr_wifi_hip_card_sdio.o	\
 		csr_wifi_hip_card_sdio_intr.o	\
 		csr_wifi_hip_card_sdio_mem.o	\
diff --git a/drivers/staging/csr/bh.c b/drivers/staging/csr/bh.c
index b089c28d5610..addee05a4516 100644
--- a/drivers/staging/csr/bh.c
+++ b/drivers/staging/csr/bh.c
@@ -32,45 +32,49 @@
  *      0 on success or else a Linux error code.
  * ---------------------------------------------------------------------------
  */
-int
-uf_start_thread(unifi_priv_t *priv, struct uf_thread *thread, int (*func)(void *))
+int uf_start_thread(unifi_priv_t *priv,
+		    struct uf_thread *thread, int (*func)(void *))
 {
-    if (thread->thread_task != NULL) {
-        unifi_error(priv, "%s thread already started\n", thread->name);
-        return 0;
-    }
-
-    /* Start the kernel thread that handles all h/w accesses. */
-    thread->thread_task = kthread_run(func, priv, "%s", thread->name);
-    if (IS_ERR(thread->thread_task)) {
-        return PTR_ERR(thread->thread_task);
-    }
-
-    /* Module parameter overides the thread priority */
-    if (bh_priority != -1) {
-        if (bh_priority >= 0 && bh_priority <= MAX_RT_PRIO) {
-            struct sched_param param;
-            priv->bh_thread.prio = bh_priority;
-            unifi_trace(priv, UDBG1, "%s thread (RT) priority = %d\n",
-                        thread->name, bh_priority);
-            param.sched_priority = bh_priority;
-            sched_setscheduler(thread->thread_task, SCHED_FIFO, &param);
-        } else if (bh_priority > MAX_RT_PRIO && bh_priority <= MAX_PRIO) {
-            priv->bh_thread.prio = bh_priority;
-            unifi_trace(priv, UDBG1, "%s thread priority = %d\n",
-                        thread->name, PRIO_TO_NICE(bh_priority));
-            set_user_nice(thread->thread_task, PRIO_TO_NICE(bh_priority));
-        } else {
-            priv->bh_thread.prio = DEFAULT_PRIO;
-            unifi_warning(priv, "%s thread unsupported (%d) priority\n",
-                          thread->name, bh_priority);
-        }
-    } else {
-        priv->bh_thread.prio = DEFAULT_PRIO;
-    }
-    unifi_trace(priv, UDBG2, "Started %s thread\n", thread->name);
-
-    return 0;
+	if (thread->thread_task != NULL) {
+		unifi_error(priv, "%s thread already started\n", thread->name);
+		return 0;
+	}
+
+	/* Start the kernel thread that handles all h/w accesses. */
+	thread->thread_task = kthread_run(func, priv, "%s", thread->name);
+	if (IS_ERR(thread->thread_task))
+		return PTR_ERR(thread->thread_task);
+
+	/* Module parameter overides the thread priority */
+	if (bh_priority != -1) {
+		if (bh_priority >= 0 && bh_priority <= MAX_RT_PRIO) {
+			struct sched_param param;
+			priv->bh_thread.prio = bh_priority;
+			unifi_trace(priv, UDBG1,
+				"%s thread (RT) priority = %d\n",
+				thread->name, bh_priority);
+			param.sched_priority = bh_priority;
+			sched_setscheduler(thread->thread_task,
+					   SCHED_FIFO, &param);
+		} else if (bh_priority > MAX_RT_PRIO &&
+			   bh_priority <= MAX_PRIO) {
+			priv->bh_thread.prio = bh_priority;
+			unifi_trace(priv, UDBG1, "%s thread priority = %d\n",
+					thread->name,
+					PRIO_TO_NICE(bh_priority));
+			set_user_nice(thread->thread_task,
+				      PRIO_TO_NICE(bh_priority));
+		} else {
+			priv->bh_thread.prio = DEFAULT_PRIO;
+			unifi_warning(priv,
+				      "%s thread unsupported (%d) priority\n",
+				      thread->name, bh_priority);
+		}
+	} else
+		priv->bh_thread.prio = DEFAULT_PRIO;
+	unifi_trace(priv, UDBG2, "Started %s thread\n", thread->name);
+
+	return 0;
 } /* uf_start_thread() */
 
 
@@ -88,18 +92,18 @@ uf_start_thread(unifi_priv_t *priv, struct uf_thread *thread, int (*func)(void *
  *
  * ---------------------------------------------------------------------------
  */
-    void
-uf_stop_thread(unifi_priv_t *priv, struct uf_thread *thread)
+void uf_stop_thread(unifi_priv_t *priv, struct uf_thread *thread)
 {
-    if (!thread->thread_task) {
-        unifi_notice(priv, "%s thread is already stopped\n", thread->name);
-        return;
-    }
+	if (!thread->thread_task) {
+		unifi_notice(priv, "%s thread is already stopped\n",
+							thread->name);
+		return;
+	}
 
-    unifi_trace(priv, UDBG2, "Stopping %s thread\n", thread->name);
+	unifi_trace(priv, UDBG2, "Stopping %s thread\n", thread->name);
 
-    kthread_stop(thread->thread_task);
-    thread->thread_task = NULL;
+	kthread_stop(thread->thread_task);
+	thread->thread_task = NULL;
 
 } /* uf_stop_thread() */
 
@@ -118,23 +122,24 @@ uf_stop_thread(unifi_priv_t *priv, struct uf_thread *thread)
  *
  * ---------------------------------------------------------------------------
  */
-    void
+void
 uf_wait_for_thread_to_stop(unifi_priv_t *priv, struct uf_thread *thread)
 {
-    /*
-     * kthread_stop() cannot handle the thread exiting while
-     * kthread_should_stop() is false, so sleep until kthread_stop()
-     * wakes us up.
-     */
-    unifi_trace(priv, UDBG2, "%s waiting for the stop signal.\n", thread->name);
-    set_current_state(TASK_INTERRUPTIBLE);
-    if (!kthread_should_stop()) {
-        unifi_trace(priv, UDBG2, "%s schedule....\n", thread->name);
-        schedule();
-    }
-
-    thread->thread_task = NULL;
-    unifi_trace(priv, UDBG2, "%s exiting....\n", thread->name);
+	/*
+	 * kthread_stop() cannot handle the thread exiting while
+	 * kthread_should_stop() is false, so sleep until kthread_stop()
+	 * wakes us up
+	 */
+	unifi_trace(priv, UDBG2, "%s waiting for the stop signal.\n",
+							thread->name);
+	set_current_state(TASK_INTERRUPTIBLE);
+	if (!kthread_should_stop()) {
+		unifi_trace(priv, UDBG2, "%s schedule....\n", thread->name);
+		schedule();
+	}
+
+	thread->thread_task = NULL;
+	unifi_trace(priv, UDBG2, "%s exiting....\n", thread->name);
 } /* uf_wait_for_thread_to_stop() */
 
 
@@ -155,39 +160,41 @@ uf_wait_for_thread_to_stop(unifi_priv_t *priv, struct uf_thread *thread)
  *      None.
  * ---------------------------------------------------------------------------
  */
-    static void
+static void
 handle_bh_error(unifi_priv_t *priv)
 {
-    u8 conf_param = CONFIG_IND_ERROR;
-    u8 interfaceTag = 0; /* used as a loop counter */
+	netInterface_priv_t *interfacePriv;
+	u8 conf_param = CONFIG_IND_ERROR;
+	u8 interfaceTag;
 
 
-    /* Block unifi_run_bh() until the error has been handled. */
-    priv->bh_thread.block_thread = 1;
+	/* Block unifi_run_bh() until the error has been handled. */
+	priv->bh_thread.block_thread = 1;
 
-    /* Consider UniFi to be uninitialised */
-    priv->init_progress = UNIFI_INIT_NONE;
+	/* Consider UniFi to be uninitialised */
+	priv->init_progress = UNIFI_INIT_NONE;
 
-    /* Stop the network traffic */
-    for( interfaceTag =0; interfaceTag <CSR_WIFI_NUM_INTERFACES;interfaceTag ++) {
-        netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
-        if (interfacePriv->netdev_registered == 1) {
-            netif_carrier_off(priv->netdev[interfaceTag]);
-        }
-    }
+	/* Stop the network traffic */
+	for (interfaceTag = 0;
+	     interfaceTag < CSR_WIFI_NUM_INTERFACES; interfaceTag++) {
+		interfacePriv = priv->interfacePriv[interfaceTag];
+		if (interfacePriv->netdev_registered)
+			netif_carrier_off(priv->netdev[interfaceTag]);
+	}
 
 #ifdef CSR_NATIVE_LINUX
-    /* Force any client waiting on an mlme_wait_for_reply() to abort. */
-    uf_abort_mlme(priv);
+	/* Force any client waiting on an mlme_wait_for_reply() to abort. */
+	uf_abort_mlme(priv);
 
-    /* Cancel any pending workqueue tasks */
-    flush_workqueue(priv->unifi_workqueue);
+	/* Cancel any pending workqueue tasks */
+	flush_workqueue(priv->unifi_workqueue);
 
 #endif /* CSR_NATIVE_LINUX */
 
-    unifi_error(priv, "handle_bh_error: fatal error is reported to the SME.\n");
-    /* Notify the clients (SME or unifi_manager) for the error. */
-    ul_log_config_ind(priv, &conf_param, sizeof(u8));
+	unifi_error(priv,
+		"handle_bh_error: fatal error is reported to the SME.\n");
+	/* Notify the clients (SME or unifi_manager) for the error. */
+	ul_log_config_ind(priv, &conf_param, sizeof(u8));
 
 } /* handle_bh_error() */
 
diff --git a/drivers/staging/csr/csr_formatted_io.c b/drivers/staging/csr/csr_formatted_io.c
deleted file mode 100644
index 7213cc8fb577..000000000000
--- a/drivers/staging/csr/csr_formatted_io.c
+++ /dev/null
@@ -1,27 +0,0 @@
-/*****************************************************************************
-
-            (c) Cambridge Silicon Radio Limited 2010
-            All rights reserved and confidential information of CSR
-
-            Refer to LICENSE.txt included with this source for details
-            on the license terms.
-
-*****************************************************************************/
-#include <linux/kernel.h>
-#include "csr_formatted_io.h"
-
-s32 CsrSnprintf(char *dest, size_t n, const char *fmt, ...)
-{
-    s32 r;
-    va_list args;
-    va_start(args, fmt);
-    r = vsnprintf(dest, n, fmt, args);
-    va_end(args);
-
-    if (dest && (n > 0))
-    {
-        dest[n - 1] = '\0';
-    }
-
-    return r;
-}
diff --git a/drivers/staging/csr/csr_formatted_io.h b/drivers/staging/csr/csr_formatted_io.h
deleted file mode 100644
index 2e238cb98d51..000000000000
--- a/drivers/staging/csr/csr_formatted_io.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef CSR_FORMATTED_IO_H__
-#define CSR_FORMATTED_IO_H__
-/*****************************************************************************
-
-            (c) Cambridge Silicon Radio Limited 2010
-            All rights reserved and confidential information of CSR
-
-            Refer to LICENSE.txt included with this source for details
-            on the license terms.
-
-*****************************************************************************/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <linux/types.h>
-
-s32 CsrSnprintf(char *dest, size_t n, const char *fmt, ...);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/drivers/staging/csr/csr_framework_ext.c b/drivers/staging/csr/csr_framework_ext.c
index 12e7ddf3220a..f91a4bf4e68f 100644
--- a/drivers/staging/csr/csr_framework_ext.c
+++ b/drivers/staging/csr/csr_framework_ext.c
@@ -12,20 +12,9 @@
 #include <linux/version.h>
 #include <linux/kthread.h>
 #include <linux/module.h>
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 34)
-#include <linux/slab.h>
-#endif
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 19)
 #include <linux/freezer.h>
-#endif
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 27)
-#include <asm/semaphore.h>
-#else
 #include <linux/semaphore.h>
-#endif
-
+#include <linux/slab.h>
 #include <linux/bitops.h>
 
 #include "csr_framework_ext.h"
diff --git a/drivers/staging/csr/csr_panic.c b/drivers/staging/csr/csr_panic.c
index 353a829bb74c..095f7fa3ae2c 100644
--- a/drivers/staging/csr/csr_panic.c
+++ b/drivers/staging/csr/csr_panic.c
@@ -9,7 +9,6 @@
 *****************************************************************************/
 
 #include <linux/kernel.h>
-#include <linux/version.h>
 #include <linux/module.h>
 
 #include "csr_panic.h"
diff --git a/drivers/staging/csr/csr_time.c b/drivers/staging/csr/csr_time.c
index 83586ca34e8c..7b597f7622a2 100644
--- a/drivers/staging/csr/csr_time.c
+++ b/drivers/staging/csr/csr_time.c
@@ -10,13 +10,6 @@
 
 #include <linux/kernel.h>
 #include <linux/version.h>
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 33)
-#elif LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 16)
-#include <linux/autoconf.h>
-#include <linux/config.h>
-#endif
-
 #include <linux/time.h>
 #include <linux/module.h>
 
@@ -24,20 +17,18 @@
 
 CsrTime CsrTimeGet(CsrTime *high)
 {
-    struct timespec ts;
-    u64 time;
-    CsrTime low;
+	struct timespec ts;
+	u64 time;
+	CsrTime low;
 
-    ts = current_kernel_time();
-    time = (u64) ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
+	ts = current_kernel_time();
+	time = (u64) ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
 
-    if (high != NULL)
-    {
-        *high = (CsrTime) ((time >> 32) & 0xFFFFFFFF);
-    }
+	if (high != NULL)
+		*high = (CsrTime) ((time >> 32) & 0xFFFFFFFF);
 
-    low = (CsrTime) (time & 0xFFFFFFFF);
+	low = (CsrTime) (time & 0xFFFFFFFF);
 
-    return low;
+	return low;
 }
 EXPORT_SYMBOL_GPL(CsrTimeGet);
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio.c b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
index 44ab00c53fec..cf148a0fec6a 100644
--- a/drivers/staging/csr/csr_wifi_hip_card_sdio.c
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
@@ -1612,13 +1612,13 @@ static CsrResult card_allocate_memory_resources(card_t *card)
     /* Reset any state carried forward from a previous life */
     card->fh_command_queue.q_rd_ptr = 0;
     card->fh_command_queue.q_wr_ptr = 0;
-    (void)CsrSnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
+    (void)scnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
                       "fh_cmd_q");
     for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
     {
         card->fh_traffic_queue[i].q_rd_ptr = 0;
         card->fh_traffic_queue[i].q_wr_ptr = 0;
-        (void)CsrSnprintf(card->fh_traffic_queue[i].name,
+        (void)scnprintf(card->fh_traffic_queue[i].name,
                           UNIFI_QUEUE_NAME_MAX_LENGTH, "fh_data_q%d", i);
     }
 #ifndef CSR_WIFI_HIP_TA_DISABLE
@@ -1826,13 +1826,13 @@ static void card_init_soft_queues(card_t *card)
     /* Reset any state carried forward from a previous life */
     card->fh_command_queue.q_rd_ptr = 0;
     card->fh_command_queue.q_wr_ptr = 0;
-    (void)CsrSnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
+    (void)scnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
                       "fh_cmd_q");
     for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
     {
         card->fh_traffic_queue[i].q_rd_ptr = 0;
         card->fh_traffic_queue[i].q_wr_ptr = 0;
-        (void)CsrSnprintf(card->fh_traffic_queue[i].name,
+        (void)scnprintf(card->fh_traffic_queue[i].name,
                           UNIFI_QUEUE_NAME_MAX_LENGTH, "fh_data_q%d", i);
     }
 #ifndef CSR_WIFI_HIP_TA_DISABLE
diff --git a/drivers/staging/csr/csr_wifi_hip_download.c b/drivers/staging/csr/csr_wifi_hip_download.c
index 8e4a4608ba5c..6db672caaa02 100644
--- a/drivers/staging/csr/csr_wifi_hip_download.c
+++ b/drivers/staging/csr/csr_wifi_hip_download.c
@@ -250,6 +250,7 @@ static CsrResult do_patch_convert_download(card_t *card, void *dlpriv, xbv1_t *p
         if (r != CSR_RESULT_SUCCESS)
         {
             unifi_error(card->ospriv, "Failed to find BOOT_LOADER_CONTROL\n");
+            kfree(pfw);
             return CSR_RESULT_FAILURE;
         }
 
@@ -265,6 +266,7 @@ static CsrResult do_patch_convert_download(card_t *card, void *dlpriv, xbv1_t *p
         desc = unifi_fw_open_buffer(card->ospriv, pfw, psize);
         if (!desc)
         {
+            kfree(pfw);
             return CSR_WIFI_HIP_RESULT_NO_MEMORY;
         }
 
diff --git a/drivers/staging/csr/csr_wifi_hip_send.c b/drivers/staging/csr/csr_wifi_hip_send.c
index 684d30459d75..86aa23cefe30 100644
--- a/drivers/staging/csr/csr_wifi_hip_send.c
+++ b/drivers/staging/csr/csr_wifi_hip_send.c
@@ -172,13 +172,8 @@ static CsrResult send_signal(card_t *card, const u8 *sigptr, u32 siglen,
     {
         const u8 *sig = sigptr;
 
-        unifi_error(card->ospriv, "Signal(%d): %02x %02x %02x %02x %02x %02x %02x %02x"
-                    " %02x %02x %02x %02x %02x %02x %02x %02x\n",
-                    siglen,
-                    sig[0], sig[1], sig[2], sig[3],
-                    sig[4], sig[5], sig[6], sig[7],
-                    sig[8], sig[9], sig[10], sig[11],
-                    sig[12], sig[13], sig[14], sig[15]);
+		unifi_error(card->ospriv, "Signal(%d): %*ph\n", siglen,
+					  16, sig);
         unifi_error(card->ospriv, "Bulkdata pointer %p(%d), %p(%d)\n",
                     bulkdata != NULL?bulkdata->d[0].os_data_ptr : NULL,
                     bulkdata != NULL?bulkdata->d[0].data_length : 0,
diff --git a/drivers/staging/csr/csr_wifi_hip_udi.c b/drivers/staging/csr/csr_wifi_hip_udi.c
index 07cfd36c4971..a65b822db698 100644
--- a/drivers/staging/csr/csr_wifi_hip_udi.c
+++ b/drivers/staging/csr/csr_wifi_hip_udi.c
@@ -64,104 +64,104 @@ s32 unifi_print_status(card_t *card, char *str, s32 *remain)
     }
 
     i = n = 0;
-    written = CsrSnprintf(p, remaining, "Chip ID %u\n",
+    written = scnprintf(p, remaining, "Chip ID %u\n",
                           (u16)card->chip_id);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "Chip Version %04X\n",
+    written = scnprintf(p, remaining, "Chip Version %04X\n",
                           card->chip_version);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "HIP v%u.%u\n",
+    written = scnprintf(p, remaining, "HIP v%u.%u\n",
                           (card->config_data.version >> 8) & 0xFF,
                           card->config_data.version & 0xFF);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "Build %lu: %s\n",
+    written = scnprintf(p, remaining, "Build %u: %s\n",
                           card->build_id, card->build_id_string);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
     cfg = &card->config_data;
 
-    written = CsrSnprintf(p, remaining, "sdio ctrl offset          %u\n",
+    written = scnprintf(p, remaining, "sdio ctrl offset          %u\n",
                           cfg->sdio_ctrl_offset);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "fromhost sigbuf handle    %u\n",
+    written = scnprintf(p, remaining, "fromhost sigbuf handle    %u\n",
                           cfg->fromhost_sigbuf_handle);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "tohost_sigbuf_handle      %u\n",
+    written = scnprintf(p, remaining, "tohost_sigbuf_handle      %u\n",
                           cfg->tohost_sigbuf_handle);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "num_fromhost_sig_frags    %u\n",
+    written = scnprintf(p, remaining, "num_fromhost_sig_frags    %u\n",
                           cfg->num_fromhost_sig_frags);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "num_tohost_sig_frags      %u\n",
+    written = scnprintf(p, remaining, "num_tohost_sig_frags      %u\n",
                           cfg->num_tohost_sig_frags);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "num_fromhost_data_slots   %u\n",
+    written = scnprintf(p, remaining, "num_fromhost_data_slots   %u\n",
                           cfg->num_fromhost_data_slots);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "num_tohost_data_slots     %u\n",
+    written = scnprintf(p, remaining, "num_tohost_data_slots     %u\n",
                           cfg->num_tohost_data_slots);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "data_slot_size            %u\n",
+    written = scnprintf(p, remaining, "data_slot_size            %u\n",
                           cfg->data_slot_size);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
     /* Added by protocol version 0x0001 */
-    written = CsrSnprintf(p, remaining, "overlay_size              %u\n",
+    written = scnprintf(p, remaining, "overlay_size              %u\n",
                           (u16)cfg->overlay_size);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
     /* Added by protocol version 0x0300 */
-    written = CsrSnprintf(p, remaining, "data_slot_round           %u\n",
+    written = scnprintf(p, remaining, "data_slot_round           %u\n",
                           cfg->data_slot_round);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "sig_frag_size             %u\n",
+    written = scnprintf(p, remaining, "sig_frag_size             %u\n",
                           cfg->sig_frag_size);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
     /* Added by protocol version 0x0300 */
-    written = CsrSnprintf(p, remaining, "tohost_sig_pad            %u\n",
+    written = scnprintf(p, remaining, "tohost_sig_pad            %u\n",
                           cfg->tohost_signal_padding);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining, "\nInternal state:\n");
+    written = scnprintf(p, remaining, "\nInternal state:\n");
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining, "Last PHY PANIC: %04x:%04x\n",
+    written = scnprintf(p, remaining, "Last PHY PANIC: %04x:%04x\n",
                           card->last_phy_panic_code, card->last_phy_panic_arg);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "Last MAC PANIC: %04x:%04x\n",
+    written = scnprintf(p, remaining, "Last MAC PANIC: %04x:%04x\n",
                           card->last_mac_panic_code, card->last_mac_panic_arg);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining, "fhsr: %u\n",
+    written = scnprintf(p, remaining, "fhsr: %u\n",
                           (u16)card->from_host_signals_r);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "fhsw: %u\n",
+    written = scnprintf(p, remaining, "fhsw: %u\n",
                           (u16)card->from_host_signals_w);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "thsr: %u\n",
+    written = scnprintf(p, remaining, "thsr: %u\n",
                           (u16)card->to_host_signals_r);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "thsw: %u\n",
+    written = scnprintf(p, remaining, "thsw: %u\n",
                           (u16)card->to_host_signals_w);
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining,
-                          "fh buffer contains: %u signals, %u bytes\n",
+    written = scnprintf(p, remaining,
+                          "fh buffer contains: %d signals, %td bytes\n",
                           card->fh_buffer.count,
                           card->fh_buffer.ptr - card->fh_buffer.buf);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining, "paused: ");
+    written = scnprintf(p, remaining, "paused: ");
     UNIFI_SNPRINTF_RET(p, remaining, written);
     for (i = 0; i < sizeof(card->tx_q_paused_flag) / sizeof(card->tx_q_paused_flag[0]); i++)
     {
-        written = CsrSnprintf(p, remaining, card->tx_q_paused_flag[i]?"1" : "0");
+        written = scnprintf(p, remaining, card->tx_q_paused_flag[i]?"1" : "0");
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
-    written = CsrSnprintf(p, remaining, "\n");
+    written = scnprintf(p, remaining, "\n");
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining,
+    written = scnprintf(p, remaining,
                           "fh command q: %u waiting, %u free of %u:\n",
                           CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_command_queue),
                           CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_command_queue),
@@ -169,7 +169,7 @@ s32 unifi_print_status(card_t *card, char *str, s32 *remain)
     UNIFI_SNPRINTF_RET(p, remaining, written);
     for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
     {
-        written = CsrSnprintf(p, remaining,
+        written = scnprintf(p, remaining,
                               "fh traffic q[%u]: %u waiting, %u free of %u:\n",
                               i,
                               CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_traffic_queue[i]),
@@ -178,58 +178,58 @@ s32 unifi_print_status(card_t *card, char *str, s32 *remain)
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
 
-    written = CsrSnprintf(p, remaining, "fh data slots free: %u\n",
+    written = scnprintf(p, remaining, "fh data slots free: %u\n",
                           card->from_host_data?CardGetFreeFromHostDataSlots(card) : 0);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
 
-    written = CsrSnprintf(p, remaining, "From host data slots:");
+    written = scnprintf(p, remaining, "From host data slots:");
     UNIFI_SNPRINTF_RET(p, remaining, written);
     n = card->config_data.num_fromhost_data_slots;
     for (i = 0; i < n && card->from_host_data; i++)
     {
-        written = CsrSnprintf(p, remaining, " %u",
+        written = scnprintf(p, remaining, " %u",
                               (u16)card->from_host_data[i].bd.data_length);
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
-    written = CsrSnprintf(p, remaining, "\n");
+    written = scnprintf(p, remaining, "\n");
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
-    written = CsrSnprintf(p, remaining, "To host data slots:");
+    written = scnprintf(p, remaining, "To host data slots:");
     UNIFI_SNPRINTF_RET(p, remaining, written);
     n = card->config_data.num_tohost_data_slots;
     for (i = 0; i < n && card->to_host_data; i++)
     {
-        written = CsrSnprintf(p, remaining, " %u",
+        written = scnprintf(p, remaining, " %u",
                               (u16)card->to_host_data[i].data_length);
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
 
-    written = CsrSnprintf(p, remaining, "\n");
+    written = scnprintf(p, remaining, "\n");
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
 #ifdef CSR_UNSAFE_SDIO_ACCESS
-    written = CsrSnprintf(p, remaining, "Host State: %s\n", states[card->host_state]);
+    written = scnprintf(p, remaining, "Host State: %s\n", states[card->host_state]);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
     r = unifi_check_io_status(card, &iostate);
     if (iostate == 1)
     {
-        written = CsrSnprintf(p, remaining, "I/O Check: F1 disabled\n");
+        written = scnprintf(p, remaining, "I/O Check: F1 disabled\n");
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
     else
     {
         if (iostate == 1)
         {
-            written = CsrSnprintf(p, remaining, "I/O Check: pending interrupt\n");
+            written = scnprintf(p, remaining, "I/O Check: pending interrupt\n");
             UNIFI_SNPRINTF_RET(p, remaining, written);
         }
 
-        written = CsrSnprintf(p, remaining, "BH reason interrupt = %d\n",
+        written = scnprintf(p, remaining, "BH reason interrupt = %d\n",
                               card->bh_reason_unifi);
         UNIFI_SNPRINTF_RET(p, remaining, written);
-        written = CsrSnprintf(p, remaining, "BH reason host      = %d\n",
+        written = scnprintf(p, remaining, "BH reason host      = %d\n",
                               card->bh_reason_host);
         UNIFI_SNPRINTF_RET(p, remaining, written);
 
@@ -238,26 +238,26 @@ s32 unifi_print_status(card_t *card, char *str, s32 *remain)
             r = unifi_read_8_or_16(card, card->sdio_ctrl_addr + 2, &b);
             if ((r == CSR_RESULT_SUCCESS) && (!(b & 0x80)))
             {
-                written = CsrSnprintf(p, remaining, "fhsr: %u (driver thinks is %u)\n",
+                written = scnprintf(p, remaining, "fhsr: %u (driver thinks is %u)\n",
                                       b, card->from_host_signals_r);
                 UNIFI_SNPRINTF_RET(p, remaining, written);
                 break;
             }
         }
         iostate = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
-        written = CsrSnprintf(p, remaining, "thsw: %u (driver thinks is %u)\n",
+        written = scnprintf(p, remaining, "thsw: %u (driver thinks is %u)\n",
                               iostate, card->to_host_signals_w);
         UNIFI_SNPRINTF_RET(p, remaining, written);
     }
 #endif
 
-    written = CsrSnprintf(p, remaining, "\nStats:\n");
+    written = scnprintf(p, remaining, "\nStats:\n");
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "Total SDIO bytes: R=%lu W=%lu\n",
+    written = scnprintf(p, remaining, "Total SDIO bytes: R=%u W=%u\n",
                           card->sdio_bytes_read, card->sdio_bytes_written);
 
     UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = CsrSnprintf(p, remaining, "Interrupts generated on card: %lu\n",
+    written = scnprintf(p, remaining, "Interrupts generated on card: %u\n",
                           card->unifi_interrupt_seq);
     UNIFI_SNPRINTF_RET(p, remaining, written);
 
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi.h b/drivers/staging/csr/csr_wifi_hip_unifi.h
index dc3c60b49702..2923e2ef12f2 100644
--- a/drivers/staging/csr/csr_wifi_hip_unifi.h
+++ b/drivers/staging/csr/csr_wifi_hip_unifi.h
@@ -98,7 +98,6 @@ extern "C" {
 #include "csr_framework_ext.h"  /* from the synergy porting folder */
 #include "csr_sdio.h"           /* from the synergy porting folder */
 #include "csr_macro.h"          /* from the synergy porting folder */
-#include "csr_formatted_io.h"   /* from the synergy gsp folder */
 #include "csr_wifi_result.h"
 
 /* Utility MACROS. Note that UNIFI_MAC_ADDRESS_CMP returns TRUE on success */
diff --git a/drivers/staging/csr/drv.c b/drivers/staging/csr/drv.c
index b2c27f4f03d4..249758076a75 100644
--- a/drivers/staging/csr/drv.c
+++ b/drivers/staging/csr/drv.c
@@ -15,8 +15,6 @@
  * ---------------------------------------------------------------------------
  */
 
-
-
 /*
  * Porting Notes:
  * Part of this file contains an example for how to glue the OS layer
@@ -37,6 +35,7 @@
 #include <linux/poll.h>
 #include <asm/uaccess.h>
 #include <linux/jiffies.h>
+#include <linux/version.h>
 
 #include "csr_wifi_hip_unifiversion.h"
 #include "unifi_priv.h"
@@ -124,11 +123,7 @@ static void udi_set_log_filter(ul_client_t *pcli,
 
 
 /* Mutex to protect access to  priv->sme_cli */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
 DEFINE_SEMAPHORE(udi_mutex);
-#else
-DECLARE_MUTEX(udi_mutex);
-#endif
 
 s32 CsrHipResultToStatus(CsrResult csrResult)
 {
@@ -1980,18 +1975,6 @@ uf_sme_queue_message(unifi_priv_t *priv, u8 *buffer, int length)
 } /* uf_sme_queue_message() */
 #endif
 
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create(_class, _parent, _devno, _priv, _fmt, _args)
-#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create_drvdata(_class, _parent, _devno, _priv, _fmt, _args)
-#else
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create(_class, _parent, _devno, _fmt, _args)
-#endif
-
 /*
  ****************************************************************************
  *
@@ -2009,17 +1992,6 @@ static struct file_operations unifi_fops = {
     .poll       = unifi_poll,
 };
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create(_class, _parent, _devno, _priv, _fmt, _args)
-#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create_drvdata(_class, _parent, _devno, _priv, _fmt, _args)
-#else
-#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
-    device_create(_class, _parent, _devno, _fmt, _args)
-#endif
-
 static dev_t unifi_first_devno;
 static struct class *unifi_class;
 
@@ -2042,11 +2014,11 @@ int uf_create_device_nodes(unifi_priv_t *priv, int bus_id)
     }
 
 #ifdef SDIO_EXPORTS_STRUCT_DEVICE
-    if (!UF_DEVICE_CREATE(unifi_class, priv->unifi_device,
-                          devno, priv, "unifi%d", bus_id)) {
+    if (!device_create(unifi_class, priv->unifi_device,
+                       devno, priv, "unifi%d", bus_id)) {
 #else
-    priv->unifi_device = UF_DEVICE_CREATE(unifi_class, NULL,
-                                          devno, priv, "unifi%d", bus_id);
+    priv->unifi_device = device_create(unifi_class, NULL,
+                                       devno, priv, "unifi%d", bus_id);
     if (priv->unifi_device == NULL) {
 #endif /* SDIO_EXPORTS_STRUCT_DEVICE */
 
@@ -2068,13 +2040,13 @@ int uf_create_device_nodes(unifi_priv_t *priv, int bus_id)
         return r;
     }
 
-    if (!UF_DEVICE_CREATE(unifi_class,
+    if (!device_create(unifi_class,
 #ifdef SDIO_EXPORTS_STRUCT_DEVICE
-                          priv->unifi_device,
+                       priv->unifi_device,
 #else
-                          NULL,
+                       NULL,
 #endif /* SDIO_EXPORTS_STRUCT_DEVICE */
-                          devno, priv, "unifiudi%d", bus_id)) {
+                       devno, priv, "unifiudi%d", bus_id)) {
         device_destroy(unifi_class, priv->unifi_cdev.dev);
         cdev_del(&priv->unifiudi_cdev);
         cdev_del(&priv->unifi_cdev);
diff --git a/drivers/staging/csr/firmware.c b/drivers/staging/csr/firmware.c
index d14e11839618..b6d8a6e52915 100644
--- a/drivers/staging/csr/firmware.c
+++ b/drivers/staging/csr/firmware.c
@@ -286,7 +286,7 @@ uf_run_unifihelper(unifi_priv_t *priv)
 
     unifi_trace(priv, UDBG2, "running %s %s %s\n", argv[0], argv[1], argv[2]);
 
-    r = call_usermodehelper(argv[0], argv, envp, 0);
+    r = call_usermodehelper(argv[0], argv, envp, UMH_WAIT_EXEC);
 
     return r;
 #else
@@ -402,9 +402,7 @@ int uf_release_firmware_files(unifi_priv_t *priv)
 int uf_release_firmware(unifi_priv_t *priv, struct dlpriv *to_free)
 {
     if (to_free != NULL) {
-        if (to_free->fw_desc != NULL) {
-            release_firmware((const struct firmware *)to_free->fw_desc);
-        }
+        release_firmware((const struct firmware *)to_free->fw_desc);
         to_free->fw_desc = NULL;
         to_free->dl_data = NULL;
         to_free->dl_len = 0;
diff --git a/drivers/staging/csr/io.c b/drivers/staging/csr/io.c
index e6503d9620a4..caf48e3120ca 100644
--- a/drivers/staging/csr/io.c
+++ b/drivers/staging/csr/io.c
@@ -31,6 +31,7 @@
  * ---------------------------------------------------------------------------
  */
 #include <linux/proc_fs.h>
+#include <linux/version.h>
 
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_unifiversion.h"
@@ -38,7 +39,6 @@
 #include "unifiio.h"
 #include "unifi_priv.h"
 
-
 /*
  * Array of pointers to context structs for unifi devices that are present.
  * The index in the array corresponds to the wlan interface number
@@ -70,11 +70,7 @@ static int In_use[MAX_UNIFI_DEVS];
  * Mutex to prevent UDI clients to open the character device before the priv
  * is created and initialised.
  */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
 DEFINE_SEMAPHORE(Unifi_instance_mutex);
-#else
-DECLARE_MUTEX(Unifi_instance_mutex);
-#endif
 /*
  * When the device is removed, unregister waits on Unifi_cleanup_wq
  * until all the UDI clients release the character device.
@@ -177,21 +173,6 @@ uf_register_netdev(unifi_priv_t *priv, int interfaceTag)
     /* The device is registed */
     interfacePriv->netdev_registered = 1;
 
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-#ifdef CONFIG_NET_SCHED
-    /*
-     * IMPORTANT:
-     * uf_install_qdisc() holds the network device lock, we can not
-     * install the qdisk before the network device is registered.
-     */
-    r = uf_install_qdisc(priv->netdev[interfaceTag]);
-    if (r) {
-        unifi_error(priv, "Failed to install qdisc\n");
-        return r;
-    }
-#endif /* CONFIG_NET_SCHED */
-#endif /* LINUX_VERSION_CODE */
-
 #ifdef CSR_SUPPORT_SME
     /*
      * Register the inet handler; it notifies us for changes in the IP address.
@@ -347,7 +328,7 @@ register_unifi_sdio(CsrSdioFunction *sdio_dev, int bus_id, struct device *dev)
     /*
      * We use the slot number as unifi device index.
      */
-    snprintf(priv->proc_entry_name, 64, "driver/unifi%d", priv->instance);
+    scnprintf(priv->proc_entry_name, 64, "driver/unifi%d", priv->instance);
     /*
      * The following complex casting is in place in order to eliminate 64-bit compilation warning
      * "cast to/from pointer from/to integer of different size"
@@ -669,7 +650,7 @@ unregister_unifi_sdio(int bus_id)
         if(interfacePriv->netdev_registered)
         {
             netif_carrier_off(priv->netdev[interfaceTag]);
-            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[interfaceTag]);
+            netif_tx_stop_all_queues(priv->netdev[interfaceTag]);
         }
     }
 
@@ -904,54 +885,54 @@ uf_read_proc(char *page, char **start, off_t offset, int count,
 
     orig_p = p;
 
-    written = CsrSnprintf(p, remain, "UniFi SDIO Driver: %s %s %s\n",
+    written = scnprintf(p, remain, "UniFi SDIO Driver: %s %s %s\n",
             CSR_WIFI_VERSION, __DATE__, __TIME__);
     UNIFI_SNPRINTF_RET(p, remain, written);
 #ifdef CSR_SME_USERSPACE
-    written = CsrSnprintf(p, remain, "SME: CSR userspace ");
+    written = scnprintf(p, remain, "SME: CSR userspace ");
     UNIFI_SNPRINTF_RET(p, remain, written);
 #ifdef CSR_SUPPORT_WEXT
-    written = CsrSnprintf(p, remain, "with WEXT support\n");
+    written = scnprintf(p, remain, "with WEXT support\n");
 #else
-    written = CsrSnprintf(p, remain, "\n");
+    written = scnprintf(p, remain, "\n");
 #endif /* CSR_SUPPORT_WEXT */
     UNIFI_SNPRINTF_RET(p, remain, written);
 #endif /* CSR_SME_USERSPACE */
 #ifdef CSR_NATIVE_LINUX
-    written = CsrSnprintf(p, remain, "SME: native\n");
+    written = scnprintf(p, remain, "SME: native\n");
     UNIFI_SNPRINTF_RET(p, remain, written);
 #endif
 
 #ifdef CSR_SUPPORT_SME
-    written = CsrSnprintf(p, remain,
-            "Firmware (ROM) build:%lu, Patch:%lu\n",
+    written = scnprintf(p, remain,
+            "Firmware (ROM) build:%u, Patch:%u\n",
             priv->card_info.fw_build,
             priv->sme_versions.firmwarePatch);
     UNIFI_SNPRINTF_RET(p, remain, written);
 #endif
     p += unifi_print_status(priv->card, p, &remain);
 
-    written = CsrSnprintf(p, remain, "Last dbg str: %s\n",
+    written = scnprintf(p, remain, "Last dbg str: %s\n",
             priv->last_debug_string);
     UNIFI_SNPRINTF_RET(p, remain, written);
 
-    written = CsrSnprintf(p, remain, "Last dbg16:");
+    written = scnprintf(p, remain, "Last dbg16:");
     UNIFI_SNPRINTF_RET(p, remain, written);
     for (i = 0; i < 8; i++) {
-        written = CsrSnprintf(p, remain, " %04X",
+        written = scnprintf(p, remain, " %04X",
                 priv->last_debug_word16[i]);
         UNIFI_SNPRINTF_RET(p, remain, written);
     }
-    written = CsrSnprintf(p, remain, "\n");
+    written = scnprintf(p, remain, "\n");
     UNIFI_SNPRINTF_RET(p, remain, written);
-    written = CsrSnprintf(p, remain, "           ");
+    written = scnprintf(p, remain, "           ");
     UNIFI_SNPRINTF_RET(p, remain, written);
     for (; i < 16; i++) {
-        written = CsrSnprintf(p, remain, " %04X",
+        written = scnprintf(p, remain, " %04X",
                 priv->last_debug_word16[i]);
         UNIFI_SNPRINTF_RET(p, remain, written);
     }
-    written = CsrSnprintf(p, remain, "\n");
+    written = scnprintf(p, remain, "\n");
     UNIFI_SNPRINTF_RET(p, remain, written);
     *start = page;
 
diff --git a/drivers/staging/csr/monitor.c b/drivers/staging/csr/monitor.c
index 628782ad641e..7c524a18958e 100644
--- a/drivers/staging/csr/monitor.c
+++ b/drivers/staging/csr/monitor.c
@@ -10,6 +10,7 @@
  * ---------------------------------------------------------------------------
  */
 
+#include <linux/version.h>
 #include "unifi_priv.h"
 
 #ifdef UNIFI_SNIFF_ARPHRD
@@ -23,8 +24,6 @@
 #define ETH_P_80211_RAW ETH_P_ALL
 #endif
 
-
-
 /*
  * ---------------------------------------------------------------------------
  *  uf_start_sniff
@@ -192,11 +191,7 @@ netrx_radiotap(unifi_priv_t *priv,
 
 
     skb->dev = dev;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
     skb->mac_header = skb->data;
-#else
-    skb->mac.raw = skb->data;
-#endif
     skb->pkt_type = PACKET_OTHERHOST;
     skb->protocol = __constant_htons(ETH_P_80211_RAW);
     memset(skb->cb, 0, sizeof(skb->cb));
diff --git a/drivers/staging/csr/netdev.c b/drivers/staging/csr/netdev.c
index 1e6e111a8e15..9a52ab408e1a 100644
--- a/drivers/staging/csr/netdev.c
+++ b/drivers/staging/csr/netdev.c
@@ -15,7 +15,6 @@
  * ---------------------------------------------------------------------------
  */
 
-
 /*
  * Porting Notes:
  * This file implements the data plane of the UniFi linux driver.
@@ -48,59 +47,14 @@
 #include <linux/etherdevice.h>
 #include <linux/mutex.h>
 #include <linux/semaphore.h>
-
+#include <linux/version.h>
 #include <linux/vmalloc.h>
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_conversions.h"
 #include "unifi_priv.h"
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
-#include <net/iw_handler.h>
-#endif
 #include <net/pkt_sched.h>
 
 
-/* ALLOW_Q_PAUSE: Pre 2.6.28 kernels do not support multiple driver queues (required for QoS).
- * In order to support QoS in these kernels, multiple queues are implemented in the driver. But since
- * there is only a single queue in the kernel (leading to multiple queues in the driver) there is no possibility
- * of stopping a particular queue in the kernel. Stopping the single kernel queue leads to undesirable starvation
- * of driver queues. One of the proposals is to not stop the kernel queue but to prevent dequeuing from the
- * 'stopped' driver queue. Allow q pause is an experimental implementation of this scheme for pre 2.6.28 kernels.
- * When NOT defined, queues are paused locally in the driver and packets are dequeued for transmission only from the
- * unpaused queues. When Allow q pause is defined the kernel queue is stopped whenever any driver queue is paused.
- */
-#define ALLOW_Q_PAUSE
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
-#ifdef UNIFI_NET_NAME
-#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
-    do {                                                                \
-        static char name[8];                                           \
-        sprintf(name, "%s%s", UNIFI_NET_NAME, _name);                   \
-        _dev = alloc_netdev_mq(_size, name, _setup, _num_of_queues);    \
-    } while (0);
-#else
-#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
-    do {                                                                \
-        _dev = alloc_etherdev_mq(_size, _num_of_queues);                \
-    } while (0);
-#endif /* UNIFI_NET_NAME */
-#else
-#ifdef UNIFI_NET_NAME
-#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
-    do {                                                                \
-        static char name[8];                                           \
-        sprintf(name, "%s%s", UNIFI_NET_NAME, _name);                   \
-        _dev = alloc_netdev(_size, name, _setup);                       \
-    } while (0);
-#else
-#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
-    do {                                                                \
-        _dev = alloc_etherdev(_size);                                   \
-    } while (0);
-#endif /* UNIFI_NET_NAME */
-#endif /* LINUX_VERSION_CODE */
-
-
 /* Wext handler is suported only if CSR_SUPPORT_WEXT is defined */
 #ifdef CSR_SUPPORT_WEXT
 extern struct iw_handler_def unifi_iw_handler_def;
@@ -119,20 +73,8 @@ static int uf_net_open(struct net_device *dev);
 static int uf_net_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 static int uf_net_stop(struct net_device *dev);
 static struct net_device_stats *uf_net_get_stats(struct net_device *dev);
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
 static u16 uf_net_select_queue(struct net_device *dev, struct sk_buff *skb);
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 static netdev_tx_t uf_net_xmit(struct sk_buff *skb, struct net_device *dev);
-#else
-static int uf_net_xmit(struct sk_buff *skb, struct net_device *dev);
-#ifndef NETDEV_TX_OK
-#define NETDEV_TX_OK        0
-#endif
-#ifndef NETDEV_TX_BUSY
-#define NETDEV_TX_BUSY      1
-#endif
-#endif
 static void uf_set_multicast_list(struct net_device *dev);
 
 
@@ -182,62 +124,8 @@ struct uf_tx_packet_data {
     unsigned long host_tag;
 };
 
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-static int uf_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd);
-static int uf_qdiscop_requeue(struct sk_buff *skb, struct Qdisc* qd);
-static struct sk_buff *uf_qdiscop_dequeue(struct Qdisc* qd);
-static void uf_qdiscop_reset(struct Qdisc* qd);
-static void uf_qdiscop_destroy(struct Qdisc* qd);
-static int uf_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb);
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
-static int uf_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt);
-static int uf_qdiscop_init(struct Qdisc *qd, struct nlattr *opt);
-#else
-static int uf_qdiscop_tune(struct Qdisc *qd, struct rtattr *opt);
-static int uf_qdiscop_init(struct Qdisc *qd, struct rtattr *opt);
-#endif
-#endif
-
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-/* queueing discipline operations */
-static struct Qdisc_ops uf_qdisc_ops =
-{
-    .next = NULL,
-    .cl_ops = NULL,
-    .id = "UniFi Qdisc",
-    .priv_size = sizeof(struct uf_sched_data),
-
-    .enqueue = uf_qdiscop_enqueue,
-    .dequeue = uf_qdiscop_dequeue,
-    .requeue = uf_qdiscop_requeue,
-    .drop = NULL, /* drop not needed since we are always the root qdisc */
-
-    .init = uf_qdiscop_init,
-    .reset = uf_qdiscop_reset,
-    .destroy = uf_qdiscop_destroy,
-    .change = uf_qdiscop_tune,
-
-    .dump = uf_qdiscop_dump,
-};
-#endif /* LINUX_VERSION_CODE */
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
-#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)
-#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
-    qdisc_create_dflt(dev, netdev_get_tx_queue(_dev, 0), _ops, _root)
-#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
-#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
-    qdisc_create_dflt(dev, _ops, _root)
-#else
-#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
-    qdisc_create_dflt(dev, _ops)
-#endif /* LINUX_VERSION_CODE */
-
 #endif /* CONFIG_NET_SCHED */
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
 static const struct net_device_ops uf_netdev_ops =
 {
     .ndo_open = uf_net_open,
@@ -248,7 +136,6 @@ static const struct net_device_ops uf_netdev_ops =
     .ndo_set_rx_mode = uf_set_multicast_list,
     .ndo_select_queue = uf_net_select_queue,
 };
-#endif
 
 static u8 oui_rfc1042[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
 static u8 oui_8021h[P80211_OUI_LEN]   = { 0x00, 0x00, 0xf8 };
@@ -310,7 +197,7 @@ uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id)
      * The RedHat 9 redhat-config-network tool doesn't recognise wlan* devices,
      * so use "eth*" (like other wireless extns drivers).
      */
-    UF_ALLOC_NETDEV(dev, sizeof(unifi_priv_t)+sizeof(netInterface_priv_t), "%d", ether_setup, UNIFI_TRAFFIC_Q_MAX);
+    dev = alloc_etherdev_mq(sizeof(unifi_priv_t) + sizeof(netInterface_priv_t), UNIFI_TRAFFIC_Q_MAX);
 
     if (dev == NULL) {
         return NULL;
@@ -332,22 +219,7 @@ uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id)
     priv->interfacePriv[0] = interfacePriv;
 
     /* Setup / override net_device fields */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
     dev->netdev_ops = &uf_netdev_ops;
-#else
-    dev->open             = uf_net_open;
-    dev->stop             = uf_net_stop;
-    dev->hard_start_xmit  = uf_net_xmit;
-    dev->do_ioctl         = uf_net_ioctl;
-
-    /* called by /proc/net/dev */
-    dev->get_stats = uf_net_get_stats;
-
-    dev->set_multicast_list = uf_set_multicast_list;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
-    dev->select_queue       = uf_net_select_queue;
-#endif
-#endif
 
 #ifdef CSR_SUPPORT_WEXT
     dev->wireless_handlers = &unifi_iw_handler_def;
@@ -440,21 +312,13 @@ uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id)
     interfacePriv->connected = UnifiConnectedUnknown;  /* -1 unknown, 0 no, 1 yes */
 
 #ifdef USE_DRIVER_LOCK
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
     sema_init(&priv->lock, 1);
-#else
-    init_MUTEX(&priv->lock);
-#endif
 #endif /* USE_DRIVER_LOCK */
 
     spin_lock_init(&priv->send_signal_lock);
 
     spin_lock_init(&priv->m4_lock);
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
     sema_init(&priv->ba_mutex, 1);
-#else
-    init_MUTEX(&priv->ba_mutex);
-#endif
 
 #if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
     spin_lock_init(&priv->wapi_lock);
@@ -487,16 +351,8 @@ uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id)
 #endif
 #endif
 
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-#ifdef CONFIG_NET_SCHED
-    /* Register the qdisc operations */
-    register_qdisc(&uf_qdisc_ops);
-#endif /* CONFIG_NET_SCHED */
-#endif /* LINUX_VERSION_CODE */
-
     priv->ref_count = 1;
 
-
     priv->amp_client = NULL;
     priv->coredump_mode = 0;
     priv->ptest_mode = 0;
@@ -566,7 +422,7 @@ uf_alloc_netdevice_for_other_interfaces(unifi_priv_t *priv, u16 interfaceTag)
      * The RedHat 9 redhat-config-network tool doesn't recognise wlan* devices,
      * so use "eth*" (like other wireless extns drivers).
      */
-    UF_ALLOC_NETDEV(dev, sizeof(netInterface_priv_t), "%d", ether_setup, 1);
+    dev = alloc_etherdev_mq(sizeof(netInterface_priv_t), 1);
     if (dev == NULL) {
         return FALSE;
     }
@@ -589,19 +445,7 @@ uf_alloc_netdevice_for_other_interfaces(unifi_priv_t *priv, u16 interfaceTag)
     INIT_LIST_HEAD(&interfacePriv->rx_controlled_list);
 
     /* Setup / override net_device fields */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
     dev->netdev_ops = &uf_netdev_ops;
-#else
-    dev->open             = uf_net_open;
-    dev->stop             = uf_net_stop;
-    dev->hard_start_xmit  = uf_net_xmit;
-    dev->do_ioctl         = uf_net_ioctl;
-
-    /* called by /proc/net/dev */
-    dev->get_stats = uf_net_get_stats;
-
-    dev->set_multicast_list = uf_set_multicast_list;
-#endif
 
 #ifdef CSR_SUPPORT_WEXT
     dev->wireless_handlers = &unifi_iw_handler_def;
@@ -686,13 +530,6 @@ uf_free_netdevice(unifi_priv_t *priv)
     spin_unlock_irqrestore(&priv->wapi_lock, flags);
 #endif
 
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-#ifdef CONFIG_NET_SCHED
-    /* Unregister the qdisc operations */
-    unregister_qdisc(&uf_qdisc_ops);
-#endif /* CONFIG_NET_SCHED */
-#endif /* LINUX_VERSION_CODE */
-
 #ifdef CSR_SUPPORT_WEXT
     /* Unregister callback for netdevice state changes */
     unregister_netdevice_notifier(&uf_netdev_notifier);
@@ -700,10 +537,8 @@ uf_free_netdevice(unifi_priv_t *priv)
 
 #ifdef CSR_SUPPORT_SME
     /* Cancel work items and destroy the workqueue */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,23)
     cancel_work_sync(&priv->multicast_list_task);
 #endif
-#endif
 /* Destroy the workqueues. */
     flush_workqueue(priv->unifi_workqueue);
     destroy_workqueue(priv->unifi_workqueue);
@@ -778,7 +613,7 @@ uf_net_open(struct net_device *dev)
     }
 #endif
 
-    UF_NETIF_TX_START_ALL_QUEUES(dev);
+    netif_tx_start_all_queues(dev);
 
     func_exit();
     return 0;
@@ -808,7 +643,7 @@ uf_net_stop(struct net_device *dev)
     func_enter();
 #endif
 
-    UF_NETIF_TX_STOP_ALL_QUEUES(dev);
+    netif_tx_stop_all_queues(dev);
 
     func_exit();
     return 0;
@@ -957,7 +792,6 @@ get_packet_priority(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr
     return priority;
 }
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
 /*
  * ---------------------------------------------------------------------------
  *  uf_net_select_queue
@@ -1005,7 +839,6 @@ uf_net_select_queue(struct net_device *dev, struct sk_buff *skb)
     func_exit();
     return (u16)queue;
 } /* uf_net_select_queue() */
-#endif
 
 int
 skb_add_llc_snap(struct net_device *dev, struct sk_buff *skb, int proto)
@@ -1915,11 +1748,7 @@ send_ma_pkt_request(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr
  *      The controlled port is handled in the qdisc dequeue handler.
  * ---------------------------------------------------------------------------
  */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 static netdev_tx_t
-#else
-static int
-#endif
 uf_net_xmit(struct sk_buff *skb, struct net_device *dev)
 {
     netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
@@ -1929,9 +1758,7 @@ uf_net_xmit(struct sk_buff *skb, struct net_device *dev)
     int result;
     static tx_signal_handler tx_handler;
     CSR_PRIORITY priority;
-#if !defined (CONFIG_NET_SCHED) || (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28))
     CsrWifiRouterCtrlPortAction port_action;
-#endif /* CONFIG_NET_SCHED */
 
     func_enter();
 
@@ -1956,11 +1783,6 @@ uf_net_xmit(struct sk_buff *skb, struct net_device *dev)
         port = UF_UNCONTROLLED_PORT_Q;
     }
 
-#if defined (CONFIG_NET_SCHED) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28))
-    /* Remove the ethernet header */
-    skb_pull(skb, ETH_HLEN);
-    result = tx_handler(priv, skb, &ehdr, priority);
-#else
     /* Uncontrolled port rules apply */
     port_action = verify_port(priv
         , (((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode)||(CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI== interfacePriv->interfaceMode))? interfacePriv->bssid.a: ehdr.h_dest)
@@ -1986,7 +1808,6 @@ uf_net_xmit(struct sk_buff *skb, struct net_device *dev)
         func_exit();
         return NETDEV_TX_OK;
     }
-#endif /* CONFIG_NET_SCHED */
 
     if (result == NETDEV_TX_OK) {
 #if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
@@ -2059,7 +1880,6 @@ unifi_pause_xmit(void *ospriv, unifi_TrafficQueue queue)
     func_enter();
     unifi_trace(priv, UDBG2, "Stopping queue %d\n", queue);
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
     for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
     {
         if (netif_running(priv->netdev[i]))
@@ -2067,24 +1887,6 @@ unifi_pause_xmit(void *ospriv, unifi_TrafficQueue queue)
             netif_stop_subqueue(priv->netdev[i], (u16)queue);
         }
     }
-#else
-#ifdef ALLOW_Q_PAUSE
-    unifi_trace(priv, UDBG2, "Stopping netif\n");
-    /* stop the traffic from all the interfaces. */
-    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
-    {
-        if (netif_running(priv->netdev[i])) {
-            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[i]);
-        }
-    }
-#else
-    if (net_is_tx_q_paused(priv, queue)) {
-        unifi_trace(priv, UDBG2, "Queue already stopped\n");
-        return;
-    }
-    net_tx_q_pause(priv, queue);
-#endif
-#endif
 
 #ifdef CSR_SUPPORT_SME
     if(queue<=3) {
@@ -2108,7 +1910,6 @@ unifi_restart_xmit(void *ospriv, unifi_TrafficQueue queue)
     func_enter();
     unifi_trace(priv, UDBG2, "Waking queue %d\n", queue);
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
     for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
     {
         if (netif_running(priv->netdev[i]))
@@ -2116,25 +1917,6 @@ unifi_restart_xmit(void *ospriv, unifi_TrafficQueue queue)
             netif_wake_subqueue(priv->netdev[i], (u16)queue);
         }
     }
-#else
-#ifdef ALLOW_Q_PAUSE
-    /* Need to supply queue number depending on Kernel support */
-    /* Resume the traffic from all the interfaces */
-    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
-    {
-        if (netif_running(priv->netdev[i])) {
-            UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[i]);
-        }
-    }
-#else
-    if (!(net_is_tx_q_paused(priv, queue))) {
-        unifi_trace(priv, UDBG2, "Queue already running\n");
-        func_exit();
-        return;
-    }
-    net_tx_q_unpause(priv, queue);
-#endif
-#endif
 
 #ifdef CSR_SUPPORT_SME
     if(queue <=3) {
@@ -2349,13 +2131,7 @@ uf_resume_data_plane(unifi_priv_t *priv, int queue,
     {
 #ifdef CONFIG_NET_SCHED
         if (netif_running(priv->netdev[interfaceTag])) {
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
             netif_tx_schedule_all(priv->netdev[interfaceTag]);
-#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-            netif_schedule_queue(netdev_get_tx_queue(priv->netdev[interfaceTag], 0));
-#else
-            netif_schedule(priv->netdev[interfaceTag]);
-#endif /* LINUX_VERSION_CODE */
         }
 #endif
         uf_process_rx_pending_queue(priv, queue, peer_address, 1,interfaceTag);
@@ -2998,19 +2774,13 @@ uf_set_multicast_list(struct net_device *dev)
 #else
 
     u8 *mc_list = interfacePriv->mc_list;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34)
     struct netdev_hw_addr *mc_addr;
     int mc_addr_count;
-#else
-    struct dev_mc_list *p;      /* Pointer to the addresses structure. */
-    int i;
-#endif
 
     if (priv->init_progress != UNIFI_INIT_COMPLETED) {
         return;
     }
 
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34)
     mc_addr_count = netdev_mc_count(dev);
 
     unifi_trace(priv, UDBG3,
@@ -3029,25 +2799,6 @@ uf_set_multicast_list(struct net_device *dev)
         mc_list += ETH_ALEN;
     }
 
-#else
-    unifi_trace(priv, UDBG3,
-            "uf_set_multicast_list (count=%d)\n", dev->mc_count);
-
-    /* Not enough space? */
-    if (dev->mc_count > UNIFI_MAX_MULTICAST_ADDRESSES) {
-        return;
-    }
-
-    /* Store the list to be processed by the work item. */
-    interfacePriv->mc_list_count = dev->mc_count;
-    p = dev->mc_list;
-    for (i = 0; i < dev->mc_count; i++) {
-        memcpy(mc_list, p->dmi_addr, ETH_ALEN);
-        p = p->next;
-        mc_list += ETH_ALEN;
-    }
-#endif
-
     /* Send a message to the workqueue */
     queue_work(priv->unifi_workqueue, &priv->multicast_list_task);
 #endif
@@ -3181,375 +2932,6 @@ void uf_net_get_name(struct net_device *dev, char *name, int len)
 
 } /* uf_net_get_name */
 
-
-
-
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-#ifdef CONFIG_NET_SCHED
-
-/*
- * ---------------------------------------------------------------------------
- *  uf_install_qdisc
- *
- *      Creates a root qdisc, registers our qdisc handlers and
- *      overrides the device's qdisc_sleeping to prevent the system
- *      from creating a new one for our network device.
- *
- *  Arguments:
- *      dev             Pointer to the network device.
- *
- *  Returns:
- *      0 on success, Linux error code otherwise.
- *
- *  Notes:
- *      This function holds the qdisk lock so it needs to be called
- *      after registering the network device in uf_register_netdev().
- *      Also, the qdisc_create_dflt() API has changed in 2.6.20 to
- *      include the parentid.
- * ---------------------------------------------------------------------------
- */
-int uf_install_qdisc(struct net_device *dev)
-{
-    struct Qdisc *qdisc;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    struct netdev_queue *queue0;
-#endif /* LINUX_VERSION_CODE */
-
-
-    func_enter();
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,27)
-    /*
-     * check that there is no qdisc currently attached to device
-     * this ensures that we will be the root qdisc. (I can't find a better
-     * way to test this explicitly)
-     */
-    if (dev->qdisc_sleeping != &noop_qdisc) {
-        func_exit_r(-EFAULT);
-        return -EINVAL;
-    }
-#endif /* LINUX_VERSION_CODE */
-
-    qdisc = UF_QDISC_CREATE_DFLT(dev, &uf_qdisc_ops, TC_H_ROOT);
-    if (!qdisc) {
-        unifi_error(NULL, "%s: qdisc installation failed\n", dev->name);
-        func_exit_r(-EFAULT);
-        return -EFAULT;
-    }
-    unifi_trace(NULL, UDBG5, "%s: parent qdisc=0x%p\n",
-            dev->name, qdisc);
-
-    qdisc->handle = 0x80020000;
-    qdisc->flags = 0x0;
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    queue0 = netdev_get_tx_queue(dev, 0);
-    if (queue0 == NULL) {
-        unifi_error(NULL, "%s: netdev_get_tx_queue returned no queue\n",
-                dev->name);
-        func_exit_r(-EFAULT);
-        return -EFAULT;
-    }
-    queue0->qdisc = qdisc;
-    queue0->qdisc_sleeping = qdisc;
-#else
-    qdisc_lock_tree(dev);
-    list_add_tail(&qdisc->list, &dev->qdisc_list);
-    dev->qdisc_sleeping = qdisc;
-    qdisc_unlock_tree(dev);
-#endif /* LINUX_VERSION_CODE */
-
-    func_exit_r(0);
-    return 0;
-
-} /* uf_install_qdisc() */
-
-static int uf_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
-{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev_queue->dev);
-#else
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev);
-#endif /* LINUX_VERSION_CODE */
-    unifi_priv_t *priv = interfacePriv->privPtr;
-    struct uf_sched_data *q = qdisc_priv(qd);
-    struct uf_tx_packet_data *pkt_data = (struct uf_tx_packet_data *) skb->cb;
-    struct ethhdr ehdr;
-    struct Qdisc *qdisc;
-    int r, proto;
-
-    func_enter();
-
-    memcpy(&ehdr, skb->data, ETH_HLEN);
-    proto = ntohs(ehdr.h_proto);
-
-    /* 802.1x - apply controlled/uncontrolled port rules */
-    if ((proto != ETH_P_PAE)
-#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
-            && (proto != ETH_P_WAI)
-#endif
-       ) {
-        /* queues 0 - 3 */
-        pkt_data->priority = get_packet_priority(priv, skb, &ehdr, interfacePriv);
-        pkt_data->queue = unifi_frame_priority_to_queue(pkt_data->priority);
-    } else {
-        pkt_data->queue = UNIFI_TRAFFIC_Q_EAPOL;
-    }
-
-    qdisc = q->queues[pkt_data->queue];
-    r = qdisc->enqueue(skb, qdisc);
-    if (r == NET_XMIT_SUCCESS) {
-        qd->q.qlen++;
-        qd->bstats.bytes += skb->len;
-        qd->bstats.packets++;
-        func_exit_r(NET_XMIT_SUCCESS);
-        return NET_XMIT_SUCCESS;
-    }
-
-    unifi_error(priv, "uf_qdiscop_enqueue: dropped\n");
-    qd->qstats.drops++;
-
-    func_exit_r(r);
-    return r;
-
-} /* uf_qdiscop_enqueue() */
-
-
-static int uf_qdiscop_requeue(struct sk_buff *skb, struct Qdisc* qd)
-{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t*)netdev_priv(qd->dev_queue->dev);
-#else
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t*)netdev_priv(qd->dev);
-#endif /* LINUX_VERSION_CODE */
-    unifi_priv_t *priv = interfacePriv->privPtr;
-    struct uf_sched_data *q = qdisc_priv(qd);
-    struct uf_tx_packet_data *pkt_data = (struct uf_tx_packet_data *) skb->cb;
-    struct Qdisc *qdisc;
-    int r;
-
-    func_enter();
-
-    unifi_trace(priv, UDBG5, "uf_qdiscop_requeue: (q=%d), tag=%u\n",
-            pkt_data->queue, pkt_data->host_tag);
-
-    /* we recorded which queue to use earlier! */
-    qdisc = q->queues[pkt_data->queue];
-
-    if ((r = qdisc->ops->requeue(skb, qdisc)) == 0) {
-        qd->q.qlen++;
-        func_exit_r(0);
-        return 0;
-    }
-
-    unifi_error(priv, "uf_qdiscop_requeue: dropped\n");
-    qd->qstats.drops++;
-
-    func_exit_r(r);
-    return r;
-} /* uf_qdiscop_requeue() */
-
-static struct sk_buff *uf_qdiscop_dequeue(struct Qdisc* qd)
-{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev_queue->dev);
-#else
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev);
-#endif /* LINUX_VERSION_CODE */
-    unifi_priv_t *priv = interfacePriv->privPtr;
-    struct uf_sched_data *q = qdisc_priv(qd);
-    struct sk_buff *skb;
-    struct Qdisc *qdisc;
-    int queue, i;
-    struct ethhdr ehdr;
-    struct uf_tx_packet_data *pkt_data;
-    CsrWifiRouterCtrlPortAction port_action;
-
-    func_enter();
-
-    /* check all the queues */
-    for (i = UNIFI_TRAFFIC_Q_MAX - 1; i >= 0; i--) {
-
-        if (i != UNIFI_TRAFFIC_Q_EAPOL) {
-            queue = priv->prev_queue;
-            if (++priv->prev_queue >= UNIFI_TRAFFIC_Q_EAPOL) {
-                priv->prev_queue = 0;
-            }
-        } else {
-            queue = i;
-        }
-
-#ifndef ALLOW_Q_PAUSE
-        /* If queue is paused, do not dequeue */
-        if (net_is_tx_q_paused(priv, queue)) {
-            unifi_trace(priv, UDBG5,
-                    "uf_qdiscop_dequeue: tx queue paused (q=%d)\n", queue);
-            continue;
-        }
-#endif
-
-        qdisc = q->queues[queue];
-        skb = qdisc->dequeue(qdisc);
-        if (skb) {
-            /* A packet has been dequeued, decrease the queued packets count */
-            qd->q.qlen--;
-
-            pkt_data = (struct uf_tx_packet_data *) skb->cb;
-
-            /* Check the (un)controlled port status */
-            memcpy(&ehdr, skb->data, ETH_HLEN);
-
-            port_action = verify_port(priv
-                            , (((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) ||(CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI == interfacePriv->interfaceMode))? interfacePriv->bssid.a: ehdr.h_dest)
-                            , (UNIFI_TRAFFIC_Q_EAPOL == queue? UF_UNCONTROLLED_PORT_Q: UF_CONTROLLED_PORT_Q)
-                            , interfacePriv->InterfaceTag);
-
-            /* Dequeue packet if port is open */
-            if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
-                unifi_trace(priv, UDBG5,
-                        "uf_qdiscop_dequeue: new (q=%d), tag=%u\n",
-                        queue, pkt_data->host_tag);
-
-                func_exit();
-                return skb;
-            }
-
-            /* Discard or block the packet if necessary */
-            if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) {
-                unifi_trace(priv, UDBG5,
-                        "uf_qdiscop_dequeue: drop (q=%d), tag=%u\n",
-                        queue, pkt_data->host_tag);
-                kfree_skb(skb);
-                break;
-            }
-
-            /* We can not send the packet now, put it back to the queue */
-            if (qdisc->ops->requeue(skb, qdisc) != 0) {
-                unifi_error(priv,
-                        "uf_qdiscop_dequeue: requeue (q=%d) failed, tag=%u, drop it\n",
-                        queue, pkt_data->host_tag);
-
-                /* Requeue failed, drop the packet */
-                kfree_skb(skb);
-                break;
-            }
-            /* We requeued the packet, increase the queued packets count */
-            qd->q.qlen++;
-
-            unifi_trace(priv, UDBG5,
-                    "uf_qdiscop_dequeue: skip (q=%d), tag=%u\n",
-                    queue, pkt_data->host_tag);
-        }
-    }
-
-    func_exit();
-    return NULL;
-} /* uf_qdiscop_dequeue() */
-
-
-static void uf_qdiscop_reset(struct Qdisc* qd)
-{
-    struct uf_sched_data *q = qdisc_priv(qd);
-    int queue;
-    func_enter();
-
-    for (queue = 0; queue < UNIFI_TRAFFIC_Q_MAX; queue++) {
-        qdisc_reset(q->queues[queue]);
-    }
-    qd->q.qlen = 0;
-
-    func_exit();
-} /* uf_qdiscop_reset() */
-
-
-static void uf_qdiscop_destroy(struct Qdisc* qd)
-{
-    struct uf_sched_data *q = qdisc_priv(qd);
-    int queue;
-
-    func_enter();
-
-    for (queue=0; queue < UNIFI_TRAFFIC_Q_MAX; queue++) {
-        qdisc_destroy(q->queues[queue]);
-        q->queues[queue] = &noop_qdisc;
-    }
-
-    func_exit();
-} /* uf_qdiscop_destroy() */
-
-
-/* called whenever parameters are updated on existing qdisc */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
-static int uf_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt)
-#else
-static int uf_qdiscop_tune(struct Qdisc *qd, struct rtattr *opt)
-#endif
-{
-    func_enter();
-    func_exit();
-    return 0;
-} /* uf_qdiscop_tune() */
-
-
-/* called during initial creation of qdisc on device */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
-static int uf_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
-#else
-static int uf_qdiscop_init(struct Qdisc *qd, struct rtattr *opt)
-#endif
-{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-    struct net_device *dev = qd->dev_queue->dev;
-#else
-    struct net_device *dev = qd->dev;
-#endif /* LINUX_VERSION_CODE */
-    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
-    unifi_priv_t *priv = interfacePriv->privPtr;
-    struct uf_sched_data *q = qdisc_priv(qd);
-    int err = 0, i;
-
-    func_enter();
-
-    /* make sure we do not mess with the ingress qdisc */
-    if (qd->flags & TCQ_F_INGRESS) {
-        func_exit();
-        return -EINVAL;
-    }
-
-    /* if options were passed in, set them */
-    if (opt) {
-        err = uf_qdiscop_tune(qd, opt);
-    }
-
-    /* create child queues */
-    for (i = 0; i < UNIFI_TRAFFIC_Q_MAX; i++) {
-        q->queues[i] = UF_QDISC_CREATE_DFLT(dev, &pfifo_qdisc_ops,
-                qd->handle);
-        if (!q->queues[i]) {
-            q->queues[i] = &noop_qdisc;
-            unifi_error(priv, "%s child qdisc %i creation failed\n");
-        }
-
-        unifi_trace(priv, UDBG5, "%s: child qdisc=0x%p\n",
-                dev->name, q->queues[i]);
-    }
-
-    func_exit_r(err);
-    return err;
-} /* uf_qdiscop_init() */
-
-
-static int uf_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb)
-{
-    func_enter();
-    func_exit_r(skb->len);
-    return skb->len;
-} /* uf_qdiscop_dump() */
-
-#endif /* CONFIG_NET_SCHED */
-#endif /* LINUX_VERSION_CODE */
-
 #ifdef CSR_SUPPORT_WEXT
 
 /*
@@ -3595,7 +2977,7 @@ uf_netdev_event(struct notifier_block *notif, unsigned long event, void* ptr) {
                     interfacePriv->wait_netdev_change ? "" : "not");
 
         if (interfacePriv->wait_netdev_change) {
-            UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[interfacePriv->InterfaceTag]);
+            netif_tx_wake_all_queues(priv->netdev[interfacePriv->InterfaceTag]);
             interfacePriv->connected = UnifiConnected;
             interfacePriv->wait_netdev_change = FALSE;
             /* Note: passing the broadcast address here will allow anyone to attempt to join our adhoc network */
diff --git a/drivers/staging/csr/sdio_events.c b/drivers/staging/csr/sdio_events.c
index 6892c2e281bc..2a80b9eb0200 100644
--- a/drivers/staging/csr/sdio_events.c
+++ b/drivers/staging/csr/sdio_events.c
@@ -66,7 +66,7 @@ void unifi_suspend(void *ospriv)
                 unifi_trace(priv, UDBG1, "unifi_suspend: netif_carrier_off");
                 netif_carrier_off(priv->netdev[interfaceTag]);
             }
-            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[interfaceTag]);
+            netif_tx_stop_all_queues(priv->netdev[interfaceTag]);
         }
     }
 
@@ -119,7 +119,7 @@ void unifi_resume(void *ospriv)
             if (interfacePriv->netdev_registered == 1)
             {
                 netif_carrier_on(priv->netdev[interfaceTag]);
-                UF_NETIF_TX_START_ALL_QUEUES(priv->netdev[interfaceTag]);
+                netif_tx_start_all_queues(priv->netdev[interfaceTag]);
             }
         }
 
diff --git a/drivers/staging/csr/sdio_mmc.c b/drivers/staging/csr/sdio_mmc.c
index d3fd57cdde0b..af3e40bb5010 100644
--- a/drivers/staging/csr/sdio_mmc.c
+++ b/drivers/staging/csr/sdio_mmc.c
@@ -14,7 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/mutex.h>
 #include <linux/gfp.h>
-
+#include <linux/version.h>
 #include <linux/mmc/core.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -31,7 +31,6 @@ struct wake_lock unifi_sdio_wake_lock; /* wakelock to prevent suspend while resu
 
 static CsrSdioFunctionDriver *sdio_func_drv;
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
 static int uf_sdio_mmc_power_event(struct notifier_block *this, unsigned long event, void *ptr);
 #endif
@@ -45,7 +44,6 @@ static int uf_sdio_mmc_power_event(struct notifier_block *this, unsigned long ev
  * returning immediately (at least on x86).
  */
 static int card_is_powered = 1;
-#endif /* 2.6.32 */
 
 /* MMC uses ENOMEDIUM to indicate card gone away */
 
@@ -637,7 +635,6 @@ CsrSdioFunctionIdle(CsrSdioFunction *function)
 CsrResult
 CsrSdioPowerOn(CsrSdioFunction *function)
 {
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
     struct sdio_func *func = (struct sdio_func *)function->priv;
     struct mmc_host *host = func->card->host;
 
@@ -649,7 +646,6 @@ CsrSdioPowerOn(CsrSdioFunction *function)
         printk(KERN_INFO "SDIO: Skip power on; card is already powered.\n");
     }
     _sdio_release_host(func);
-#endif /* 2.6.32 */
 
     return CSR_RESULT_SUCCESS;
 } /* CsrSdioPowerOn() */
@@ -667,7 +663,6 @@ CsrSdioPowerOn(CsrSdioFunction *function)
 void
 CsrSdioPowerOff(CsrSdioFunction *function)
 {
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
     struct sdio_func *func = (struct sdio_func *)function->priv;
     struct mmc_host *host = func->card->host;
 
@@ -679,7 +674,6 @@ CsrSdioPowerOff(CsrSdioFunction *function)
         printk(KERN_INFO "SDIO: Skip power off; card is already powered off.\n");
     }
     _sdio_release_host(func);
-#endif /* 2.6.32 */
 } /* CsrSdioPowerOff() */
 
 
@@ -845,19 +839,18 @@ uf_glue_sdio_int_handler(struct sdio_func *func)
  *      Status of the removal.
  * ---------------------------------------------------------------------------
  */
-int
-csr_sdio_linux_remove_irq(CsrSdioFunction *function)
+int csr_sdio_linux_remove_irq(CsrSdioFunction *function)
 {
-    struct sdio_func *func = (struct sdio_func *)function->priv;
-    int r;
+	struct sdio_func *func = (struct sdio_func *)function->priv;
+	int r;
 
-    unifi_trace(NULL, UDBG1, "csr_sdio_linux_remove_irq\n");
+	unifi_trace(NULL, UDBG1, "csr_sdio_linux_remove_irq\n");
 
-    sdio_claim_host(func);
-    r = sdio_release_irq(func);
-    sdio_release_host(func);
+	sdio_claim_host(func);
+	r = sdio_release_irq(func);
+	sdio_release_host(func);
 
-    return r;
+	return r;
 
 } /* csr_sdio_linux_remove_irq() */
 
@@ -876,28 +869,25 @@ csr_sdio_linux_remove_irq(CsrSdioFunction *function)
  *      Status of the removal.
  * ---------------------------------------------------------------------------
  */
-int
-csr_sdio_linux_install_irq(CsrSdioFunction *function)
+int csr_sdio_linux_install_irq(CsrSdioFunction *function)
 {
-    struct sdio_func *func = (struct sdio_func *)function->priv;
-    int r;
+	struct sdio_func *func = (struct sdio_func *)function->priv;
+	int r;
 
-    unifi_trace(NULL, UDBG1, "csr_sdio_linux_install_irq\n");
+	unifi_trace(NULL, UDBG1, "csr_sdio_linux_install_irq\n");
 
-    /* Register our interrupt handle */
-    sdio_claim_host(func);
-    r = sdio_claim_irq(func, uf_glue_sdio_int_handler);
-    sdio_release_host(func);
+	/* Register our interrupt handle */
+	sdio_claim_host(func);
+	r = sdio_claim_irq(func, uf_glue_sdio_int_handler);
+	sdio_release_host(func);
 
-    /* If the interrupt was installed earlier, is fine */
-    if (r == -EBUSY) {
-        r = 0;
-    }
+	/* If the interrupt was installed earlier, is fine */
+	if (r == -EBUSY)
+		r = 0;
 
-    return r;
+	return r;
 } /* csr_sdio_linux_install_irq() */
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
 
 /*
@@ -1023,7 +1013,6 @@ uf_sdio_mmc_power_event(struct notifier_block *this, unsigned long event, void *
 }
 
 #endif /* CONFIG_PM */
-#endif /* 2.6.32 */
 
 /*
  * ---------------------------------------------------------------------------
@@ -1050,10 +1039,8 @@ uf_glue_sdio_probe(struct sdio_func *func,
     /* First of all claim the SDIO driver */
     sdio_claim_host(func);
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
     /* Assume that the card is already powered */
     card_is_powered = 1;
-#endif
 
     /* Assumes one card per host, which is true for SDIO */
     instance = func->card->host->index;
@@ -1093,14 +1080,12 @@ uf_glue_sdio_probe(struct sdio_func *func,
     /* Pass context to the SDIO driver */
     sdio_set_drvdata(func, sdio_ctx);
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
     /* Register to get PM events */
     if (uf_sdio_mmc_register_pm_notifier(sdio_ctx) == NULL) {
         unifi_error(NULL, "%s: Failed to register for PM events\n", __FUNCTION__);
     }
 #endif
-#endif
 
     /* Register this device with the SDIO function driver */
     /* Call the main UniFi driver inserted handler */
@@ -1156,12 +1141,10 @@ uf_glue_sdio_remove(struct sdio_func *func)
         sdio_func_drv->removed(sdio_ctx);
     }
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
     /* Unregister for PM events */
     uf_sdio_mmc_unregister_pm_notifier(sdio_ctx);
 #endif
-#endif
 
     kfree(sdio_ctx);
 
@@ -1182,7 +1165,6 @@ static const struct sdio_device_id unifi_ids[] = {
 
 MODULE_DEVICE_TABLE(sdio, unifi_ids);
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
 
 /*
@@ -1252,16 +1234,13 @@ static struct dev_pm_ops unifi_pm_ops = {
 #define UNIFI_PM_OPS  NULL
 
 #endif /* CONFIG_PM */
-#endif /* 2.6.32 */
 
 static struct sdio_driver unifi_driver = {
     .probe      = uf_glue_sdio_probe,
     .remove     = uf_glue_sdio_remove,
     .name       = "unifi",
     .id_table	= unifi_ids,
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
     .drv.pm     = UNIFI_PM_OPS,
-#endif /* 2.6.32 */
 };
 
 
@@ -1305,12 +1284,10 @@ CsrSdioFunctionDriverRegister(CsrSdioFunctionDriver *sdio_drv)
      */
     sdio_func_drv = sdio_drv;
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 #ifdef CONFIG_PM
     /* Initialise PM notifier list */
     INIT_LIST_HEAD(&uf_sdio_mmc_pm_notifiers.list);
 #endif
-#endif
 
     /* Register ourself with mmc_core */
     r = sdio_register_driver(&unifi_driver);
diff --git a/drivers/staging/csr/sme_blocking.c b/drivers/staging/csr/sme_blocking.c
index acf0f0fe3b37..543e8f2c407a 100644
--- a/drivers/staging/csr/sme_blocking.c
+++ b/drivers/staging/csr/sme_blocking.c
@@ -18,10 +18,10 @@
 
 
 /*
- * This file also contains the implementation of the asyncronous
+ * This file also contains the implementation of the asynchronous
  * requests to the SME.
  *
- * Before calling an asyncronous SME function, we call sme_init_request()
+ * Before calling an asynchronous SME function, we call sme_init_request()
  * which gets hold of the SME semaphore and updates the request status.
  * The semaphore makes sure that there is only one pending request to
  * the SME at a time.
diff --git a/drivers/staging/csr/sme_native.c b/drivers/staging/csr/sme_native.c
index 229268fd746c..d7a5125d9a8e 100644
--- a/drivers/staging/csr/sme_native.c
+++ b/drivers/staging/csr/sme_native.c
@@ -12,7 +12,7 @@
  */
 
 #include <linux/netdevice.h>
-
+#include <linux/version.h>
 #include "unifi_priv.h"
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_conversions.h"
@@ -24,11 +24,7 @@ uf_sme_init(unifi_priv_t *priv)
 {
     func_enter();
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
     sema_init(&priv->mlme_blocking_mutex, 1);
-#else
-    init_MUTEX(&priv->mlme_blocking_mutex);
-#endif
 
 #ifdef CSR_SUPPORT_WEXT
     {
diff --git a/drivers/staging/csr/sme_sys.c b/drivers/staging/csr/sme_sys.c
index 99de27e678d2..5b26c41c01f6 100644
--- a/drivers/staging/csr/sme_sys.c
+++ b/drivers/staging/csr/sme_sys.c
@@ -14,6 +14,7 @@
  * ---------------------------------------------------------------------------
  */
 
+#include <linux/version.h>
 #include "csr_wifi_hip_unifiversion.h"
 #include "unifi_priv.h"
 #include "csr_wifi_hip_conversions.h"
@@ -21,7 +22,6 @@
 #include "csr_wifi_sme_sef.h"
 #endif
 
-
 /*
  * This file implements the SME SYS API and contains the following functions:
  * CsrWifiRouterCtrlMediaStatusReqHandler()
@@ -192,7 +192,7 @@ void CsrWifiRouterCtrlMediaStatusReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
 #endif
                     unifi_trace(priv, UDBG1,
                                 "CsrWifiRouterCtrlMediaStatusReqHandler: AP/P2PGO setting netif_carrier_on\n");
-                    UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[req->interfaceTag]);
+                    netif_tx_wake_all_queues(priv->netdev[req->interfaceTag]);
                     break;
 
                 default:
@@ -226,7 +226,7 @@ void CsrWifiRouterCtrlMediaStatusReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
                     unifi_trace(priv, UDBG1,
                                 "CsrWifiRouterMediaStatusReqHandler: UnifiConnected && netif_carrier_on\n");
                     netif_carrier_on(priv->netdev[req->interfaceTag]);
-                    UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[req->interfaceTag]);
+                    netif_tx_wake_all_queues(priv->netdev[req->interfaceTag]);
                     uf_process_rx_pending_queue(priv, UF_UNCONTROLLED_PORT_Q, broadcast_address, 1, interfacePriv->InterfaceTag);
                     uf_process_rx_pending_queue(priv, UF_CONTROLLED_PORT_Q, broadcast_address, 1, interfacePriv->InterfaceTag);
                 }
@@ -869,7 +869,6 @@ wifi_off(unifi_priv_t *priv)
     unifi_trace(priv, UDBG1, "wifi_off\n");
 
     /* Destroy the Traffic Analysis Module */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,23)
     cancel_work_sync(&priv->ta_ind_work.task);
     cancel_work_sync(&priv->ta_sample_ind_work.task);
 #ifdef CSR_SUPPORT_WEXT
@@ -884,7 +883,6 @@ wifi_off(unifi_priv_t *priv)
             cancel_work_sync(&netpriv->send_m4_ready_task);
         }
     }
-#endif
     flush_workqueue(priv->unifi_workqueue);
 
     /* fw_init parameter can prevent power off UniFi, for debugging */
@@ -955,7 +953,7 @@ void CsrWifiRouterCtrlWifiOffReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
         netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
         if (interfacePriv->netdev_registered == 1) {
             netif_carrier_off(priv->netdev[i]);
-            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[i]);
+            netif_tx_stop_all_queues(priv->netdev[i]);
             interfacePriv->connected = UnifiConnectedUnknown;
         }
         interfacePriv->interfaceMode = 0;
@@ -2123,7 +2121,7 @@ static int peer_add_new_record(unifi_priv_t *priv,CsrWifiRouterCtrlPeerAddReq *r
             /* Allocate for the new station record , to avoid race condition would happen between ADD_PEER &
              * DEL_PEER the allocation made atomic memory rather than kernel memory
              */
-            newRecord = (CsrWifiRouterCtrlStaInfo_t *) kmalloc(sizeof(CsrWifiRouterCtrlStaInfo_t), GFP_ATOMIC);
+            newRecord = kmalloc(sizeof(CsrWifiRouterCtrlStaInfo_t), GFP_ATOMIC);
             if (!newRecord) {
                 unifi_error(priv, "failed to allocate the %d bytes of mem for station record\n",
                             sizeof(CsrWifiRouterCtrlStaInfo_t));
@@ -2815,12 +2813,11 @@ u8 blockack_session_start(unifi_priv_t *priv,
         }
 
         /* create and populate the new BA session structure */
-        ba_session_tx = kmalloc(sizeof(ba_session_tx_struct), GFP_KERNEL);
+        ba_session_tx = kzalloc(sizeof(ba_session_tx_struct), GFP_KERNEL);
         if (!ba_session_tx) {
             unifi_error(priv, "%s: kmalloc failed for ba_session_tx\n", __FUNCTION__);
             return FALSE;
         }
-        memset(ba_session_tx, 0, sizeof(ba_session_tx_struct));
 
         ba_session_tx->interfacePriv = interfacePriv;
         ba_session_tx->tID = tID;
@@ -2905,26 +2902,23 @@ u8 blockack_session_start(unifi_priv_t *priv,
             return FALSE;
         }
 
-        ba_session_rx = kmalloc(sizeof(ba_session_rx_struct), GFP_KERNEL);
+        ba_session_rx = kzalloc(sizeof(ba_session_rx_struct), GFP_KERNEL);
         if (!ba_session_rx) {
             unifi_error(priv, "%s: kmalloc failed for ba_session_rx\n", __FUNCTION__);
             return FALSE;
         }
-        memset(ba_session_rx, 0, sizeof(ba_session_rx_struct));
 
         ba_session_rx->wind_size = wind_size;
         ba_session_rx->start_sn = ba_session_rx->expected_sn = start_sn;
         ba_session_rx->trigger_ba_after_ssn = FALSE;
 
-        ba_session_rx->buffer = kmalloc(ba_session_rx->wind_size*sizeof(frame_desc_struct), GFP_KERNEL);
+        ba_session_rx->buffer = kzalloc(ba_session_rx->wind_size*sizeof(frame_desc_struct), GFP_KERNEL);
         if (!ba_session_rx->buffer) {
             kfree(ba_session_rx);
             unifi_error(priv, "%s: kmalloc failed for buffer\n", __FUNCTION__);
             return FALSE;
         }
 
-        memset(ba_session_rx->buffer, 0, ba_session_rx->wind_size*sizeof(frame_desc_struct));
-
         INIT_WORK(&ba_session_rx->send_ba_err_task, uf_send_ba_err_wq);
         if (timeout) {
             ba_session_rx->timeout = timeout;
diff --git a/drivers/staging/csr/sme_wext.c b/drivers/staging/csr/sme_wext.c
index 7e85907e29a3..b58c0c6b171c 100644
--- a/drivers/staging/csr/sme_wext.c
+++ b/drivers/staging/csr/sme_wext.c
@@ -1191,8 +1191,6 @@ unifi_siwap(struct net_device *dev, struct iw_request_info *info,
     netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
     unifi_priv_t *priv = interfacePriv->privPtr;
     int err = 0;
-    const unsigned char zero_bssid[ETH_ALEN] = {0x00, 0x00, 0x00,
-        0x00, 0x00, 0x00};
 
     func_enter();
 
@@ -1213,7 +1211,7 @@ unifi_siwap(struct net_device *dev, struct iw_request_info *info,
 	unifi_trace(priv, UDBG1, "unifi_siwap: asked for %pM\n",
 		wrqu->ap_addr.sa_data);
 
-    if (!memcmp(wrqu->ap_addr.sa_data, zero_bssid, ETH_ALEN)) {
+    if (is_zero_ether_addr(wrqu->ap_addr.sa_data)) {
         priv->ignore_bssid_join = FALSE;
         err = sme_mgt_disconnect(priv);
         if (err) {
@@ -3043,8 +3041,8 @@ _unifi_siwencodeext(struct net_device *dev, struct iw_request_info *info,
     memcpy(sme_key.address.a, ext->addr.sa_data, ETH_ALEN);
     if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
 
-        unifi_trace(priv, UDBG5, "RSC first 6 bytes = %02X:%02X:%02X:%02X:%02X:%02X\n",
-                    ext->rx_seq[0], ext->rx_seq[1], ext->rx_seq[2], ext->rx_seq[3], ext->rx_seq[4], ext->rx_seq[5]);
+		unifi_trace(priv, UDBG5, "RSC first 6 bytes = %*phC\n",
+					 6, ext->rx_seq);
 
         /* memcpy((u8*)(&sme_key.keyRsc), ext->rx_seq, 8); */
         sme_key.keyRsc[0] = ext->rx_seq[1] << 8 | ext->rx_seq[0];
diff --git a/drivers/staging/csr/ul_int.c b/drivers/staging/csr/ul_int.c
index 46d3507fd8f1..4013d021ebbf 100644
--- a/drivers/staging/csr/ul_int.c
+++ b/drivers/staging/csr/ul_int.c
@@ -12,6 +12,7 @@
  *
  * ***************************************************************************
  */
+#include <linux/version.h>
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_conversions.h"
 #include "unifi_priv.h"
@@ -44,11 +45,7 @@ ul_init_clients(unifi_priv_t *priv)
     int id;
     ul_client_t *ul_clients;
 
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
     sema_init(&priv->udi_logging_mutex, 1);
-#else
-    init_MUTEX(&priv->udi_logging_mutex);
-#endif
     priv->logging_client = NULL;
 
     ul_clients = priv->ul_clients;
diff --git a/drivers/staging/csr/unifi_pdu_processing.c b/drivers/staging/csr/unifi_pdu_processing.c
index 7c7e8d49ae42..ae7c8fc94092 100644
--- a/drivers/staging/csr/unifi_pdu_processing.c
+++ b/drivers/staging/csr/unifi_pdu_processing.c
@@ -14,7 +14,7 @@
  * ---------------------------------------------------------------------------
  */
 
-
+#include <linux/version.h>
 #include <linux/types.h>
 #include <linux/etherdevice.h>
 #include <linux/vmalloc.h>
@@ -23,9 +23,6 @@
 #include "csr_wifi_hip_conversions.h"
 #include "csr_time.h"
 #include "unifi_priv.h"
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
-#include <net/iw_handler.h>
-#endif
 #include <net/pkt_sched.h>
 
 #ifdef CSR_SUPPORT_SME
diff --git a/drivers/staging/csr/unifi_priv.h b/drivers/staging/csr/unifi_priv.h
index 6d6b46191a1a..aec8e28fb60d 100644
--- a/drivers/staging/csr/unifi_priv.h
+++ b/drivers/staging/csr/unifi_priv.h
@@ -29,9 +29,7 @@
 #include <linux/wireless.h>
 #include <linux/cdev.h>
 #include <linux/kthread.h>
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,19)
 #include <linux/freezer.h>
-#endif
 
 #ifdef CSR_WIFI_SUPPORT_MMC_DRIVER
 #include <linux/mmc/core.h>
@@ -73,33 +71,6 @@ extern struct wake_lock unifi_sdio_wake_lock;
 
 #include "unifi_clients.h"
 
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
-#include <linux/workqueue.h>
-
-#undef INIT_WORK
-#define INIT_WORK(_work, _func)                                         \
-    do {                                                                \
-        INIT_LIST_HEAD(&(_work)->entry);                                \
-        (_work)->pending = 0;                                           \
-        PREPARE_WORK((_work), (_func), (_work));                        \
-        init_timer(&(_work)->timer);                                    \
-    } while(0)
-
-#endif  /* Linux kernel < 2.6.20 */
-
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
-#define UF_NETIF_TX_WAKE_ALL_QUEUES(_netdev)    netif_tx_wake_all_queues(_netdev)
-#define UF_NETIF_TX_START_ALL_QUEUES(_netdev)   netif_tx_start_all_queues(_netdev)
-#define UF_NETIF_TX_STOP_ALL_QUEUES(_netdev)    netif_tx_stop_all_queues(_netdev)
-#else
-#define UF_NETIF_TX_WAKE_ALL_QUEUES(_netdev)    netif_wake_queue(_netdev)
-#define UF_NETIF_TX_START_ALL_QUEUES(_netdev)   netif_start_queue(_netdev)
-#define UF_NETIF_TX_STOP_ALL_QUEUES(_netdev)    netif_stop_queue(_netdev)
-#endif  /* Linux kernel >= 2.6.27 */
-
-
 #ifdef CSR_NATIVE_LINUX
 #include "sme_native/unifi_native.h"
 #else
@@ -634,12 +605,10 @@ struct unifi_priv {
     spinlock_t wapi_lock;
 #endif
 
-#ifndef ALLOW_Q_PAUSE
     /* Array to indicate if a particular Tx queue is paused, this may not be
      * required in a multiqueue implementation since we can directly stop kernel
      * queues */
     u8 tx_q_paused_flag[UNIFI_TRAFFIC_Q_MAX];
-#endif
 
 #ifdef CSR_WIFI_RX_PATH_SPLIT
     struct workqueue_struct *rx_workqueue;
@@ -798,12 +767,6 @@ typedef struct netInterface_priv
     u8 bcTimSetReqQueued;
 } netInterface_priv_t;
 
-#ifndef ALLOW_Q_PAUSE
-#define net_is_tx_q_paused(priv, q)   (priv->tx_q_paused_flag[q])
-#define net_tx_q_unpause(priv, q)   (priv->tx_q_paused_flag[q] = 0)
-#define net_tx_q_pause(priv, q)   (priv->tx_q_paused_flag[q] = 1)
-#endif
-
 #ifdef CSR_SUPPORT_SME
 #define routerStartBuffering(priv,queue) priv->routerBufferEnable[(queue)] = TRUE;
 #define routerStopBuffering(priv,queue) priv->routerBufferEnable[(queue)]  = FALSE;
@@ -1088,9 +1051,6 @@ CsrWifiRouterCtrlStaInfo_t * CsrWifiRouterCtrlGetStationRecordFromHandle(unifi_p
 void uf_update_sta_activity(unifi_priv_t *priv, u16 interfaceTag, const u8 *peerMacAddress);
 void uf_process_ma_pkt_cfm_for_ap(unifi_priv_t *priv,u16 interfaceTag, const CSR_MA_PACKET_CONFIRM *pkt_cfm);
 #endif
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
-int uf_install_qdisc(struct net_device *dev);
-#endif
 
 void uf_resume_data_plane(unifi_priv_t *priv, int queue,
                           CsrWifiMacAddress peer_address,
diff --git a/drivers/staging/csr/unifi_wext.h b/drivers/staging/csr/unifi_wext.h
index 6d7a99595083..6834c43abfbb 100644
--- a/drivers/staging/csr/unifi_wext.h
+++ b/drivers/staging/csr/unifi_wext.h
@@ -16,6 +16,7 @@
 #define __LINUX_UNIFI_WEXT_H__ 1
 
 #include <linux/kernel.h>
+#include <linux/version.h>
 #include <net/iw_handler.h>
 #include "csr_wifi_sme_prim.h"
 
@@ -70,15 +71,9 @@ uf_iwe_stream_add_point(struct iw_request_info *info, char *start, char *stop,
 {
     char *new_start;
 
-    new_start = iwe_stream_add_point(
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined (IW_REQUEST_FLAG_COMPAT)
-                                     info,
-#endif
-                                     start, stop, piwe, extra);
+    new_start = iwe_stream_add_point(info, start, stop, piwe, extra);
     if (unlikely(new_start == start))
-    {
         return -E2BIG;
-    }
 
     return (new_start - start);
 }
@@ -90,14 +85,9 @@ uf_iwe_stream_add_event(struct iw_request_info *info, char *start, char *stop,
 {
     char *new_start;
 
-    new_start = iwe_stream_add_event(
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined(IW_REQUEST_FLAG_COMPAT)
-                                     info,
-#endif
-                                     start, stop, piwe, len);
-    if (unlikely(new_start == start)) {
+    new_start = iwe_stream_add_event(info, start, stop, piwe, len);
+    if (unlikely(new_start == start))
         return -E2BIG;
-    }
 
     return (new_start - start);
 }
@@ -108,14 +98,9 @@ uf_iwe_stream_add_value(struct iw_request_info *info, char *stream, char *start,
 {
     char *new_start;
 
-    new_start = iwe_stream_add_value(
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined(IW_REQUEST_FLAG_COMPAT)
-                                     info,
-#endif
-                                     stream, start, stop, piwe, len);
-    if (unlikely(new_start == start)) {
+    new_start = iwe_stream_add_value(info, stream, start, stop, piwe, len);
+    if (unlikely(new_start == start))
         return -E2BIG;
-    }
 
     return (new_start - start);
 }
diff --git a/drivers/staging/csr/wext_events.c b/drivers/staging/csr/wext_events.c
index d356887ac4c6..9860ea30da25 100644
--- a/drivers/staging/csr/wext_events.c
+++ b/drivers/staging/csr/wext_events.c
@@ -194,11 +194,9 @@ _send_michaelmicfailure_event(struct net_device *dev,
     union iwreq_data wrqu;
     char buf[128];
 
-    sprintf(buf,
-            "MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr=%02x:%02x:%02x:%02x:%02x:%02x)",
-            key_idx, (key_type == CSR_GROUP) ? "broad" : "uni",
-            macaddr[0], macaddr[1], macaddr[2],
-            macaddr[3], macaddr[4], macaddr[5]);
+	sprintf(buf,
+		"MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr=%pM)",
+		key_idx, (key_type == CSR_GROUP) ? "broad" : "uni", macaddr);
     memset(&wrqu, 0, sizeof(wrqu));
     wrqu.data.length = strlen(buf);
     wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
diff --git a/drivers/staging/cxt1e1/linux.c b/drivers/staging/cxt1e1/linux.c
index 911c0e4375fd..0ff2865edec8 100644
--- a/drivers/staging/cxt1e1/linux.c
+++ b/drivers/staging/cxt1e1/linux.c
@@ -405,7 +405,7 @@ c4_linux_xmit (struct sk_buff * skb, struct net_device * ndev)
     priv = hdlc->priv;
 
     rval = musycc_start_xmit (priv->ci, priv->channum, skb);
-    return -rval;
+    return rval;
 }
 
 static const struct net_device_ops chan_ops = {
@@ -1169,11 +1169,11 @@ cleanup_hdlc (void)
 STATIC void __exit
 c4_mod_remove (void)
 {
-    cleanup_hdlc ();            /* delete any missed channels */
-    cleanup_devs ();
-    c4_cleanup ();
-    cleanup_ioremap ();
-    pr_info("SBE - driver removed.\n");
+	cleanup_hdlc();            /* delete any missed channels */
+	cleanup_devs();
+	c4_cleanup();
+	cleanup_ioremap();
+	pr_info("SBE - driver removed.\n");
 }
 
 module_init (c4_mod_init);
diff --git a/drivers/staging/cxt1e1/musycc.c b/drivers/staging/cxt1e1/musycc.c
index 90c0f1e30f65..ba721c604061 100644
--- a/drivers/staging/cxt1e1/musycc.c
+++ b/drivers/staging/cxt1e1/musycc.c
@@ -1761,15 +1761,15 @@ musycc_start_xmit (ci_t * ci, int channum, void *mem_token)
     u_int32_t   len;
 
     if (!(ch = sd_find_chan (ci, channum)))
-        return ENOENT;
+        return -ENOENT;
 
     if (ci->state != C_RUNNING)     /* full interrupt processing available */
-        return EINVAL;
+        return -EINVAL;
     if (ch->state != UP)
-        return EINVAL;
+        return -EINVAL;
 
     if (!(ch->status & TX_ENABLED))
-        return EROFS;               /* how else to flag unwritable state ? */
+        return -EROFS;               /* how else to flag unwritable state ? */
 
 #ifdef RLD_TRANS_DEBUGx
     if (1 || cxt1e1_log_level >= LOG_MONITOR2)
@@ -1836,7 +1836,7 @@ musycc_start_xmit (ci_t * ci, int channum, void *mem_token)
 #if 0
         spin_unlock_irqrestore (&ch->ch_txlock, flags);
 #endif
-        return EBUSY;               /* tell user to try again later */
+        return -EBUSY;               /* tell user to try again later */
     }
     /**************************************************/
     /** Put the user data into MUSYCC data buffer(s) **/
diff --git a/drivers/staging/dgrp/Kconfig b/drivers/staging/dgrp/Kconfig
new file mode 100644
index 000000000000..39f4bb65ec83
--- /dev/null
+++ b/drivers/staging/dgrp/Kconfig
@@ -0,0 +1,9 @@
+config DGRP
+       tristate "Digi Realport driver"
+       default n
+       depends on SYSFS
+       ---help---
+       Support for Digi Realport devices.  These devices allow you to
+       access remote serial ports as if they are local tty devices.  This
+       will build the kernel driver, you will still need the userspace
+       component to make your Realport device work.
diff --git a/drivers/staging/dgrp/Makefile b/drivers/staging/dgrp/Makefile
new file mode 100644
index 000000000000..d9c3b88d7162
--- /dev/null
+++ b/drivers/staging/dgrp/Makefile
@@ -0,0 +1,12 @@
+obj-$(CONFIG_DGRP) += dgrp.o
+
+dgrp-y := 			\
+	dgrp_common.o 		\
+	dgrp_dpa_ops.o 		\
+	dgrp_driver.o 		\
+	dgrp_mon_ops.o 	 	\
+	dgrp_net_ops.o 		\
+	dgrp_ports_ops.o 	\
+	dgrp_specproc.o 	\
+	dgrp_tty.o 		\
+	dgrp_sysfs.o
diff --git a/drivers/staging/dgrp/README b/drivers/staging/dgrp/README
new file mode 100644
index 000000000000..1d8aaee270e8
--- /dev/null
+++ b/drivers/staging/dgrp/README
@@ -0,0 +1,2 @@
+The user space code to work with this driver is located at
+https://github.com/wfp5p/dgrp-utils
diff --git a/drivers/staging/dgrp/TODO b/drivers/staging/dgrp/TODO
new file mode 100644
index 000000000000..3ef2611bc6d7
--- /dev/null
+++ b/drivers/staging/dgrp/TODO
@@ -0,0 +1,13 @@
+- Use configfs for config stuff.  This will require changes to the
+  user space code.
+
+- dgrp_send() and dgrp_receive() could use some refactoring
+
+- Don't automatically create CHAN_MAX (64) channel array entries for
+  every device as many devices are going to have much less than 64
+  channels.
+
+- The locking needs to be checked.  It seems haphazardly done in most
+  places.
+
+- Check Kconfig dependencies
diff --git a/drivers/staging/dgrp/dgrp_common.c b/drivers/staging/dgrp/dgrp_common.c
new file mode 100644
index 000000000000..fce74e7fb834
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_common.c
@@ -0,0 +1,200 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_common.c
+ *
+ *  Description:
+ *
+ *     Definitions of global variables and functions which are either
+ *     shared by the tty, mon, and net drivers; or which cross them
+ *     functionally (like the poller).
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+#include <linux/cred.h>
+
+#include "dgrp_common.h"
+
+/**
+ * dgrp_carrier -- check for carrier change state and act
+ * @ch: struct ch_struct *
+ */
+void dgrp_carrier(struct ch_struct *ch)
+{
+	struct nd_struct *nd;
+
+	int virt_carrier = 0;
+	int phys_carrier = 0;
+
+	/* fix case when the tty has already closed. */
+
+	if (!ch)
+		return;
+	nd  = ch->ch_nd;
+	if (!nd)
+		return;
+
+	/*
+	 *  If we are currently waiting to determine the status of the port,
+	 *  we don't yet know the state of the modem lines.  As a result,
+	 *  we ignore state changes when we are waiting for the modem lines
+	 *  to be established.  We know, as a result of code in dgrp_net_ops,
+	 *  that we will be called again immediately following the reception
+	 *  of the status message with the true modem status flags in it.
+	 */
+	if (ch->ch_expect & RR_STATUS)
+		return;
+
+	/*
+	 * If CH_HANGUP is set, we gotta keep trying to get all the processes
+	 * that have the port open to close the port.
+	 * So lets just keep sending a hangup every time we get here.
+	 */
+	if ((ch->ch_flag & CH_HANGUP) &&
+	    (ch->ch_tun.un_open_count > 0))
+		tty_hangup(ch->ch_tun.un_tty);
+
+	/*
+	 *  Compute the effective state of both the physical and virtual
+	 *  senses of carrier.
+	 */
+
+	if (ch->ch_s_mlast & DM_CD)
+		phys_carrier = 1;
+
+	if ((ch->ch_s_mlast & DM_CD) ||
+	    (ch->ch_digi.digi_flags & DIGI_FORCEDCD) ||
+	    (ch->ch_flag & CH_CLOCAL))
+		virt_carrier = 1;
+
+	/*
+	 *  Test for a VIRTUAL carrier transition to HIGH.
+	 *
+	 *  The CH_HANGUP condition is intended to prevent any action
+	 *  except for close.  As a result, we ignore positive carrier
+	 *  transitions during CH_HANGUP.
+	 */
+	if (((ch->ch_flag & CH_HANGUP)  == 0) &&
+	    ((ch->ch_flag & CH_VIRT_CD) == 0) &&
+	    (virt_carrier == 1)) {
+		/*
+		 * When carrier rises, wake any threads waiting
+		 * for carrier in the open routine.
+		 */
+		nd->nd_tx_work = 1;
+
+		if (waitqueue_active(&ch->ch_flag_wait))
+			wake_up_interruptible(&ch->ch_flag_wait);
+	}
+
+	/*
+	 *  Test for a PHYSICAL transition to low, so long as we aren't
+	 *  currently ignoring physical transitions (which is what "virtual
+	 *  carrier" indicates).
+	 *
+	 *  The transition of the virtual carrier to low really doesn't
+	 *  matter... it really only means "ignore carrier state", not
+	 *  "make pretend that carrier is there".
+	 */
+	if ((virt_carrier == 0) &&
+	    ((ch->ch_flag & CH_PHYS_CD) != 0) &&
+	    (phys_carrier == 0)) {
+		/*
+		 * When carrier drops:
+		 *
+		 *   Do a Hard Hangup if that is called for.
+		 *
+		 *   Drop carrier on all open units.
+		 *
+		 *   Flush queues, waking up any task waiting in the
+		 *   line discipline.
+		 *
+		 *   Send a hangup to the control terminal.
+		 *
+		 *   Enable all select calls.
+		 */
+
+		nd->nd_tx_work = 1;
+
+		ch->ch_flag &= ~(CH_LOW | CH_EMPTY | CH_DRAIN | CH_INPUT);
+
+		if (waitqueue_active(&ch->ch_flag_wait))
+			wake_up_interruptible(&ch->ch_flag_wait);
+
+		if (ch->ch_tun.un_open_count > 0)
+			tty_hangup(ch->ch_tun.un_tty);
+
+		if (ch->ch_pun.un_open_count > 0)
+			tty_hangup(ch->ch_pun.un_tty);
+	}
+
+	/*
+	 *  Make sure that our cached values reflect the current reality.
+	 */
+	if (virt_carrier == 1)
+		ch->ch_flag |= CH_VIRT_CD;
+	else
+		ch->ch_flag &= ~CH_VIRT_CD;
+
+	if (phys_carrier == 1)
+		ch->ch_flag |= CH_PHYS_CD;
+	else
+		ch->ch_flag &= ~CH_PHYS_CD;
+
+}
+
+/**
+ * dgrp_chk_perm() -- check permissions for net device
+ * @inode: pointer to inode structure for the net communication device
+ * @op: operation to be tested
+ *
+ * The file permissions and ownerships are tested to determine whether
+ * the operation "op" is permitted on the file pointed to by the inode.
+ * Returns 0 if the operation is permitted, -EACCESS otherwise
+ */
+int dgrp_chk_perm(int mode, int op)
+{
+	if (!current_euid())
+		mode >>= 6;
+	else if (in_egroup_p(0))
+		mode >>= 3;
+
+	if ((mode & op & 0007) == op)
+		return 0;
+
+	if (capable(CAP_SYS_ADMIN))
+		return 0;
+
+	return -EACCES;
+}
+
+/* dgrp_chk_perm wrapper for permission call in struct inode_operations */
+int dgrp_inode_permission(struct inode *inode, int op)
+{
+	return dgrp_chk_perm(inode->i_mode, op);
+}
diff --git a/drivers/staging/dgrp/dgrp_common.h b/drivers/staging/dgrp/dgrp_common.h
new file mode 100644
index 000000000000..05ff338471ac
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_common.h
@@ -0,0 +1,208 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo <jamesp 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.
+ *
+ */
+
+#ifndef __DGRP_COMMON_H
+#define __DGRP_COMMON_H
+
+#define DIGI_VERSION "1.9-29"
+
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include "drp.h"
+
+#define DGRP_TTIME 100
+#define DGRP_RTIME 100
+
+/************************************************************************
+ * All global storage allocation.
+ ************************************************************************/
+
+extern int dgrp_rawreadok;  /* Allow raw writing of input */
+extern int dgrp_register_cudevices; /* enable legacy cu devices */
+extern int dgrp_register_prdevices; /* enable transparent print devices */
+extern int dgrp_poll_tick;          /* Poll interval - in ms */
+
+extern struct list_head nd_struct_list;
+
+struct dgrp_poll_data {
+	spinlock_t poll_lock;
+	struct timer_list timer;
+	int poll_tick;
+	ulong poll_round;	/* Timer rouding factor */
+	long node_active_count;
+};
+
+extern struct dgrp_poll_data dgrp_poll_data;
+extern void dgrp_poll_handler(unsigned long arg);
+
+/* from dgrp_mon_ops.c */
+extern void dgrp_register_mon_hook(struct proc_dir_entry *de);
+
+/* from dgrp_tty.c */
+extern int dgrp_tty_init(struct nd_struct *nd);
+extern void dgrp_tty_uninit(struct nd_struct *nd);
+
+/* from dgrp_ports_ops.c */
+extern void dgrp_register_ports_hook(struct proc_dir_entry *de);
+
+/* from dgrp_net_ops.c */
+extern void dgrp_register_net_hook(struct proc_dir_entry *de);
+
+/* from dgrp_dpa_ops.c */
+extern void dgrp_register_dpa_hook(struct proc_dir_entry *de);
+extern void dgrp_dpa_data(struct nd_struct *, int, u8 *, int);
+
+/* from dgrp_sysfs.c */
+extern void dgrp_create_class_sysfs_files(void);
+extern void dgrp_remove_class_sysfs_files(void);
+
+extern void dgrp_create_node_class_sysfs_files(struct nd_struct *nd);
+extern void dgrp_remove_node_class_sysfs_files(struct nd_struct *nd);
+
+extern void dgrp_create_tty_sysfs(struct un_struct *un, struct device *c);
+extern void dgrp_remove_tty_sysfs(struct device *c);
+
+/* from dgrp_specproc.c */
+/*
+ *  The list of DGRP entries with r/w capabilities.  These
+ *  magic numbers are used for identification purposes.
+ */
+enum {
+	DGRP_CONFIG = 1,	/* Configure portservers */
+	DGRP_NETDIR = 2,	/* Directory for "net" devices */
+	DGRP_MONDIR = 3,	/* Directory for "mon" devices */
+	DGRP_PORTSDIR = 4,	/* Directory for "ports" devices */
+	DGRP_INFO = 5,		/* Get info. about the running module */
+	DGRP_NODEINFO = 6,	/* Get info. about the configured nodes */
+	DGRP_DPADIR = 7,	/* Directory for the "dpa" devices */
+};
+
+/*
+ *  Directions for proc handlers
+ */
+enum {
+	INBOUND = 1,		/* Data being written to kernel */
+	OUTBOUND = 2,		/* Data being read from the kernel */
+};
+
+/**
+ * dgrp_proc_entry: structure for dgrp proc dirs
+ * @id: ID number associated with this particular entry.  Should be
+ *    unique across all of DGRP.
+ * @name: text name associated with the /proc entry
+ * @mode: file access permisssions for the /proc entry
+ * @child: pointer to table describing a subdirectory for this entry
+ * @de: pointer to directory entry for this object once registered.  Used
+ *    to grab the handle of the object for unregistration
+ * @excl_sem: semaphore to provide exclusive to struct
+ * @excl_cnt: counter of current accesses
+ *
+ *  Each entry in a DGRP proc directory is described with a
+ *  dgrp_proc_entry structure.  A collection of these
+ *  entries (in an array) represents the members associated
+ *  with a particular /proc directory, and is referred to
+ *  as a table.  All tables are terminated by an entry with
+ *  zeros for every member.
+ */
+struct dgrp_proc_entry {
+	int                  id;          /* Integer identifier */
+	const char        *name;          /* ASCII identifier */
+	mode_t             mode;          /* File access permissions */
+	struct dgrp_proc_entry *child;    /* Child pointer */
+
+	/* file ops to use, pass NULL to use default */
+	struct file_operations *proc_file_ops;
+
+	struct proc_dir_entry *de;        /* proc entry pointer */
+	struct semaphore   excl_sem;      /* Protects exclusive access var */
+	int                excl_cnt;      /* Counts number of curr accesses */
+};
+
+extern void dgrp_unregister_proc(void);
+extern void dgrp_register_proc(void);
+
+/*-----------------------------------------------------------------------*
+ *
+ *  Declarations for common operations:
+ *
+ *      (either used by more than one of net, mon, or tty,
+ *       or in interrupt context (i.e. the poller))
+ *
+ *-----------------------------------------------------------------------*/
+
+void dgrp_carrier(struct ch_struct *ch);
+extern int dgrp_inode_permission(struct inode *inode, int op);
+extern int dgrp_chk_perm(int mode, int op);
+
+
+/*
+ *  ID manipulation macros (where c1 & c2 are characters, i is
+ *  a long integer, and s is a character array of at least three members
+ */
+
+static inline void ID_TO_CHAR(long i, char *s)
+{
+	s[0] = ((i & 0xff00)>>8);
+	s[1] = (i & 0xff);
+	s[2] = 0;
+}
+
+static inline long CHAR_TO_ID(char *s)
+{
+	return ((s[0] & 0xff) << 8) | (s[1] & 0xff);
+}
+
+static inline struct nd_struct *nd_struct_get(long major)
+{
+	struct nd_struct *nd;
+
+	list_for_each_entry(nd, &nd_struct_list, list) {
+		if (major == nd->nd_major)
+			return nd;
+	}
+
+	return NULL;
+}
+
+static inline int nd_struct_add(struct nd_struct *entry)
+{
+	struct nd_struct *ptr;
+
+	ptr = nd_struct_get(entry->nd_major);
+
+	if (ptr)
+		return -EBUSY;
+
+	list_add_tail(&entry->list, &nd_struct_list);
+
+	return 0;
+}
+
+static inline int nd_struct_del(struct nd_struct *entry)
+{
+	struct nd_struct *nd;
+
+	nd = nd_struct_get(entry->nd_major);
+
+	if (!nd)
+		return -ENODEV;
+
+	list_del(&nd->list);
+	return 0;
+}
+
+#endif /* __DGRP_COMMON_H */
diff --git a/drivers/staging/dgrp/dgrp_dpa_ops.c b/drivers/staging/dgrp/dgrp_dpa_ops.c
new file mode 100644
index 000000000000..49e670915e5c
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_dpa_ops.c
@@ -0,0 +1,556 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_dpa_ops.c
+ *
+ *  Description:
+ *
+ *     Handle the file operations required for the "dpa" devices.
+ *     Includes those functions required to register the "dpa" devices
+ *     in "/proc".
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/proc_fs.h>
+#include <linux/tty.h>
+#include <linux/poll.h>
+#include <linux/cred.h>
+#include <linux/sched.h>
+#include <linux/ratelimit.h>
+#include <asm/unaligned.h>
+
+#include "dgrp_common.h"
+
+/* File operation declarations */
+static int dgrp_dpa_open(struct inode *, struct file *);
+static int dgrp_dpa_release(struct inode *, struct file *);
+static ssize_t dgrp_dpa_read(struct file *, char __user *, size_t, loff_t *);
+static long dgrp_dpa_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg);
+static unsigned int dgrp_dpa_select(struct file *, struct poll_table_struct *);
+
+static const struct file_operations dpa_ops = {
+	.owner   =  THIS_MODULE,
+	.read    =  dgrp_dpa_read,
+	.poll    =  dgrp_dpa_select,
+	.unlocked_ioctl =  dgrp_dpa_ioctl,
+	.open    =  dgrp_dpa_open,
+	.release =  dgrp_dpa_release,
+};
+
+static struct inode_operations dpa_inode_ops = {
+	.permission = dgrp_inode_permission
+};
+
+
+
+struct digi_node {
+	uint	nd_state;		/* Node state: 1 = up, 0 = down. */
+	uint	nd_chan_count;		/* Number of channels found */
+	uint	nd_tx_byte;		/* Tx data count */
+	uint	nd_rx_byte;		/* RX data count */
+	u8	nd_ps_desc[MAX_DESC_LEN]; /* Description from PS */
+};
+
+#define DIGI_GETNODE      (('d'<<8) | 249)	/* get board info */
+
+
+struct digi_chan {
+	uint	ch_port;	/* Port number to get info on */
+	uint	ch_open;	/* 1 if open, 0 if not */
+	uint	ch_txcount;	/* TX data count  */
+	uint	ch_rxcount;	/* RX data count  */
+	uint	ch_s_brate;	/* Realport BRATE */
+	uint	ch_s_estat;	/* Realport ELAST */
+	uint	ch_s_cflag;	/* Realport CFLAG */
+	uint	ch_s_iflag;	/* Realport IFLAG */
+	uint	ch_s_oflag;	/* Realport OFLAG */
+	uint	ch_s_xflag;	/* Realport XFLAG */
+	uint	ch_s_mstat;	/* Realport MLAST */
+};
+
+#define DIGI_GETCHAN      (('d'<<8) | 248)	/* get channel info */
+
+
+struct digi_vpd {
+	int vpd_len;
+	char vpd_data[VPDSIZE];
+};
+
+#define DIGI_GETVPD       (('d'<<8) | 246)	/* get VPD info */
+
+
+struct digi_debug {
+	int onoff;
+	int port;
+};
+
+#define DIGI_SETDEBUG      (('d'<<8) | 247)	/* set debug info */
+
+
+void dgrp_register_dpa_hook(struct proc_dir_entry *de)
+{
+	struct nd_struct *node = de->data;
+
+	de->proc_iops = &dpa_inode_ops;
+	de->proc_fops = &dpa_ops;
+
+	node->nd_dpa_de = de;
+	spin_lock_init(&node->nd_dpa_lock);
+}
+
+/*
+ * dgrp_dpa_open -- open the DPA device for a particular PortServer
+ */
+static int dgrp_dpa_open(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+	int rtn = 0;
+
+	struct proc_dir_entry *de;
+
+	rtn = try_module_get(THIS_MODULE);
+	if (!rtn)
+		return -ENXIO;
+
+	rtn = 0;
+
+	if (!capable(CAP_SYS_ADMIN)) {
+		rtn = -EPERM;
+		goto done;
+	}
+
+	/*
+	 *  Make sure that the "private_data" field hasn't already been used.
+	 */
+	if (file->private_data) {
+		rtn = -EINVAL;
+		goto done;
+	}
+
+	/*
+	 *  Get the node pointer, and fail if it doesn't exist.
+	 */
+	de = PDE(inode);
+	if (!de) {
+		rtn = -ENXIO;
+		goto done;
+	}
+	nd = (struct nd_struct *)de->data;
+	if (!nd) {
+		rtn = -ENXIO;
+		goto done;
+	}
+
+	file->private_data = (void *) nd;
+
+	/*
+	 * Allocate the DPA buffer.
+	 */
+
+	if (nd->nd_dpa_buf) {
+		rtn = -EBUSY;
+	} else {
+		nd->nd_dpa_buf = kmalloc(DPA_MAX, GFP_KERNEL);
+
+		if (!nd->nd_dpa_buf) {
+			rtn = -ENOMEM;
+		} else {
+			nd->nd_dpa_out = 0;
+			nd->nd_dpa_in = 0;
+			nd->nd_dpa_lbolt = jiffies;
+		}
+	}
+
+done:
+
+	if (rtn)
+		module_put(THIS_MODULE);
+	return rtn;
+}
+
+/*
+ * dgrp_dpa_release -- close the DPA device for a particular PortServer
+ */
+static int dgrp_dpa_release(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+	u8 *buf;
+	unsigned long lock_flags;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		goto done;
+
+	/*
+	 *  Free the dpa buffer.
+	 */
+
+	spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+
+	buf = nd->nd_dpa_buf;
+
+	nd->nd_dpa_buf = NULL;
+	nd->nd_dpa_out = nd->nd_dpa_in;
+
+	/*
+	 *  Wakeup any thread waiting for buffer space.
+	 */
+
+	if (nd->nd_dpa_flag & DPA_WAIT_SPACE) {
+		nd->nd_dpa_flag &= ~DPA_WAIT_SPACE;
+		wake_up_interruptible(&nd->nd_dpa_wqueue);
+	}
+
+	spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+
+	kfree(buf);
+
+done:
+	module_put(THIS_MODULE);
+	file->private_data = NULL;
+	return 0;
+}
+
+/*
+ * dgrp_dpa_read
+ *
+ * Copy data from the monitoring buffer to the user, freeing space
+ * in the monitoring buffer for more messages
+ */
+static ssize_t dgrp_dpa_read(struct file *file, char __user *buf, size_t count,
+			     loff_t *ppos)
+{
+	struct nd_struct *nd;
+	int n;
+	int r;
+	int offset = 0;
+	int res = 0;
+	ssize_t rtn;
+	unsigned long lock_flags;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		return -ENXIO;
+
+	/*
+	 *  Wait for some data to appear in the buffer.
+	 */
+
+	spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+
+	for (;;) {
+		n = (nd->nd_dpa_in - nd->nd_dpa_out) & DPA_MASK;
+
+		if (n != 0)
+			break;
+
+		nd->nd_dpa_flag |= DPA_WAIT_DATA;
+
+		spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+
+		/*
+		 * Go to sleep waiting until the condition becomes true.
+		 */
+		rtn = wait_event_interruptible(nd->nd_dpa_wqueue,
+			((nd->nd_dpa_flag & DPA_WAIT_DATA) == 0));
+
+		if (rtn)
+			return rtn;
+
+		spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+	}
+
+	/*
+	 *  Read whatever is there.
+	 */
+
+	if (n > count)
+		n = count;
+
+	res = n;
+
+	r = DPA_MAX - nd->nd_dpa_out;
+
+	if (r <= n) {
+
+		spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+		rtn = copy_to_user((void __user *)buf,
+				   nd->nd_dpa_buf + nd->nd_dpa_out, r);
+		spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+
+		if (rtn) {
+			rtn = -EFAULT;
+			goto done;
+		}
+
+		nd->nd_dpa_out = 0;
+		n -= r;
+		offset = r;
+	}
+
+	spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+	rtn = copy_to_user((void __user *)buf + offset,
+			   nd->nd_dpa_buf + nd->nd_dpa_out, n);
+	spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+
+	if (rtn) {
+		rtn = -EFAULT;
+		goto done;
+	}
+
+	nd->nd_dpa_out += n;
+
+	*ppos += res;
+
+	rtn = res;
+
+	/*
+	 *  Wakeup any thread waiting for buffer space.
+	 */
+
+	n = (nd->nd_dpa_in - nd->nd_dpa_out) & DPA_MASK;
+
+	if (nd->nd_dpa_flag & DPA_WAIT_SPACE &&
+	    (DPA_MAX - n) > DPA_HIGH_WATER) {
+		nd->nd_dpa_flag &= ~DPA_WAIT_SPACE;
+		wake_up_interruptible(&nd->nd_dpa_wqueue);
+	}
+
+ done:
+	spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+	return rtn;
+}
+
+static unsigned int dgrp_dpa_select(struct file *file,
+				    struct poll_table_struct *table)
+{
+	unsigned int retval = 0;
+	struct nd_struct *nd = file->private_data;
+
+	if (nd->nd_dpa_out != nd->nd_dpa_in)
+		retval |= POLLIN | POLLRDNORM; /* Conditionally readable */
+
+	retval |= POLLOUT | POLLWRNORM;        /* Always writeable */
+
+	return retval;
+}
+
+static long dgrp_dpa_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg)
+{
+
+	struct nd_struct  *nd;
+	struct digi_chan getchan;
+	struct digi_node getnode;
+	struct ch_struct *ch;
+	struct digi_debug setdebug;
+	struct digi_vpd vpd;
+	unsigned int port;
+	void __user *uarg = (void __user *) arg;
+
+	nd = file->private_data;
+
+	switch (cmd) {
+	case DIGI_GETCHAN:
+		if (copy_from_user(&getchan, uarg, sizeof(struct digi_chan)))
+			return -EFAULT;
+
+		port = getchan.ch_port;
+
+		if (port < 0 || port > nd->nd_chan_count)
+			return -EINVAL;
+
+		ch = nd->nd_chan + port;
+
+		getchan.ch_open = (ch->ch_open_count > 0) ? 1 : 0;
+		getchan.ch_txcount = ch->ch_txcount;
+		getchan.ch_rxcount = ch->ch_rxcount;
+		getchan.ch_s_brate = ch->ch_s_brate;
+		getchan.ch_s_estat = ch->ch_s_elast;
+		getchan.ch_s_cflag = ch->ch_s_cflag;
+		getchan.ch_s_iflag = ch->ch_s_iflag;
+		getchan.ch_s_oflag = ch->ch_s_oflag;
+		getchan.ch_s_xflag = ch->ch_s_xflag;
+		getchan.ch_s_mstat = ch->ch_s_mlast;
+
+		if (copy_to_user(uarg, &getchan, sizeof(struct digi_chan)))
+			return -EFAULT;
+		break;
+
+
+	case DIGI_GETNODE:
+		getnode.nd_state = (nd->nd_state & NS_READY) ? 1 : 0;
+		getnode.nd_chan_count = nd->nd_chan_count;
+		getnode.nd_tx_byte = nd->nd_tx_byte;
+		getnode.nd_rx_byte = nd->nd_rx_byte;
+
+		memset(&getnode.nd_ps_desc, 0, MAX_DESC_LEN);
+		strncpy(getnode.nd_ps_desc, nd->nd_ps_desc, MAX_DESC_LEN);
+
+		if (copy_to_user(uarg, &getnode, sizeof(struct digi_node)))
+			return -EFAULT;
+		break;
+
+
+	case DIGI_SETDEBUG:
+		if (copy_from_user(&setdebug, uarg, sizeof(struct digi_debug)))
+			return -EFAULT;
+
+		nd->nd_dpa_debug = setdebug.onoff;
+		nd->nd_dpa_port = setdebug.port;
+		break;
+
+
+	case DIGI_GETVPD:
+		if (nd->nd_vpd_len > 0) {
+			vpd.vpd_len = nd->nd_vpd_len;
+			memcpy(&vpd.vpd_data, &nd->nd_vpd, nd->nd_vpd_len);
+		} else {
+			vpd.vpd_len = 0;
+		}
+
+		if (copy_to_user(uarg, &vpd, sizeof(struct digi_vpd)))
+			return -EFAULT;
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * dgrp_dpa() -- send data to the device monitor queue
+ * @nd: pointer to a node structure
+ * @buf: buffer of data to copy to the monitoring buffer
+ * @len: number of bytes to transfer to the buffer
+ *
+ * Called by the net device routines to send data to the device
+ * monitor queue.  If the device monitor buffer is too full to
+ * accept the data, it waits until the buffer is ready.
+ */
+static void dgrp_dpa(struct nd_struct *nd, u8 *buf, int nbuf)
+{
+	int n;
+	int r;
+	unsigned long lock_flags;
+
+	/*
+	 *  Grab DPA lock.
+	 */
+	spin_lock_irqsave(&nd->nd_dpa_lock, lock_flags);
+
+	/*
+	 *  Loop while data remains.
+	 */
+	while (nbuf > 0 && nd->nd_dpa_buf != NULL) {
+
+		n = (nd->nd_dpa_out - nd->nd_dpa_in - 1) & DPA_MASK;
+
+		/*
+		 * Enforce flow control on the DPA device.
+		 */
+		if (n < (DPA_MAX - DPA_HIGH_WATER))
+			nd->nd_dpa_flag |= DPA_WAIT_SPACE;
+
+		/*
+		 * This should never happen, as the flow control above
+		 * should have stopped things before they got to this point.
+		 */
+		if (n == 0) {
+			spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+			return;
+		}
+
+		/*
+		 * Copy as much data as will fit.
+		 */
+
+		if (n > nbuf)
+			n = nbuf;
+
+		r = DPA_MAX - nd->nd_dpa_in;
+
+		if (r <= n) {
+			memcpy(nd->nd_dpa_buf + nd->nd_dpa_in, buf, r);
+
+			n -= r;
+
+			nd->nd_dpa_in = 0;
+
+			buf += r;
+			nbuf -= r;
+		}
+
+		memcpy(nd->nd_dpa_buf + nd->nd_dpa_in, buf, n);
+
+		nd->nd_dpa_in += n;
+
+		buf += n;
+		nbuf -= n;
+
+		if (nd->nd_dpa_in >= DPA_MAX)
+			pr_info_ratelimited("%s - nd->nd_dpa_in (%i) >= DPA_MAX\n",
+					    __func__, nd->nd_dpa_in);
+
+		/*
+		 *  Wakeup any thread waiting for data
+		 */
+		if (nd->nd_dpa_flag & DPA_WAIT_DATA) {
+			nd->nd_dpa_flag &= ~DPA_WAIT_DATA;
+			wake_up_interruptible(&nd->nd_dpa_wqueue);
+		}
+	}
+
+	/*
+	 *  Release the DPA lock.
+	 */
+	spin_unlock_irqrestore(&nd->nd_dpa_lock, lock_flags);
+}
+
+/**
+ * dgrp_monitor_data() -- builds a DPA data packet
+ * @nd: pointer to a node structure
+ * @type: type of message to be logged in the DPA buffer
+ * @buf: buffer of data to be logged in the DPA buffer
+ * @size -- number of bytes in the "buf" buffer
+ */
+void dgrp_dpa_data(struct nd_struct *nd, int type, u8 *buf, int size)
+{
+	u8 header[5];
+
+	header[0] = type;
+
+	put_unaligned_be32(size, header + 1);
+
+	dgrp_dpa(nd, header, sizeof(header));
+	dgrp_dpa(nd, buf, size);
+}
diff --git a/drivers/staging/dgrp/dgrp_driver.c b/drivers/staging/dgrp/dgrp_driver.c
new file mode 100644
index 000000000000..6e4a0ebc0749
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_driver.c
@@ -0,0 +1,110 @@
+/*
+ *
+ * Copyright 1999-2003 Digi International (www.digi.com)
+ *     Jeff Randall
+ *     James Puzzo  <jamesp at digi dot com>
+ *     Scott 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.
+ *
+ */
+
+/*
+ *	Driver specific includes
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/init.h>
+
+/*
+ *  PortServer includes
+ */
+#include "dgrp_common.h"
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Digi International, http://www.digi.com");
+MODULE_DESCRIPTION("RealPort driver for Digi's ethernet-based serial connectivity product line");
+MODULE_VERSION(DIGI_VERSION);
+
+struct list_head nd_struct_list;
+struct dgrp_poll_data dgrp_poll_data;
+
+int dgrp_rawreadok = 1;		/* Bypass flipbuf on input */
+int dgrp_register_cudevices = 1;/* Turn on/off registering legacy cu devices */
+int dgrp_register_prdevices = 1;/* Turn on/off registering transparent print */
+int dgrp_poll_tick = 20;	/* Poll interval - in ms */
+
+module_param_named(rawreadok, dgrp_rawreadok, int, 0644);
+MODULE_PARM_DESC(rawreadok, "Bypass flip buffers on input");
+
+module_param_named(register_cudevices, dgrp_register_cudevices, int, 0644);
+MODULE_PARM_DESC(register_cudevices, "Turn on/off registering legacy cu devices");
+
+module_param_named(register_prdevices, dgrp_register_prdevices, int, 0644);
+MODULE_PARM_DESC(register_prdevices, "Turn on/off registering transparent print devices");
+
+module_param_named(pollrate, dgrp_poll_tick, int, 0644);
+MODULE_PARM_DESC(pollrate, "Poll interval in ms");
+
+/* Driver load/unload functions */
+static int dgrp_init_module(void);
+static void dgrp_cleanup_module(void);
+
+module_init(dgrp_init_module);
+module_exit(dgrp_cleanup_module);
+
+/*
+ * init_module()
+ *
+ * Module load.  This is where it all starts.
+ */
+static int dgrp_init_module(void)
+{
+	INIT_LIST_HEAD(&nd_struct_list);
+
+	spin_lock_init(&dgrp_poll_data.poll_lock);
+	init_timer(&dgrp_poll_data.timer);
+	dgrp_poll_data.poll_tick = dgrp_poll_tick;
+	dgrp_poll_data.timer.function = dgrp_poll_handler;
+	dgrp_poll_data.timer.data = (unsigned long) &dgrp_poll_data;
+
+	dgrp_create_class_sysfs_files();
+
+	dgrp_register_proc();
+
+	return 0;
+}
+
+
+/*
+ *	Module unload.  This is where it all ends.
+ */
+static void dgrp_cleanup_module(void)
+{
+	struct nd_struct *nd, *next;
+
+	/*
+	 *	Attempting to free resources in backwards
+	 *	order of allocation, in case that helps
+	 *	memory pool fragmentation.
+	 */
+	dgrp_unregister_proc();
+
+	dgrp_remove_class_sysfs_files();
+
+
+	list_for_each_entry_safe(nd, next, &nd_struct_list, list) {
+		dgrp_tty_uninit(nd);
+		kfree(nd);
+	}
+}
diff --git a/drivers/staging/dgrp/dgrp_mon_ops.c b/drivers/staging/dgrp/dgrp_mon_ops.c
new file mode 100644
index 000000000000..268dcb95204b
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_mon_ops.c
@@ -0,0 +1,346 @@
+/*****************************************************************************
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_mon_ops.c
+ *
+ *  Description:
+ *
+ *     Handle the file operations required for the "monitor" devices.
+ *     Includes those functions required to register the "mon" devices
+ *     in "/proc".
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+#include <asm/unaligned.h>
+#include <linux/proc_fs.h>
+
+#include "dgrp_common.h"
+
+/* File operation declarations */
+static int dgrp_mon_open(struct inode *, struct file *);
+static int dgrp_mon_release(struct inode *, struct file *);
+static ssize_t dgrp_mon_read(struct file *, char __user *, size_t, loff_t *);
+static long dgrp_mon_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg);
+
+static const struct file_operations mon_ops = {
+	.owner   = THIS_MODULE,
+	.read    = dgrp_mon_read,
+	.unlocked_ioctl = dgrp_mon_ioctl,
+	.open    = dgrp_mon_open,
+	.release = dgrp_mon_release,
+};
+
+static struct inode_operations mon_inode_ops = {
+	.permission = dgrp_inode_permission
+};
+
+void dgrp_register_mon_hook(struct proc_dir_entry *de)
+{
+	struct nd_struct *node = de->data;
+
+	de->proc_iops = &mon_inode_ops;
+	de->proc_fops = &mon_ops;
+	node->nd_mon_de = de;
+	sema_init(&node->nd_mon_semaphore, 1);
+}
+
+/**
+ * dgrp_mon_open() -- open /proc/dgrp/ports device for a PortServer
+ * @inode: struct inode *
+ * @file: struct file *
+ *
+ * Open function to open the /proc/dgrp/ports device for a PortServer.
+ */
+static int dgrp_mon_open(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+	struct proc_dir_entry *de;
+	struct timeval tv;
+	uint32_t time;
+	u8 *buf;
+	int rtn;
+
+	rtn = try_module_get(THIS_MODULE);
+	if (!rtn)
+		return -ENXIO;
+
+	rtn = 0;
+
+	if (!capable(CAP_SYS_ADMIN)) {
+		rtn = -EPERM;
+		goto done;
+	}
+
+	/*
+	 *  Make sure that the "private_data" field hasn't already been used.
+	 */
+	if (file->private_data) {
+		rtn = -EINVAL;
+		goto done;
+	}
+
+	/*
+	 *  Get the node pointer, and fail if it doesn't exist.
+	 */
+	de = PDE(inode);
+	if (!de) {
+		rtn = -ENXIO;
+		goto done;
+	}
+
+	nd = (struct nd_struct *)de->data;
+	if (!nd) {
+		rtn = -ENXIO;
+		goto done;
+	}
+
+	file->private_data = (void *) nd;
+
+	/*
+	 * Allocate the monitor buffer.
+	 */
+
+	/*
+	 *  Grab the MON lock.
+	 */
+	down(&nd->nd_mon_semaphore);
+
+	if (nd->nd_mon_buf) {
+		rtn = -EBUSY;
+		goto done_up;
+	}
+
+	nd->nd_mon_buf = kmalloc(MON_MAX, GFP_KERNEL);
+
+	if (!nd->nd_mon_buf) {
+		rtn = -ENOMEM;
+		goto done_up;
+	}
+
+	/*
+	 *  Enter an RPDUMP file header into the buffer.
+	 */
+
+	buf = nd->nd_mon_buf;
+
+	strcpy(buf, RPDUMP_MAGIC);
+	buf += strlen(buf) + 1;
+
+	do_gettimeofday(&tv);
+
+	/*
+	 *  tv.tv_sec might be a 64 bit quantity.  Pare
+	 *  it down to 32 bits before attempting to encode
+	 *  it.
+	 */
+	time = (uint32_t) (tv.tv_sec & 0xffffffff);
+
+	put_unaligned_be32(time, buf);
+	put_unaligned_be16(0, buf + 4);
+	buf += 6;
+
+	if (nd->nd_tx_module) {
+		buf[0] = RPDUMP_CLIENT;
+		put_unaligned_be32(0, buf + 1);
+		put_unaligned_be16(1, buf + 5);
+		buf[7] = 0xf0 + nd->nd_tx_module;
+		buf += 8;
+	}
+
+	if (nd->nd_rx_module) {
+		buf[0] = RPDUMP_SERVER;
+		put_unaligned_be32(0, buf + 1);
+		put_unaligned_be16(1, buf + 5);
+		buf[7] = 0xf0 + nd->nd_rx_module;
+		buf += 8;
+	}
+
+	nd->nd_mon_out = 0;
+	nd->nd_mon_in  = buf - nd->nd_mon_buf;
+	nd->nd_mon_lbolt = jiffies;
+
+done_up:
+	up(&nd->nd_mon_semaphore);
+
+done:
+	if (rtn)
+		module_put(THIS_MODULE);
+	return rtn;
+}
+
+
+/**
+ * dgrp_mon_release() - Close the MON device for a particular PortServer
+ * @inode: struct inode *
+ * @file: struct file *
+ */
+static int dgrp_mon_release(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		goto done;
+
+	/*
+	 *  Free the monitor buffer.
+	 */
+
+	down(&nd->nd_mon_semaphore);
+
+	kfree(nd->nd_mon_buf);
+	nd->nd_mon_buf = NULL;
+	nd->nd_mon_out = nd->nd_mon_in;
+
+	/*
+	 *  Wakeup any thread waiting for buffer space.
+	 */
+
+	if (nd->nd_mon_flag & MON_WAIT_SPACE) {
+		nd->nd_mon_flag &= ~MON_WAIT_SPACE;
+		wake_up_interruptible(&nd->nd_mon_wqueue);
+	}
+
+	up(&nd->nd_mon_semaphore);
+
+	/*
+	 *  Make sure there is no thread in the middle of writing a packet.
+	 */
+	down(&nd->nd_net_semaphore);
+	up(&nd->nd_net_semaphore);
+
+done:
+	module_put(THIS_MODULE);
+	file->private_data = NULL;
+	return 0;
+}
+
+/**
+ * dgrp_mon_read() -- Copy data from the monitoring buffer to the user
+ */
+static ssize_t dgrp_mon_read(struct file *file, char __user *buf, size_t count,
+			     loff_t *ppos)
+{
+	struct nd_struct *nd;
+	int r;
+	int offset = 0;
+	int res = 0;
+	ssize_t rtn;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		return -ENXIO;
+
+	/*
+	 *  Wait for some data to appear in the buffer.
+	 */
+
+	down(&nd->nd_mon_semaphore);
+
+	for (;;) {
+		res = (nd->nd_mon_in - nd->nd_mon_out) & MON_MASK;
+
+		if (res)
+			break;
+
+		nd->nd_mon_flag |= MON_WAIT_DATA;
+
+		up(&nd->nd_mon_semaphore);
+
+		/*
+		 * Go to sleep waiting until the condition becomes true.
+		 */
+		rtn = wait_event_interruptible(nd->nd_mon_wqueue,
+					       ((nd->nd_mon_flag & MON_WAIT_DATA) == 0));
+
+		if (rtn)
+			return rtn;
+
+		down(&nd->nd_mon_semaphore);
+	}
+
+	/*
+	 *  Read whatever is there.
+	 */
+
+	if (res > count)
+		res = count;
+
+	r = MON_MAX - nd->nd_mon_out;
+
+	if (r <= res) {
+		rtn = copy_to_user((void __user *)buf,
+				   nd->nd_mon_buf + nd->nd_mon_out, r);
+		if (rtn) {
+			up(&nd->nd_mon_semaphore);
+			return -EFAULT;
+		}
+
+		nd->nd_mon_out = 0;
+		res -= r;
+		offset = r;
+	}
+
+	rtn = copy_to_user((void __user *) buf + offset,
+			   nd->nd_mon_buf + nd->nd_mon_out, res);
+	if (rtn) {
+		up(&nd->nd_mon_semaphore);
+		return -EFAULT;
+	}
+
+	nd->nd_mon_out += res;
+
+	*ppos += res;
+
+	up(&nd->nd_mon_semaphore);
+
+	/*
+	 *  Wakeup any thread waiting for buffer space.
+	 */
+
+	if (nd->nd_mon_flag & MON_WAIT_SPACE) {
+		nd->nd_mon_flag &= ~MON_WAIT_SPACE;
+		wake_up_interruptible(&nd->nd_mon_wqueue);
+	}
+
+	return res;
+}
+
+/*  ioctl is not valid on monitor device */
+static long dgrp_mon_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg)
+{
+	return -EINVAL;
+}
diff --git a/drivers/staging/dgrp/dgrp_net_ops.c b/drivers/staging/dgrp/dgrp_net_ops.c
new file mode 100644
index 000000000000..ab839ea3b44c
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_net_ops.c
@@ -0,0 +1,3737 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo  <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_net_ops.c
+ *
+ *  Description:
+ *
+ *     Handle the file operations required for the "network" devices.
+ *     Includes those functions required to register the "net" devices
+ *     in "/proc".
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/proc_fs.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/spinlock.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/ratelimit.h>
+#include <asm/unaligned.h>
+
+#define MYFLIPLEN	TBUF_MAX
+
+#include "dgrp_common.h"
+
+#define TTY_FLIPBUF_SIZE 512
+#define DEVICE_NAME_SIZE 50
+
+/*
+ *  Generic helper function declarations
+ */
+static void   parity_scan(struct ch_struct *ch, unsigned char *cbuf,
+				unsigned char *fbuf, int *len);
+
+/*
+ *  File operation declarations
+ */
+static int dgrp_net_open(struct inode *, struct file *);
+static int dgrp_net_release(struct inode *, struct file *);
+static ssize_t dgrp_net_read(struct file *, char __user *, size_t, loff_t *);
+static ssize_t dgrp_net_write(struct file *, const char __user *, size_t,
+			      loff_t *);
+static long dgrp_net_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg);
+static unsigned int dgrp_net_select(struct file *file,
+				    struct poll_table_struct *table);
+
+static const struct file_operations net_ops = {
+	.owner   =  THIS_MODULE,
+	.read    =  dgrp_net_read,
+	.write   =  dgrp_net_write,
+	.poll    =  dgrp_net_select,
+	.unlocked_ioctl =  dgrp_net_ioctl,
+	.open    =  dgrp_net_open,
+	.release =  dgrp_net_release,
+};
+
+static struct inode_operations net_inode_ops = {
+	.permission = dgrp_inode_permission
+};
+
+void dgrp_register_net_hook(struct proc_dir_entry *de)
+{
+	struct nd_struct *node = de->data;
+
+	de->proc_iops = &net_inode_ops;
+	de->proc_fops = &net_ops;
+	node->nd_net_de = de;
+	sema_init(&node->nd_net_semaphore, 1);
+	node->nd_state = NS_CLOSED;
+	dgrp_create_node_class_sysfs_files(node);
+}
+
+
+/**
+ * dgrp_dump() -- prints memory for debugging purposes.
+ * @mem: Memory location which should be printed to the console
+ * @len: Number of bytes to be dumped
+ */
+static void dgrp_dump(u8 *mem, int len)
+{
+	int i;
+
+	pr_debug("dgrp dump length = %d, data = ", len);
+	for (i = 0; i < len; ++i)
+		pr_debug("%.2x ", mem[i]);
+	pr_debug("\n");
+}
+
+/**
+ * dgrp_read_data_block() -- Read a data block
+ * @ch: struct ch_struct *
+ * @flipbuf: u8 *
+ * @flipbuf_size: size of flipbuf
+ */
+static void dgrp_read_data_block(struct ch_struct *ch, u8 *flipbuf,
+				 int flipbuf_size)
+{
+	int t;
+	int n;
+
+	if (flipbuf_size <= 0)
+		return;
+
+	t = RBUF_MAX - ch->ch_rout;
+	n = flipbuf_size;
+
+	if (n >= t) {
+		memcpy(flipbuf, ch->ch_rbuf + ch->ch_rout, t);
+		flipbuf += t;
+		n -= t;
+		ch->ch_rout = 0;
+	}
+
+	memcpy(flipbuf, ch->ch_rbuf + ch->ch_rout, n);
+	flipbuf += n;
+	ch->ch_rout += n;
+}
+
+
+/**
+ * dgrp_input() -- send data to the line disipline
+ * @ch: pointer to channel struct
+ *
+ * Copys the rbuf to the flipbuf and sends to line discipline.
+ * Sends input buffer data to the line discipline.
+ *
+ * There are several modes to consider here:
+ *    rawreadok, tty->real_raw, and IF_PARMRK
+ */
+static void dgrp_input(struct ch_struct *ch)
+{
+	struct nd_struct *nd;
+	struct tty_struct *tty;
+	int remain;
+	int data_len;
+	int len;
+	int flip_len;
+	int tty_count;
+	ulong lock_flags;
+	struct tty_ldisc *ld;
+	u8  *myflipbuf;
+	u8  *myflipflagbuf;
+
+	if (!ch)
+		return;
+
+	nd = ch->ch_nd;
+
+	if (!nd)
+		return;
+
+	spin_lock_irqsave(&nd->nd_lock, lock_flags);
+
+	myflipbuf = nd->nd_inputbuf;
+	myflipflagbuf = nd->nd_inputflagbuf;
+
+	if (!ch->ch_open_count) {
+		ch->ch_rout = ch->ch_rin;
+		goto out;
+	}
+
+	if (ch->ch_tun.un_flag & UN_CLOSING) {
+		ch->ch_rout = ch->ch_rin;
+		goto out;
+	}
+
+	tty = (ch->ch_tun).un_tty;
+
+
+	if (!tty || tty->magic != TTY_MAGIC) {
+		ch->ch_rout = ch->ch_rin;
+		goto out;
+	}
+
+	tty_count = tty->count;
+	if (!tty_count) {
+		ch->ch_rout = ch->ch_rin;
+		goto out;
+	}
+
+	if (tty->closing || test_bit(TTY_CLOSING, &tty->flags)) {
+		ch->ch_rout = ch->ch_rin;
+		goto out;
+	}
+
+	spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+
+	/* Decide how much data we can send into the tty layer */
+	if (dgrp_rawreadok && tty->real_raw)
+		flip_len = MYFLIPLEN;
+	else
+		flip_len = TTY_FLIPBUF_SIZE;
+
+	/* data_len should be the number of chars that we read in */
+	data_len = (ch->ch_rin - ch->ch_rout) & RBUF_MASK;
+	remain = data_len;
+
+	/* len is the amount of data we are going to transfer here */
+	len = min(data_len, flip_len);
+
+	/* take into consideration length of ldisc */
+	len = min(len, (N_TTY_BUF_SIZE - 1) - tty->read_cnt);
+
+	ld = tty_ldisc_ref(tty);
+
+	/*
+	 * If we were unable to get a reference to the ld,
+	 * don't flush our buffer, and act like the ld doesn't
+	 * have any space to put the data right now.
+	 */
+	if (!ld) {
+		len = 0;
+	} else if (!ld->ops->receive_buf) {
+		spin_lock_irqsave(&nd->nd_lock, lock_flags);
+		ch->ch_rout = ch->ch_rin;
+		spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+		len = 0;
+	}
+
+	/* Check DPA flow control */
+	if ((nd->nd_dpa_debug) &&
+	    (nd->nd_dpa_flag & DPA_WAIT_SPACE) &&
+	    (nd->nd_dpa_port == MINOR(tty_devnum(ch->ch_tun.un_tty))))
+		len = 0;
+
+	if ((len) && !(ch->ch_flag & CH_RXSTOP)) {
+
+		dgrp_read_data_block(ch, myflipbuf, len);
+
+		/*
+		 * In high performance mode, we don't have to update
+		 * flag_buf or any of the counts or pointers into flip buf.
+		 */
+		if (!dgrp_rawreadok || !tty->real_raw) {
+			if (I_PARMRK(tty) || I_BRKINT(tty) || I_INPCK(tty))
+				parity_scan(ch, myflipbuf, myflipflagbuf, &len);
+			else
+				memset(myflipflagbuf, TTY_NORMAL, len);
+		}
+
+		if ((nd->nd_dpa_debug) &&
+		    (nd->nd_dpa_port == PORT_NUM(MINOR(tty_devnum(tty)))))
+			dgrp_dpa_data(nd, 1, myflipbuf, len);
+
+		/*
+		 * If we're doing raw reads, jam it right into the
+		 * line disc bypassing the flip buffers.
+		 */
+		if (dgrp_rawreadok && tty->real_raw)
+			ld->ops->receive_buf(tty, myflipbuf, NULL, len);
+		else {
+			len = tty_buffer_request_room(tty, len);
+			tty_insert_flip_string_flags(tty, myflipbuf,
+						     myflipflagbuf, len);
+
+			/* Tell the tty layer its okay to "eat" the data now */
+			tty_flip_buffer_push(tty);
+		}
+
+		ch->ch_rxcount += len;
+	}
+
+	if (ld)
+		tty_ldisc_deref(ld);
+
+	/*
+	 * Wake up any sleepers (maybe dgrp close) that might be waiting
+	 * for a channel flag state change.
+	 */
+	wake_up_interruptible(&ch->ch_flag_wait);
+	return;
+
+out:
+	spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+}
+
+
+/*
+ *  parity_scan
+ *
+ *  Loop to inspect each single character or 0xFF escape.
+ *
+ *  if PARMRK & ~DOSMODE:
+ *     0xFF  0xFF           Normal 0xFF character, escaped
+ *                          to eliminate confusion.
+ *     0xFF  0x00  0x00     Break
+ *     0xFF  0x00  CC       Error character CC.
+ *     CC                   Normal character CC.
+ *
+ *  if PARMRK & DOSMODE:
+ *     0xFF  0x18  0x00     Break
+ *     0xFF  0x08  0x00     Framing Error
+ *     0xFF  0x04  0x00     Parity error
+ *     0xFF  0x0C  0x00     Both Framing and Parity error
+ *
+ *  TODO:  do we need to do the XMODEM, XOFF, XON, XANY processing??
+ *         as per protocol
+ */
+static void parity_scan(struct ch_struct *ch, unsigned char *cbuf,
+			unsigned char *fbuf, int *len)
+{
+	int l = *len;
+	int count = 0;
+	int DOS = ((ch->ch_iflag & IF_DOSMODE) == 0 ? 0 : 1);
+	unsigned char *cout; /* character buffer */
+	unsigned char *fout; /* flag buffer */
+	unsigned char *in;
+	unsigned char c;
+
+	in = cbuf;
+	cout = cbuf;
+	fout = fbuf;
+
+	while (l--) {
+		c = *in;
+		in++;
+
+		switch (ch->ch_pscan_state) {
+		default:
+			/* reset to sanity and fall through */
+			ch->ch_pscan_state = 0 ;
+
+		case 0:
+			/* No FF seen yet */
+			if (c == 0xff) /* delete this character from stream */
+				ch->ch_pscan_state = 1;
+			else {
+				*cout++ = c;
+				*fout++ = TTY_NORMAL;
+				count += 1;
+			}
+			break;
+
+		case 1:
+			/* first FF seen */
+			if (c == 0xff) {
+				/* doubled ff, transform to single ff */
+				*cout++ = c;
+				*fout++ = TTY_NORMAL;
+				count += 1;
+				ch->ch_pscan_state = 0;
+			} else {
+				/* save value examination in next state */
+				ch->ch_pscan_savechar = c;
+				ch->ch_pscan_state = 2;
+			}
+			break;
+
+		case 2:
+			/* third character of ff sequence */
+			*cout++ = c;
+			if (DOS) {
+				if (ch->ch_pscan_savechar & 0x10)
+					*fout++ = TTY_BREAK;
+				else if (ch->ch_pscan_savechar & 0x08)
+					*fout++ = TTY_FRAME;
+				else
+					/*
+					 * either marked as a parity error,
+					 * indeterminate, or not in DOSMODE
+					 * call it a parity error
+					 */
+					*fout++ = TTY_PARITY;
+			} else {
+				/* case FF XX ?? where XX is not 00 */
+				if (ch->ch_pscan_savechar & 0xff) {
+					/* this should not happen */
+					pr_info("%s: parity_scan: error unexpected byte\n",
+						__func__);
+					*fout++ = TTY_PARITY;
+				}
+				/* case FF 00 XX where XX is not 00 */
+				else if (c == 0xff)
+					*fout++ = TTY_PARITY;
+				/* case FF 00 00 */
+				else
+					*fout++ = TTY_BREAK;
+
+			}
+			count += 1;
+			ch->ch_pscan_state = 0;
+		}
+	}
+	*len = count;
+}
+
+
+/**
+ * dgrp_net_idle() -- Idle the network connection
+ * @nd: pointer to node structure to idle
+ */
+static void dgrp_net_idle(struct nd_struct *nd)
+{
+	struct ch_struct *ch;
+	int i;
+
+	nd->nd_tx_work = 1;
+
+	nd->nd_state = NS_IDLE;
+	nd->nd_flag = 0;
+
+	for (i = nd->nd_seq_out; ; i = (i + 1) & SEQ_MASK) {
+		if (!nd->nd_seq_wait[i]) {
+			nd->nd_seq_wait[i] = 0;
+			wake_up_interruptible(&nd->nd_seq_wque[i]);
+		}
+
+		if (i == nd->nd_seq_in)
+			break;
+	}
+
+	nd->nd_seq_out = nd->nd_seq_in;
+
+	nd->nd_unack = 0;
+	nd->nd_remain = 0;
+
+	nd->nd_tx_module = 0x10;
+	nd->nd_rx_module = 0x00;
+
+	for (i = 0, ch = nd->nd_chan; i < CHAN_MAX; i++, ch++) {
+		ch->ch_state = CS_IDLE;
+
+		ch->ch_otype = 0;
+		ch->ch_otype_waiting = 0;
+	}
+}
+
+/*
+ *  Increase the number of channels, waking up any
+ *  threads that might be waiting for the channels
+ *  to appear.
+ */
+static void increase_channel_count(struct nd_struct *nd, int n)
+{
+	struct ch_struct *ch;
+	struct device *classp;
+	char name[DEVICE_NAME_SIZE];
+	int ret;
+	u8 *buf;
+	int i;
+
+	for (i = nd->nd_chan_count; i < n; ++i) {
+		ch = nd->nd_chan + i;
+
+		/* FIXME: return a useful error instead! */
+		buf = kmalloc(TBUF_MAX, GFP_KERNEL);
+		if (!buf)
+			return;
+
+		if (ch->ch_tbuf)
+			pr_info_ratelimited("%s - ch_tbuf was not NULL\n",
+					    __func__);
+
+		ch->ch_tbuf = buf;
+
+		buf = kmalloc(RBUF_MAX, GFP_KERNEL);
+		if (!buf)
+			return;
+
+		if (ch->ch_rbuf)
+			pr_info("%s - ch_rbuf was not NULL\n",
+				__func__);
+		ch->ch_rbuf = buf;
+
+		classp = tty_port_register_device(&ch->port,
+						  nd->nd_serial_ttdriver, i,
+						  NULL);
+
+		ch->ch_tun.un_sysfs = classp;
+		snprintf(name, DEVICE_NAME_SIZE, "tty_%d", i);
+
+		dgrp_create_tty_sysfs(&ch->ch_tun, classp);
+		ret = sysfs_create_link(&nd->nd_class_dev->kobj,
+					&classp->kobj, name);
+
+		/* NOTE: We don't support "cu" devices anymore,
+		 * so you will notice we don't register them
+		 * here anymore. */
+		if (dgrp_register_prdevices) {
+			classp = tty_register_device(nd->nd_xprint_ttdriver,
+						     i, NULL);
+			ch->ch_pun.un_sysfs = classp;
+			snprintf(name, DEVICE_NAME_SIZE, "pr_%d", i);
+
+			dgrp_create_tty_sysfs(&ch->ch_pun, classp);
+			ret = sysfs_create_link(&nd->nd_class_dev->kobj,
+						&classp->kobj, name);
+		}
+
+		nd->nd_chan_count = i + 1;
+		wake_up_interruptible(&ch->ch_flag_wait);
+	}
+}
+
+/*
+ * Decrease the number of channels, and wake up any threads that might
+ * be waiting on the channels that vanished.
+ */
+static void decrease_channel_count(struct nd_struct *nd, int n)
+{
+	struct ch_struct *ch;
+	char name[DEVICE_NAME_SIZE];
+	int i;
+
+	for (i = nd->nd_chan_count - 1; i >= n; --i) {
+		ch = nd->nd_chan + i;
+
+		/*
+		 *  Make any open ports inoperative.
+		 */
+		ch->ch_state = CS_IDLE;
+
+		ch->ch_otype = 0;
+		ch->ch_otype_waiting = 0;
+
+		/*
+		 *  Only "HANGUP" if we care about carrier
+		 *  transitions and we are already open.
+		 */
+		if (ch->ch_open_count != 0) {
+			ch->ch_flag |= CH_HANGUP;
+			dgrp_carrier(ch);
+		}
+
+		/*
+		 * Unlike the CH_HANGUP flag above, use another
+		 * flag to indicate to the RealPort state machine
+		 * that this port has disappeared.
+		 */
+		if (ch->ch_open_count != 0)
+			ch->ch_flag |= CH_PORT_GONE;
+
+		wake_up_interruptible(&ch->ch_flag_wait);
+
+		nd->nd_chan_count = i;
+
+		kfree(ch->ch_tbuf);
+		ch->ch_tbuf = NULL;
+
+		kfree(ch->ch_rbuf);
+		ch->ch_rbuf = NULL;
+
+		nd->nd_chan_count = i;
+
+		dgrp_remove_tty_sysfs(ch->ch_tun.un_sysfs);
+		snprintf(name, DEVICE_NAME_SIZE, "tty_%d", i);
+		sysfs_remove_link(&nd->nd_class_dev->kobj, name);
+		tty_unregister_device(nd->nd_serial_ttdriver, i);
+
+		/*
+		 * NOTE: We don't support "cu" devices anymore, so don't
+		 * unregister them here anymore.
+		 */
+
+		if (dgrp_register_prdevices) {
+			dgrp_remove_tty_sysfs(ch->ch_pun.un_sysfs);
+			snprintf(name, DEVICE_NAME_SIZE, "pr_%d", i);
+			sysfs_remove_link(&nd->nd_class_dev->kobj, name);
+			tty_unregister_device(nd->nd_xprint_ttdriver, i);
+		}
+	}
+}
+
+/**
+ * dgrp_chan_count() -- Adjust the node channel count.
+ * @nd: pointer to a node structure
+ * @n: new value for channel count
+ *
+ * Adjusts the node channel count.  If new ports have appeared, it tries
+ * to signal those processes that might have been waiting for ports to
+ * appear.  If ports have disappeared it tries to signal those processes
+ * that might be hung waiting for a response for the now non-existant port.
+ */
+static void dgrp_chan_count(struct nd_struct *nd, int n)
+{
+	if (n == nd->nd_chan_count)
+		return;
+
+	if (n > nd->nd_chan_count)
+		increase_channel_count(nd, n);
+
+	if (n < nd->nd_chan_count)
+		decrease_channel_count(nd, n);
+}
+
+/**
+ * dgrp_monitor() -- send data to the device monitor queue
+ * @nd: pointer to a node structure
+ * @buf: data to copy to the monitoring buffer
+ * @len: number of bytes to transfer to the buffer
+ *
+ * Called by the net device routines to send data to the device
+ * monitor queue.  If the device monitor buffer is too full to
+ * accept the data, it waits until the buffer is ready.
+ */
+static void dgrp_monitor(struct nd_struct *nd, u8 *buf, int len)
+{
+	int n;
+	int r;
+	int rtn;
+
+	/*
+	 *  Grab monitor lock.
+	 */
+	down(&nd->nd_mon_semaphore);
+
+	/*
+	 *  Loop while data remains.
+	 */
+	while ((len > 0) && (nd->nd_mon_buf)) {
+		/*
+		 *  Determine the amount of available space left in the
+		 *  buffer.  If there's none, wait until some appears.
+		 */
+
+		n = (nd->nd_mon_out - nd->nd_mon_in - 1) & MON_MASK;
+
+		if (!n) {
+			nd->nd_mon_flag |= MON_WAIT_SPACE;
+
+			up(&nd->nd_mon_semaphore);
+
+			/*
+			 * Go to sleep waiting until the condition becomes true.
+			 */
+			rtn = wait_event_interruptible(nd->nd_mon_wqueue,
+						       ((nd->nd_mon_flag & MON_WAIT_SPACE) == 0));
+
+/* FIXME: really ignore rtn? */
+
+			/*
+			 *  We can't exit here if we receive a signal, since
+			 *  to do so would trash the debug stream.
+			 */
+
+			down(&nd->nd_mon_semaphore);
+
+			continue;
+		}
+
+		/*
+		 * Copy as much data as will fit.
+		 */
+
+		if (n > len)
+			n = len;
+
+		r = MON_MAX - nd->nd_mon_in;
+
+		if (r <= n) {
+			memcpy(nd->nd_mon_buf + nd->nd_mon_in, buf, r);
+
+			n -= r;
+
+			nd->nd_mon_in = 0;
+
+			buf += r;
+			len -= r;
+		}
+
+		memcpy(nd->nd_mon_buf + nd->nd_mon_in, buf, n);
+
+		nd->nd_mon_in += n;
+
+		buf += n;
+		len -= n;
+
+		if (nd->nd_mon_in >= MON_MAX)
+			pr_info_ratelimited("%s - nd_mon_in (%i) >= MON_MAX\n",
+					    __func__, nd->nd_mon_in);
+
+		/*
+		 *  Wakeup any thread waiting for data
+		 */
+
+		if (nd->nd_mon_flag & MON_WAIT_DATA) {
+			nd->nd_mon_flag &= ~MON_WAIT_DATA;
+			wake_up_interruptible(&nd->nd_mon_wqueue);
+		}
+	}
+
+	/*
+	 *  Release the monitor lock.
+	 */
+	up(&nd->nd_mon_semaphore);
+}
+
+/**
+ * dgrp_encode_time() -- Encodes rpdump time into a 4-byte quantity.
+ * @nd: pointer to a node structure
+ * @buf: destination buffer
+ *
+ * Encodes "rpdump" time into a 4-byte quantity.  Time is measured since
+ * open.
+ */
+static void dgrp_encode_time(struct nd_struct *nd, u8 *buf)
+{
+	ulong t;
+
+	/*
+	 *  Convert time in HZ since open to time in milliseconds
+	 *  since open.
+	 */
+	t = jiffies - nd->nd_mon_lbolt;
+	t = 1000 * (t / HZ) + 1000 * (t % HZ) / HZ;
+
+	put_unaligned_be32((uint)(t & 0xffffffff), buf);
+}
+
+
+
+/**
+ * dgrp_monitor_message() -- Builds a rpdump style message.
+ * @nd: pointer to a node structure
+ * @message: destination buffer
+ */
+static void dgrp_monitor_message(struct nd_struct *nd, char *message)
+{
+	u8 header[7];
+	int n;
+
+	header[0] = RPDUMP_MESSAGE;
+
+	dgrp_encode_time(nd, header + 1);
+
+	n = strlen(message);
+
+	put_unaligned_be16(n, header + 5);
+
+	dgrp_monitor(nd, header, sizeof(header));
+	dgrp_monitor(nd, (u8 *) message, n);
+}
+
+
+
+/**
+ * dgrp_monitor_reset() -- Note a reset in the monitoring buffer.
+ * @nd: pointer to a node structure
+ */
+static void dgrp_monitor_reset(struct nd_struct *nd)
+{
+	u8 header[5];
+
+	header[0] = RPDUMP_RESET;
+
+	dgrp_encode_time(nd, header + 1);
+
+	dgrp_monitor(nd, header, sizeof(header));
+}
+
+/**
+ * dgrp_monitor_data() -- builds a monitor data packet
+ * @nd: pointer to a node structure
+ * @type: type of message to be logged
+ * @buf: data to be logged
+ * @size: number of bytes in the buffer
+ */
+static void dgrp_monitor_data(struct nd_struct *nd, u8 type, u8 *buf, int size)
+{
+	u8 header[7];
+
+	header[0] = type;
+
+	dgrp_encode_time(nd, header + 1);
+
+	put_unaligned_be16(size, header + 5);
+
+	dgrp_monitor(nd, header, sizeof(header));
+	dgrp_monitor(nd, buf, size);
+}
+
+static int alloc_nd_buffers(struct nd_struct *nd)
+{
+
+	nd->nd_iobuf = NULL;
+	nd->nd_writebuf = NULL;
+	nd->nd_inputbuf = NULL;
+	nd->nd_inputflagbuf = NULL;
+
+	/*
+	 *  Allocate the network read/write buffer.
+	 */
+	nd->nd_iobuf = kzalloc(UIO_MAX + 10, GFP_KERNEL);
+	if (!nd->nd_iobuf)
+		goto out_err;
+
+	/*
+	 * Allocate a buffer for doing the copy from user space to
+	 * kernel space in the write routines.
+	 */
+	nd->nd_writebuf = kzalloc(WRITEBUFLEN, GFP_KERNEL);
+	if (!nd->nd_writebuf)
+		goto out_err;
+
+	/*
+	 * Allocate a buffer for doing the copy from kernel space to
+	 * tty buffer space in the read routines.
+	 */
+	nd->nd_inputbuf = kzalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!nd->nd_inputbuf)
+		goto out_err;
+
+	/*
+	 * Allocate a buffer for doing the copy from kernel space to
+	 * tty buffer space in the read routines.
+	 */
+	nd->nd_inputflagbuf = kzalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!nd->nd_inputflagbuf)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	kfree(nd->nd_iobuf);
+	kfree(nd->nd_writebuf);
+	kfree(nd->nd_inputbuf);
+	kfree(nd->nd_inputflagbuf);
+	return -ENOMEM;
+}
+
+/*
+ * dgrp_net_open() -- Open the NET device for a particular PortServer
+ */
+static int dgrp_net_open(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+	struct proc_dir_entry *de;
+	ulong  lock_flags;
+	int rtn;
+
+	rtn = try_module_get(THIS_MODULE);
+	if (!rtn)
+		return -EAGAIN;
+
+	if (!capable(CAP_SYS_ADMIN)) {
+		rtn = -EPERM;
+		goto done;
+	}
+
+	/*
+	 *  Make sure that the "private_data" field hasn't already been used.
+	 */
+	if (file->private_data) {
+		rtn = -EINVAL;
+		goto done;
+	}
+
+	/*
+	 *  Get the node pointer, and fail if it doesn't exist.
+	 */
+	de = PDE(inode);
+	if (!de) {
+		rtn = -ENXIO;
+		goto done;
+	}
+
+	nd = (struct nd_struct *) de->data;
+	if (!nd) {
+		rtn = -ENXIO;
+		goto done;
+	}
+
+	file->private_data = (void *) nd;
+
+	/*
+	 *  Grab the NET lock.
+	 */
+	down(&nd->nd_net_semaphore);
+
+	if (nd->nd_state != NS_CLOSED) {
+		rtn = -EBUSY;
+		goto unlock;
+	}
+
+	/*
+	 *  Initialize the link speed parameters.
+	 */
+
+	nd->nd_link.lk_fast_rate = UIO_MAX;
+	nd->nd_link.lk_slow_rate = UIO_MAX;
+
+	nd->nd_link.lk_fast_delay = 1000;
+	nd->nd_link.lk_slow_delay = 1000;
+
+	nd->nd_link.lk_header_size = 46;
+
+
+	rtn = alloc_nd_buffers(nd);
+	if (rtn)
+		goto unlock;
+
+	/*
+	 *  The port is now open, so move it to the IDLE state
+	 */
+	dgrp_net_idle(nd);
+
+	nd->nd_tx_time = jiffies;
+
+	/*
+	 *  If the polling routing is not running, start it running here
+	 */
+	spin_lock_irqsave(&dgrp_poll_data.poll_lock, lock_flags);
+
+	if (!dgrp_poll_data.node_active_count) {
+		dgrp_poll_data.node_active_count = 2;
+		dgrp_poll_data.timer.expires = jiffies +
+			dgrp_poll_tick * HZ / 1000;
+		add_timer(&dgrp_poll_data.timer);
+	}
+
+	spin_unlock_irqrestore(&dgrp_poll_data.poll_lock, lock_flags);
+
+	dgrp_monitor_message(nd, "Net Open");
+
+unlock:
+	/*
+	 *  Release the NET lock.
+	 */
+	up(&nd->nd_net_semaphore);
+
+done:
+	if (rtn)
+		module_put(THIS_MODULE);
+
+	return rtn;
+}
+
+/* dgrp_net_release() -- close the NET device for a particular PortServer */
+static int dgrp_net_release(struct inode *inode, struct file *file)
+{
+	struct nd_struct *nd;
+	ulong  lock_flags;
+
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		goto done;
+
+/* TODO : historical locking placeholder */
+/*
+ *  In the HPUX version of the RealPort driver (which served as a basis
+ *  for this driver) this locking code was used.  Saved if ever we need
+ *  to review the locking under Linux.
+ */
+/*	spinlock(&nd->nd_lock); */
+
+
+	/*
+	 *  Grab the NET lock.
+	 */
+	down(&nd->nd_net_semaphore);
+
+	/*
+	 *  Before "closing" the internal connection, make sure all
+	 *  ports are "idle".
+	 */
+	dgrp_net_idle(nd);
+
+	nd->nd_state = NS_CLOSED;
+	nd->nd_flag = 0;
+
+	/*
+	 *  TODO ... must the wait queue be reset on close?
+	 *  should any pending waiters be reset?
+	 *  Let's decide to assert that the waitq is empty... and see
+	 *  how soon we break.
+	 */
+	if (waitqueue_active(&nd->nd_tx_waitq))
+		pr_info("%s - expected waitqueue_active to be false\n",
+			__func__);
+
+	nd->nd_send = 0;
+
+	kfree(nd->nd_iobuf);
+	nd->nd_iobuf = NULL;
+
+/* TODO : historical locking placeholder */
+/*
+ *  In the HPUX version of the RealPort driver (which served as a basis
+ *  for this driver) this locking code was used.  Saved if ever we need
+ *  to review the locking under Linux.
+ */
+/*	spinunlock( &nd->nd_lock ); */
+
+
+	kfree(nd->nd_writebuf);
+	nd->nd_writebuf = NULL;
+
+	kfree(nd->nd_inputbuf);
+	nd->nd_inputbuf = NULL;
+
+	kfree(nd->nd_inputflagbuf);
+	nd->nd_inputflagbuf = NULL;
+
+/* TODO : historical locking placeholder */
+/*
+ *  In the HPUX version of the RealPort driver (which served as a basis
+ *  for this driver) this locking code was used.  Saved if ever we need
+ *  to review the locking under Linux.
+ */
+/*	spinlock(&nd->nd_lock); */
+
+	/*
+	 *  Set the active port count to zero.
+	 */
+	dgrp_chan_count(nd, 0);
+
+/* TODO : historical locking placeholder */
+/*
+ *  In the HPUX version of the RealPort driver (which served as a basis
+ *  for this driver) this locking code was used.  Saved if ever we need
+ *  to review the locking under Linux.
+ */
+/*	spinunlock(&nd->nd_lock); */
+
+	/*
+	 *  Release the NET lock.
+	 */
+	up(&nd->nd_net_semaphore);
+
+	/*
+	 *  Cause the poller to stop scheduling itself if this is
+	 *  the last active node.
+	 */
+	spin_lock_irqsave(&dgrp_poll_data.poll_lock, lock_flags);
+
+	if (dgrp_poll_data.node_active_count == 2) {
+		del_timer(&dgrp_poll_data.timer);
+		dgrp_poll_data.node_active_count = 0;
+	}
+
+	spin_unlock_irqrestore(&dgrp_poll_data.poll_lock, lock_flags);
+
+done:
+	down(&nd->nd_net_semaphore);
+
+	dgrp_monitor_message(nd, "Net Close");
+
+	up(&nd->nd_net_semaphore);
+
+	module_put(THIS_MODULE);
+	file->private_data = NULL;
+	return 0;
+}
+
+/* used in dgrp_send to setup command header */
+static inline u8 *set_cmd_header(u8 *b, u8 port, u8 cmd)
+{
+	*b++ = 0xb0 + (port & 0x0f);
+	*b++ = cmd;
+	return b;
+}
+
+/**
+ * dgrp_send() -- build a packet for transmission to the server
+ * @nd: pointer to a node structure
+ * @tmax: maximum bytes to transmit
+ *
+ * returns number of bytes sent
+ */
+static int dgrp_send(struct nd_struct *nd, long tmax)
+{
+	struct ch_struct *ch = nd->nd_chan;
+	u8 *b;
+	u8 *buf;
+	u8 *mbuf;
+	u8 port;
+	int mod;
+	long send;
+	int maxport;
+	long lastport = -1;
+	ushort rwin;
+	long in;
+	ushort n;
+	long t;
+	long ttotal;
+	long tchan;
+	long tsend;
+	ushort tsafe;
+	long work;
+	long send_sync;
+	long wanted_sync_port = -1;
+	ushort tdata[CHAN_MAX];
+	long used_buffer;
+
+	mbuf = nd->nd_iobuf + UIO_BASE;
+	buf = b = mbuf;
+
+	send_sync = nd->nd_link.lk_slow_rate < UIO_MAX;
+
+	ttotal = 0;
+	tchan = 0;
+
+	memset(tdata, 0, sizeof(tdata));
+
+
+	/*
+	 * If there are any outstanding requests to be serviced,
+	 * service them here.
+	 */
+	if (nd->nd_send & NR_PASSWORD) {
+
+		/*
+		 *  Send Password response.
+		 */
+
+		b[0] = 0xfc;
+		b[1] = 0x20;
+		put_unaligned_be16(strlen(nd->password), b + 2);
+		b += 4;
+		b += strlen(nd->password);
+		nd->nd_send &= ~(NR_PASSWORD);
+	}
+
+
+	/*
+	 *  Loop over all modules to generate commands, and determine
+	 *  the amount of data queued for transmit.
+	 */
+
+	for (mod = 0, port = 0; port < nd->nd_chan_count; mod++) {
+		/*
+		 *  If this is not the current module, enter a module select
+		 *  code in the buffer.
+		 */
+
+		if (mod != nd->nd_tx_module)
+			mbuf = ++b;
+
+		/*
+		 *  Loop to process one module.
+		 */
+
+		maxport = port + 16;
+
+		if (maxport > nd->nd_chan_count)
+			maxport = nd->nd_chan_count;
+
+		for (; port < maxport; port++, ch++) {
+			/*
+			 *  Switch based on channel state.
+			 */
+
+			switch (ch->ch_state) {
+			/*
+			 *  Send requests when the port is closed, and there
+			 *  are no Open, Close or Cancel requests expected.
+			 */
+
+			case CS_IDLE:
+				/*
+				 * Wait until any open error code
+				 * has been delivered to all
+				 * associated ports.
+				 */
+
+				if (ch->ch_open_error) {
+					if (ch->ch_wait_count[ch->ch_otype]) {
+						work = 1;
+						break;
+					}
+
+					ch->ch_open_error = 0;
+				}
+
+				/*
+				 *  Wait until the channel HANGUP flag is reset
+				 *  before sending the first open.  We can only
+				 *  get to this state after a server disconnect.
+				 */
+
+				if ((ch->ch_flag & CH_HANGUP) != 0)
+					break;
+
+				/*
+				 *  If recovering from a TCP disconnect, or if
+				 *  there is an immediate open pending, send an
+				 *  Immediate Open request.
+				 */
+				if ((ch->ch_flag & CH_PORT_GONE) ||
+				    ch->ch_wait_count[OTYPE_IMMEDIATE] != 0) {
+					b = set_cmd_header(b, port, 10);
+					*b++ = 0;
+
+					ch->ch_state = CS_WAIT_OPEN;
+					ch->ch_otype = OTYPE_IMMEDIATE;
+					break;
+				}
+
+	/*
+	 *  If there is no Persistent or Incoming Open on the wait
+	 *  list in the server, and a thread is waiting for a
+	 *  Persistent or Incoming Open, send a Persistent or Incoming
+	 *  Open Request.
+	 */
+				if (ch->ch_otype_waiting == 0) {
+					if (ch->ch_wait_count[OTYPE_PERSISTENT] != 0) {
+						b = set_cmd_header(b, port, 10);
+						*b++ = 1;
+
+						ch->ch_state = CS_WAIT_OPEN;
+						ch->ch_otype = OTYPE_PERSISTENT;
+					} else if (ch->ch_wait_count[OTYPE_INCOMING] != 0) {
+						b = set_cmd_header(b, port, 10);
+						*b++ = 2;
+
+						ch->ch_state = CS_WAIT_OPEN;
+						ch->ch_otype = OTYPE_INCOMING;
+					}
+					break;
+				}
+
+				/*
+				 *  If a Persistent or Incoming Open is pending in
+				 *  the server, but there is no longer an open
+				 *  thread waiting for it, cancel the request.
+				 */
+
+				if (ch->ch_wait_count[ch->ch_otype_waiting] == 0) {
+					b = set_cmd_header(b, port, 10);
+					*b++ = 4;
+
+					ch->ch_state = CS_WAIT_CANCEL;
+					ch->ch_otype = ch->ch_otype_waiting;
+				}
+				break;
+
+				/*
+				 *  Send port parameter queries.
+				 */
+			case CS_SEND_QUERY:
+				/*
+				 *  Clear out all FEP state that might remain
+				 *  from the last connection.
+				 */
+
+				ch->ch_flag |= CH_PARAM;
+
+				ch->ch_flag &= ~CH_RX_FLUSH;
+
+				ch->ch_expect = 0;
+
+				ch->ch_s_tin   = 0;
+				ch->ch_s_tpos  = 0;
+				ch->ch_s_tsize = 0;
+				ch->ch_s_treq  = 0;
+				ch->ch_s_elast = 0;
+
+				ch->ch_s_rin   = 0;
+				ch->ch_s_rwin  = 0;
+				ch->ch_s_rsize = 0;
+
+				ch->ch_s_tmax  = 0;
+				ch->ch_s_ttime = 0;
+				ch->ch_s_rmax  = 0;
+				ch->ch_s_rtime = 0;
+				ch->ch_s_rlow  = 0;
+				ch->ch_s_rhigh = 0;
+
+				ch->ch_s_brate = 0;
+				ch->ch_s_iflag = 0;
+				ch->ch_s_cflag = 0;
+				ch->ch_s_oflag = 0;
+				ch->ch_s_xflag = 0;
+
+				ch->ch_s_mout  = 0;
+				ch->ch_s_mflow = 0;
+				ch->ch_s_mctrl = 0;
+				ch->ch_s_xon   = 0;
+				ch->ch_s_xoff  = 0;
+				ch->ch_s_lnext = 0;
+				ch->ch_s_xxon  = 0;
+				ch->ch_s_xxoff = 0;
+
+				/* Send Sequence Request */
+				b = set_cmd_header(b, port, 14);
+
+				/* Configure Event Conditions Packet */
+				b = set_cmd_header(b, port, 42);
+				put_unaligned_be16(0x02c0, b);
+				b += 2;
+				*b++ = (DM_DTR | DM_RTS | DM_CTS |
+					DM_DSR | DM_RI | DM_CD);
+
+				/* Send Status Request */
+				b = set_cmd_header(b, port, 16);
+
+				/* Send Buffer Request  */
+				b = set_cmd_header(b, port, 20);
+
+				/* Send Port Capability Request */
+				b = set_cmd_header(b, port, 22);
+
+				ch->ch_expect = (RR_SEQUENCE |
+						 RR_STATUS  |
+						 RR_BUFFER |
+						 RR_CAPABILITY);
+
+				ch->ch_state = CS_WAIT_QUERY;
+
+				/* Raise modem signals */
+				b = set_cmd_header(b, port, 44);
+
+				if (ch->ch_flag & CH_PORT_GONE)
+					ch->ch_s_mout = ch->ch_mout;
+				else
+					ch->ch_s_mout = ch->ch_mout = DM_DTR | DM_RTS;
+
+				*b++ = ch->ch_mout;
+				*b++ = ch->ch_s_mflow = 0;
+				*b++ = ch->ch_s_mctrl = ch->ch_mctrl = 0;
+
+				if (ch->ch_flag & CH_PORT_GONE)
+					ch->ch_flag &= ~CH_PORT_GONE;
+
+				break;
+
+			/*
+			 *  Handle normal open and ready mode.
+			 */
+
+			case CS_READY:
+
+				/*
+				 *  If the port is not open, and there are no
+				 *  no longer any ports requesting an open,
+				 *  then close the port.
+				 */
+
+				if (ch->ch_open_count == 0 &&
+				    ch->ch_wait_count[ch->ch_otype] == 0) {
+					goto send_close;
+				}
+
+	/*
+	 *  Process waiting input.
+	 *
+	 *  If there is no one to read it, discard the data.
+	 *
+	 *  Otherwise if we are not in fastcook mode, or if there is a
+	 *  fastcook thread waiting for data, send the data to the
+	 *  line discipline.
+	 */
+				if (ch->ch_rin != ch->ch_rout) {
+					if (ch->ch_tun.un_open_count == 0 ||
+					     (ch->ch_tun.un_flag & UN_CLOSING) ||
+					    (ch->ch_cflag & CF_CREAD) == 0) {
+						ch->ch_rout = ch->ch_rin;
+					} else if ((ch->ch_flag & CH_FAST_READ) == 0 ||
+							ch->ch_inwait != 0) {
+						dgrp_input(ch);
+
+						if (ch->ch_rin != ch->ch_rout)
+							work = 1;
+					}
+				}
+
+				/*
+				 *  Handle receive flush, and changes to
+				 *  server port parameters.
+				 */
+
+				if (ch->ch_flag & (CH_RX_FLUSH | CH_PARAM)) {
+				/*
+				 *  If we are in receive flush mode,
+				 *  and enough data has gone by, reset
+				 *  receive flush mode.
+				 */
+					if (ch->ch_flag & CH_RX_FLUSH) {
+						if (((ch->ch_flush_seq - nd->nd_seq_out) & SEQ_MASK) >
+						    ((nd->nd_seq_in - nd->nd_seq_out) & SEQ_MASK))
+							ch->ch_flag &= ~CH_RX_FLUSH;
+						else
+							work = 1;
+					}
+
+					/*
+					 *  Send TMAX, TTIME.
+					 */
+
+					if (ch->ch_s_tmax  != ch->ch_tmax ||
+					    ch->ch_s_ttime != ch->ch_ttime) {
+						b = set_cmd_header(b, port, 48);
+
+						ch->ch_s_tmax = ch->ch_tmax;
+						ch->ch_s_ttime = ch->ch_ttime;
+
+						put_unaligned_be16(ch->ch_s_tmax,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_ttime,
+								   b);
+						b += 2;
+					}
+
+					/*
+					 *  Send RLOW, RHIGH.
+					 */
+
+					if (ch->ch_s_rlow  != ch->ch_rlow ||
+					    ch->ch_s_rhigh != ch->ch_rhigh) {
+						b = set_cmd_header(b, port, 45);
+
+						ch->ch_s_rlow  = ch->ch_rlow;
+						ch->ch_s_rhigh = ch->ch_rhigh;
+
+						put_unaligned_be16(ch->ch_s_rlow,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_rhigh,
+								   b);
+						b += 2;
+					}
+
+					/*
+					 *  Send BRATE, CFLAG, IFLAG,
+					 *  OFLAG, XFLAG.
+					 */
+
+					if (ch->ch_s_brate != ch->ch_brate ||
+					    ch->ch_s_cflag != ch->ch_cflag ||
+					    ch->ch_s_iflag != ch->ch_iflag ||
+					    ch->ch_s_oflag != ch->ch_oflag ||
+					    ch->ch_s_xflag != ch->ch_xflag) {
+						b = set_cmd_header(b, port, 40);
+
+						ch->ch_s_brate = ch->ch_brate;
+						ch->ch_s_cflag = ch->ch_cflag;
+						ch->ch_s_iflag = ch->ch_iflag;
+						ch->ch_s_oflag = ch->ch_oflag;
+						ch->ch_s_xflag = ch->ch_xflag;
+
+						put_unaligned_be16(ch->ch_s_brate,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_cflag,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_iflag,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_oflag,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_xflag,
+								   b);
+						b += 2;
+					}
+
+					/*
+					 *  Send MOUT, MFLOW, MCTRL.
+					 */
+
+					if (ch->ch_s_mout  != ch->ch_mout  ||
+					    ch->ch_s_mflow != ch->ch_mflow ||
+					    ch->ch_s_mctrl != ch->ch_mctrl) {
+						b = set_cmd_header(b, port, 44);
+
+						*b++ = ch->ch_s_mout  = ch->ch_mout;
+						*b++ = ch->ch_s_mflow = ch->ch_mflow;
+						*b++ = ch->ch_s_mctrl = ch->ch_mctrl;
+					}
+
+					/*
+					 *  Send Flow control characters.
+					 */
+
+					if (ch->ch_s_xon   != ch->ch_xon   ||
+					    ch->ch_s_xoff  != ch->ch_xoff  ||
+					    ch->ch_s_lnext != ch->ch_lnext ||
+					    ch->ch_s_xxon  != ch->ch_xxon  ||
+					    ch->ch_s_xxoff != ch->ch_xxoff) {
+						b = set_cmd_header(b, port, 46);
+
+						*b++ = ch->ch_s_xon   = ch->ch_xon;
+						*b++ = ch->ch_s_xoff  = ch->ch_xoff;
+						*b++ = ch->ch_s_lnext = ch->ch_lnext;
+						*b++ = ch->ch_s_xxon  = ch->ch_xxon;
+						*b++ = ch->ch_s_xxoff = ch->ch_xxoff;
+					}
+
+					/*
+					 *  Send RMAX, RTIME.
+					 */
+
+					if (ch->ch_s_rmax != ch->ch_rmax ||
+					    ch->ch_s_rtime != ch->ch_rtime) {
+						b = set_cmd_header(b, port, 47);
+
+						ch->ch_s_rmax  = ch->ch_rmax;
+						ch->ch_s_rtime = ch->ch_rtime;
+
+						put_unaligned_be16(ch->ch_s_rmax,
+								   b);
+						b += 2;
+
+						put_unaligned_be16(ch->ch_s_rtime,
+								   b);
+						b += 2;
+					}
+
+					ch->ch_flag &= ~CH_PARAM;
+					wake_up_interruptible(&ch->ch_flag_wait);
+				}
+
+
+				/*
+				 *  Handle action commands.
+				 */
+
+				if (ch->ch_send != 0) {
+					/* int send = ch->ch_send & ~ch->ch_expect; */
+					send = ch->ch_send & ~ch->ch_expect;
+
+					/* Send character immediate */
+					if ((send & RR_TX_ICHAR) != 0) {
+						b = set_cmd_header(b, port, 60);
+
+						*b++ = ch->ch_xon;
+						ch->ch_expect |= RR_TX_ICHAR;
+					}
+
+					/* BREAK request */
+					if ((send & RR_TX_BREAK) != 0) {
+						if (ch->ch_break_time != 0) {
+							b = set_cmd_header(b, port, 61);
+							put_unaligned_be16(ch->ch_break_time,
+									   b);
+							b += 2;
+
+							ch->ch_expect |= RR_TX_BREAK;
+							ch->ch_break_time = 0;
+						} else {
+							ch->ch_send &= ~RR_TX_BREAK;
+							ch->ch_flag &= ~CH_TX_BREAK;
+							wake_up_interruptible(&ch->ch_flag_wait);
+						}
+					}
+
+					/*
+					 *  Flush input/output buffers.
+					 */
+
+					if ((send & (RR_RX_FLUSH | RR_TX_FLUSH)) != 0) {
+						b = set_cmd_header(b, port, 62);
+
+						*b++ = ((send & RR_TX_FLUSH) == 0 ? 1 :
+							(send & RR_RX_FLUSH) == 0 ? 2 : 3);
+
+						if (send & RR_RX_FLUSH) {
+							ch->ch_flush_seq = nd->nd_seq_in;
+							ch->ch_flag |= CH_RX_FLUSH;
+							work = 1;
+							send_sync = 1;
+							wanted_sync_port = port;
+						}
+
+						ch->ch_send &= ~(RR_RX_FLUSH | RR_TX_FLUSH);
+					}
+
+					/*  Pause input/output */
+					if ((send & (RR_RX_STOP | RR_TX_STOP)) != 0) {
+						b = set_cmd_header(b, port, 63);
+						*b = 0;
+
+						if ((send & RR_TX_STOP) != 0)
+							*b |= EV_OPU;
+
+						if ((send & RR_RX_STOP) != 0)
+							*b |= EV_IPU;
+
+						b++;
+
+						ch->ch_send &= ~(RR_RX_STOP | RR_TX_STOP);
+					}
+
+					/* Start input/output */
+					if ((send & (RR_RX_START | RR_TX_START)) != 0) {
+						b = set_cmd_header(b, port, 64);
+						*b = 0;
+
+						if ((send & RR_TX_START) != 0)
+							*b |= EV_OPU | EV_OPS | EV_OPX;
+
+						if ((send & RR_RX_START) != 0)
+							*b |= EV_IPU | EV_IPS;
+
+						b++;
+
+						ch->ch_send &= ~(RR_RX_START | RR_TX_START);
+					}
+				}
+
+
+				/*
+				 *  Send a window sequence to acknowledge received data.
+				 */
+
+				rwin = (ch->ch_s_rin +
+					((ch->ch_rout - ch->ch_rin - 1) & RBUF_MASK));
+
+				n = (rwin - ch->ch_s_rwin) & 0xffff;
+
+				if (n >= RBUF_MAX / 4) {
+					b[0] = 0xa0 + (port & 0xf);
+					ch->ch_s_rwin = rwin;
+					put_unaligned_be16(rwin, b + 1);
+					b += 3;
+				}
+
+				/*
+				 *  If the terminal is waiting on LOW
+				 *  water or EMPTY, and the condition
+				 *  is now satisfied, call the line
+				 *  discipline to put more data in the
+				 *  buffer.
+				 */
+
+				n = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+
+				if ((ch->ch_tun.un_flag & (UN_EMPTY|UN_LOW)) != 0) {
+					if ((ch->ch_tun.un_flag & UN_LOW) != 0 ?
+					    (n <= TBUF_LOW) :
+					    (n == 0 && ch->ch_s_tpos == ch->ch_s_tin)) {
+						ch->ch_tun.un_flag &= ~(UN_EMPTY|UN_LOW);
+
+						if (waitqueue_active(&((ch->ch_tun.un_tty)->write_wait)))
+							wake_up_interruptible(&((ch->ch_tun.un_tty)->write_wait));
+						tty_wakeup(ch->ch_tun.un_tty);
+						n = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+					}
+				}
+
+				/*
+				 * If the printer is waiting on LOW
+				 * water, TIME, EMPTY or PWAIT, and is
+				 * now ready to put more data in the
+				 * buffer, call the line discipline to
+				 * do the job.
+				 */
+
+				if (ch->ch_pun.un_open_count &&
+				    (ch->ch_pun.un_flag &
+				    (UN_EMPTY|UN_TIME|UN_LOW|UN_PWAIT)) != 0) {
+
+					if ((ch->ch_pun.un_flag & UN_LOW) != 0 ?
+					    (n <= TBUF_LOW) :
+					    (ch->ch_pun.un_flag & UN_TIME) != 0 ?
+					    ((jiffies - ch->ch_waketime) >= 0) :
+					    (n == 0 && ch->ch_s_tpos == ch->ch_s_tin) &&
+					    ((ch->ch_pun.un_flag & UN_EMPTY) != 0 ||
+					    ((ch->ch_tun.un_open_count &&
+					      ch->ch_tun.un_tty->ops->chars_in_buffer) ?
+					     (ch->ch_tun.un_tty->ops->chars_in_buffer)(ch->ch_tun.un_tty) == 0
+					     : 1
+					    )
+					    )) {
+						ch->ch_pun.un_flag &= ~(UN_EMPTY | UN_TIME | UN_LOW | UN_PWAIT);
+
+						if (waitqueue_active(&((ch->ch_pun.un_tty)->write_wait)))
+							wake_up_interruptible(&((ch->ch_pun.un_tty)->write_wait));
+						tty_wakeup(ch->ch_pun.un_tty);
+						n = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+
+					} else if ((ch->ch_pun.un_flag & UN_TIME) != 0) {
+						work = 1;
+					}
+				}
+
+
+				/*
+				 *  Determine the max number of bytes
+				 *  this port can send, including
+				 *  packet header overhead.
+				 */
+
+				t = ((ch->ch_s_tsize + ch->ch_s_tpos - ch->ch_s_tin) & 0xffff);
+
+				if (n > t)
+					n = t;
+
+				if (n != 0) {
+					n += (n <= 8 ? 1 : n <= 255 ? 2 : 3);
+
+					tdata[tchan++] = n;
+					ttotal += n;
+				}
+				break;
+
+			/*
+			 *  Close the port.
+			 */
+
+send_close:
+			case CS_SEND_CLOSE:
+				b = set_cmd_header(b, port, 10);
+				if (ch->ch_otype == OTYPE_IMMEDIATE)
+					*b++ = 3;
+				else
+					*b++ = 4;
+
+				ch->ch_state = CS_WAIT_CLOSE;
+				break;
+
+			/*
+			 *  Wait for a previous server request.
+			 */
+
+			case CS_WAIT_OPEN:
+			case CS_WAIT_CANCEL:
+			case CS_WAIT_FAIL:
+			case CS_WAIT_QUERY:
+			case CS_WAIT_CLOSE:
+				break;
+
+			default:
+				pr_info("%s - unexpected channel state (%i)\n",
+					__func__, ch->ch_state);
+			}
+		}
+
+		/*
+		 *  If a module select code is needed, drop one in.  If space
+		 *  was reserved for one, but none is needed, recover the space.
+		 */
+
+		if (mod != nd->nd_tx_module) {
+			if (b != mbuf) {
+				mbuf[-1] = 0xf0 | mod;
+				nd->nd_tx_module = mod;
+			} else {
+				b--;
+			}
+		}
+	}
+
+	/*
+	 *  Adjust "tmax" so that under worst case conditions we do
+	 *  not overflow either the daemon buffer or the internal
+	 *  buffer in the loop that follows.   Leave a safe area
+	 *  of 64 bytes so we start getting asserts before we start
+	 *  losing data or clobbering memory.
+	 */
+
+	n = UIO_MAX - UIO_BASE;
+
+	if (tmax > n)
+		tmax = n;
+
+	tmax -= 64;
+
+	tsafe = tmax;
+
+	/*
+	 *  Allocate space for 5 Module Selects, 1 Sequence Request,
+	 *  and 1 Set TREQ for each active channel.
+	 */
+
+	tmax -= 5 + 3 + 4 * nd->nd_chan_count;
+
+	/*
+	 *  Further reduce "tmax" to the available transmit credit.
+	 *  Note that this is a soft constraint;  The transmit credit
+	 *  can go negative for a time and then recover.
+	 */
+
+	n = nd->nd_tx_deposit - nd->nd_tx_charge - nd->nd_link.lk_header_size;
+
+	if (tmax > n)
+		tmax = n;
+
+	/*
+	 *  Finally reduce tmax by the number of bytes already in
+	 *  the buffer.
+	 */
+
+	tmax -= b - buf;
+
+	/*
+	 *  Suspend data transmit unless every ready channel can send
+	 *  at least 1 character.
+	 */
+	if (tmax < 2 * nd->nd_chan_count) {
+		tsend = 1;
+
+	} else if (tchan > 1 && ttotal > tmax) {
+
+		/*
+		 *  If transmit is limited by the credit budget, find the
+		 *  largest number of characters we can send without driving
+		 *  the credit negative.
+		 */
+
+		long tm = tmax;
+		int tc = tchan;
+		int try;
+
+		tsend = tm / tc;
+
+		for (try = 0; try < 3; try++) {
+			int i;
+			int c = 0;
+
+			for (i = 0; i < tc; i++) {
+				if (tsend < tdata[i])
+					tdata[c++] = tdata[i];
+				else
+					tm -= tdata[i];
+			}
+
+			if (c == tc)
+				break;
+
+			tsend = tm / c;
+
+			if (c == 1)
+				break;
+
+			tc = c;
+		}
+
+		tsend = tm / nd->nd_chan_count;
+
+		if (tsend < 2)
+			tsend = 1;
+
+	} else {
+		/*
+		 *  If no budgetary constraints, or only one channel ready
+		 *  to send, set the character limit to the remaining
+		 *  buffer size.
+		 */
+
+		tsend = tmax;
+	}
+
+	tsend -= (tsend <= 9) ? 1 : (tsend <= 257) ? 2 : 3;
+
+	/*
+	 *  Loop over all channels, sending queued data.
+	 */
+
+	port = 0;
+	ch = nd->nd_chan;
+	used_buffer = tmax;
+
+	for (mod = 0; port < nd->nd_chan_count; mod++) {
+		/*
+		 *  If this is not the current module, enter a module select
+		 *  code in the buffer.
+		 */
+
+		if (mod != nd->nd_tx_module)
+			mbuf = ++b;
+
+		/*
+		 *  Loop to process one module.
+		 */
+
+		maxport = port + 16;
+
+		if (maxport > nd->nd_chan_count)
+			maxport = nd->nd_chan_count;
+
+		for (; port < maxport; port++, ch++) {
+			if (ch->ch_state != CS_READY)
+				continue;
+
+			lastport = port;
+
+			n = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+
+			/*
+			 *  If there is data that can be sent, send it.
+			 */
+
+			if (n != 0 && used_buffer > 0) {
+				t = (ch->ch_s_tsize + ch->ch_s_tpos - ch->ch_s_tin) & 0xffff;
+
+				if (n > t)
+					n = t;
+
+				if (n > tsend) {
+					work = 1;
+					n = tsend;
+				}
+
+				if (n > used_buffer) {
+					work = 1;
+					n = used_buffer;
+				}
+
+				if (n <= 0)
+					continue;
+
+				/*
+				 *  Create the correct size transmit header,
+				 *  depending on the amount of data to transmit.
+				 */
+
+				if (n <= 8) {
+
+					b[0] = ((n - 1) << 4) + (port & 0xf);
+					b += 1;
+
+				} else if (n <= 255) {
+
+					b[0] = 0x80 + (port & 0xf);
+					b[1] = n;
+					b += 2;
+
+				} else {
+
+					b[0] = 0x90 + (port & 0xf);
+					put_unaligned_be16(n, b + 1);
+					b += 3;
+				}
+
+				ch->ch_s_tin = (ch->ch_s_tin + n) & 0xffff;
+
+				/*
+				 *  Copy transmit data to the packet.
+				 */
+
+				t = TBUF_MAX - ch->ch_tout;
+
+				if (n >= t) {
+					memcpy(b, ch->ch_tbuf + ch->ch_tout, t);
+					b += t;
+					n -= t;
+					used_buffer -= t;
+					ch->ch_tout = 0;
+				}
+
+				memcpy(b, ch->ch_tbuf + ch->ch_tout, n);
+				b += n;
+				used_buffer -= n;
+				ch->ch_tout += n;
+				n = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+			}
+
+			/*
+			 *  Wake any terminal unit process waiting in the
+			 *  dgrp_write routine for low water.
+			 */
+
+			if (n > TBUF_LOW)
+				continue;
+
+			if ((ch->ch_flag & CH_LOW) != 0) {
+				ch->ch_flag &= ~CH_LOW;
+				wake_up_interruptible(&ch->ch_flag_wait);
+			}
+
+			/* selwakeup tty_sel */
+			if (ch->ch_tun.un_open_count) {
+				struct tty_struct *tty = (ch->ch_tun.un_tty);
+
+				if (waitqueue_active(&tty->write_wait))
+					wake_up_interruptible(&tty->write_wait);
+
+				tty_wakeup(tty);
+			}
+
+			if (ch->ch_pun.un_open_count) {
+				struct tty_struct *tty = (ch->ch_pun.un_tty);
+
+				if (waitqueue_active(&tty->write_wait))
+					wake_up_interruptible(&tty->write_wait);
+
+				tty_wakeup(tty);
+			}
+
+			/*
+			 *  Do EMPTY processing.
+			 */
+
+			if (n != 0)
+				continue;
+
+			if ((ch->ch_flag & (CH_EMPTY | CH_DRAIN)) != 0 ||
+			    (ch->ch_pun.un_flag & UN_EMPTY) != 0) {
+				/*
+				 *  If there is still data in the server, ask the server
+				 *  to notify us when its all gone.
+				 */
+
+				if (ch->ch_s_treq != ch->ch_s_tin) {
+					b = set_cmd_header(b, port, 43);
+
+					ch->ch_s_treq = ch->ch_s_tin;
+					put_unaligned_be16(ch->ch_s_treq,
+							   b);
+					b += 2;
+				}
+
+				/*
+				 *  If there is a thread waiting for buffer empty,
+				 *  and we are truly empty, wake the thread.
+				 */
+
+				else if ((ch->ch_flag & CH_EMPTY) != 0 &&
+					(ch->ch_send & RR_TX_BREAK) == 0) {
+					ch->ch_flag &= ~CH_EMPTY;
+
+					wake_up_interruptible(&ch->ch_flag_wait);
+				}
+			}
+		}
+
+		/*
+		 *  If a module select code is needed, drop one in.  If space
+		 *  was reserved for one, but none is needed, recover the space.
+		 */
+
+		if (mod != nd->nd_tx_module) {
+			if (b != mbuf) {
+				mbuf[-1] = 0xf0 | mod;
+				nd->nd_tx_module = mod;
+			} else {
+				b--;
+			}
+		}
+	}
+
+	/*
+	 *  Send a synchronization sequence associated with the last open
+	 *  channel that sent data, and remember the time when the data was
+	 *  sent.
+	 */
+
+	in = nd->nd_seq_in;
+
+	if ((send_sync || nd->nd_seq_wait[in] != 0) && lastport >= 0) {
+		u8 *bb = b;
+
+		/*
+		 * Attempt the use the port that really wanted the sync.
+		 * This gets around a race condition where the "lastport" is in
+		 * the middle of the close() routine, and by the time we
+		 * send this command, it will have already acked the close, and
+		 * thus not send the sync response.
+		 */
+		if (wanted_sync_port >= 0)
+			lastport = wanted_sync_port;
+		/*
+		 * Set a flag just in case the port is in the middle of a close,
+		 * it will not be permitted to actually close until we get an
+		 * sync response, and clear the flag there.
+		 */
+		ch = nd->nd_chan + lastport;
+		ch->ch_flag |= CH_WAITING_SYNC;
+
+		mod = lastport >> 4;
+
+		if (mod != nd->nd_tx_module) {
+			bb[0] = 0xf0 + mod;
+			bb += 1;
+
+			nd->nd_tx_module = mod;
+		}
+
+		bb = set_cmd_header(bb, lastport, 12);
+		*bb++ = in;
+
+		nd->nd_seq_size[in] = bb - buf;
+		nd->nd_seq_time[in] = jiffies;
+
+		if (++in >= SEQ_MAX)
+			in = 0;
+
+		if (in != nd->nd_seq_out) {
+			b = bb;
+			nd->nd_seq_in = in;
+			nd->nd_unack += b - buf;
+		}
+	}
+
+	/*
+	 *  If there are no open ports, a sync cannot be sent.
+	 *  There is nothing left to wait for anyway, so wake any
+	 *  thread waiting for an acknowledgement.
+	 */
+
+	else if (nd->nd_seq_wait[in] != 0) {
+		nd->nd_seq_wait[in] = 0;
+
+		wake_up_interruptible(&nd->nd_seq_wque[in]);
+	}
+
+	/*
+	 *  If there is no traffic for an interval of IDLE_MAX, then
+	 *  send a single byte packet.
+	 */
+
+	if (b != buf) {
+		nd->nd_tx_time = jiffies;
+	} else if ((ulong)(jiffies - nd->nd_tx_time) >= IDLE_MAX) {
+		*b++ = 0xf0 | nd->nd_tx_module;
+		nd->nd_tx_time = jiffies;
+	}
+
+	n = b - buf;
+
+	if (n >= tsafe)
+		pr_info("%s - n(%i) >= tsafe(%i)\n",
+			__func__, n, tsafe);
+
+	if (tsend < 0)
+		dgrp_dump(buf, n);
+
+	nd->nd_tx_work = work;
+
+	return n;
+}
+
+/*
+ * dgrp_net_read()
+ * Data to be sent TO the PortServer from the "async." half of the driver.
+ */
+static ssize_t dgrp_net_read(struct file *file, char __user *buf, size_t count,
+			     loff_t *ppos)
+{
+	struct nd_struct *nd;
+	long n;
+	u8 *local_buf;
+	u8 *b;
+	ssize_t rtn;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		return -ENXIO;
+
+	if (count < UIO_MIN)
+		return -EINVAL;
+
+	/*
+	 *  Only one read/write operation may be in progress at
+	 *  any given time.
+	 */
+
+	/*
+	 *  Grab the NET lock.
+	 */
+	down(&nd->nd_net_semaphore);
+
+	nd->nd_read_count++;
+
+	nd->nd_tx_ready = 0;
+
+	/*
+	 *  Determine the effective size of the buffer.
+	 */
+
+	if (nd->nd_remain > UIO_BASE)
+		pr_info_ratelimited("%s - nd_remain(%i) > UIO_BASE\n",
+				    __func__, nd->nd_remain);
+
+	b = local_buf = nd->nd_iobuf + UIO_BASE;
+
+	/*
+	 *  Generate data according to the node state.
+	 */
+
+	switch (nd->nd_state) {
+	/*
+	 *  Initialize the connection.
+	 */
+
+	case NS_IDLE:
+		if (nd->nd_mon_buf)
+			dgrp_monitor_reset(nd);
+
+		/*
+		 *  Request a Product ID Packet.
+		 */
+
+		b[0] = 0xfb;
+		b[1] = 0x01;
+		b += 2;
+
+		nd->nd_expect |= NR_IDENT;
+
+		/*
+		 *  Request a Server Capability ID Response.
+		 */
+
+		b[0] = 0xfb;
+		b[1] = 0x02;
+		b += 2;
+
+		nd->nd_expect |= NR_CAPABILITY;
+
+		/*
+		 *  Request a Server VPD Response.
+		 */
+
+		b[0] = 0xfb;
+		b[1] = 0x18;
+		b += 2;
+
+		nd->nd_expect |= NR_VPD;
+
+		nd->nd_state = NS_WAIT_QUERY;
+		break;
+
+	/*
+	 *  We do serious communication with the server only in
+	 *  the READY state.
+	 */
+
+	case NS_READY:
+		b = dgrp_send(nd, count) + local_buf;
+		break;
+
+	/*
+	 *  Send off an error after receiving a bogus message
+	 *  from the server.
+	 */
+
+	case NS_SEND_ERROR:
+		n = strlen(nd->nd_error);
+
+		b[0] = 0xff;
+		b[1] = n;
+		memcpy(b + 2, nd->nd_error, n);
+		b += 2 + n;
+
+		dgrp_net_idle(nd);
+		/*
+		 *  Set the active port count to zero.
+		 */
+		dgrp_chan_count(nd, 0);
+		break;
+
+	default:
+		break;
+	}
+
+	n = b - local_buf;
+
+	if (n != 0) {
+		nd->nd_send_count++;
+
+		nd->nd_tx_byte   += n + nd->nd_link.lk_header_size;
+		nd->nd_tx_charge += n + nd->nd_link.lk_header_size;
+	}
+
+	rtn = copy_to_user((void __user *)buf, local_buf, n);
+	if (rtn) {
+		rtn = -EFAULT;
+		goto done;
+	}
+
+	*ppos += n;
+
+	rtn = n;
+
+	if (nd->nd_mon_buf)
+		dgrp_monitor_data(nd, RPDUMP_CLIENT, local_buf, n);
+
+	/*
+	 *  Release the NET lock.
+	 */
+done:
+	up(&nd->nd_net_semaphore);
+
+	return rtn;
+}
+
+/**
+ * dgrp_receive() -- decode data packets received from the remote PortServer.
+ * @nd: pointer to a node structure
+ */
+static void dgrp_receive(struct nd_struct *nd)
+{
+	struct ch_struct *ch;
+	u8 *buf;
+	u8 *b;
+	u8 *dbuf;
+	char *error;
+	long port;
+	long dlen;
+	long plen;
+	long remain;
+	long n;
+	long mlast;
+	long elast;
+	long mstat;
+	long estat;
+
+	char ID[3];
+
+	nd->nd_tx_time = jiffies;
+
+	ID_TO_CHAR(nd->nd_ID, ID);
+
+	b = buf = nd->nd_iobuf;
+	remain = nd->nd_remain;
+
+	/*
+	 *  Loop to process Realport protocol packets.
+	 */
+
+	while (remain > 0) {
+		int n0 = b[0] >> 4;
+		int n1 = b[0] & 0x0f;
+
+		if (n0 <= 12) {
+			port = (nd->nd_rx_module << 4) + n1;
+
+			if (port >= nd->nd_chan_count) {
+				error = "Improper Port Number";
+				goto prot_error;
+			}
+
+			ch = nd->nd_chan + port;
+		} else {
+			port = -1;
+			ch = NULL;
+		}
+
+		/*
+		 *  Process by major packet type.
+		 */
+
+		switch (n0) {
+
+		/*
+		 *  Process 1-byte header data packet.
+		 */
+
+		case 0:
+		case 1:
+		case 2:
+		case 3:
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			dlen = n0 + 1;
+			plen = dlen + 1;
+
+			dbuf = b + 1;
+			goto data;
+
+		/*
+		 *  Process 2-byte header data packet.
+		 */
+
+		case 8:
+			if (remain < 3)
+				goto done;
+
+			dlen = b[1];
+			plen = dlen + 2;
+
+			dbuf = b + 2;
+			goto data;
+
+		/*
+		 *  Process 3-byte header data packet.
+		 */
+
+		case 9:
+			if (remain < 4)
+				goto done;
+
+			dlen = get_unaligned_be16(b + 1);
+			plen = dlen + 3;
+
+			dbuf = b + 3;
+
+		/*
+		 *  Common packet handling code.
+		 */
+
+data:
+			nd->nd_tx_work = 1;
+
+			/*
+			 *  Otherwise data should appear only when we are
+			 *  in the CS_READY state.
+			 */
+
+			if (ch->ch_state < CS_READY) {
+				error = "Data received before RWIN established";
+				goto prot_error;
+			}
+
+			/*
+			 *  Assure that the data received is within the
+			 *  allowable window.
+			 */
+
+			n = (ch->ch_s_rwin - ch->ch_s_rin) & 0xffff;
+
+			if (dlen > n) {
+				error = "Receive data overrun";
+				goto prot_error;
+			}
+
+			/*
+			 *  If we received 3 or less characters,
+			 *  assume it is a human typing, and set RTIME
+			 *  to 10 milliseconds.
+			 *
+			 *  If we receive 10 or more characters,
+			 *  assume its not a human typing, and set RTIME
+			 *  to 100 milliseconds.
+			 */
+
+			if (ch->ch_edelay != DGRP_RTIME) {
+				if (ch->ch_rtime != ch->ch_edelay) {
+					ch->ch_rtime = ch->ch_edelay;
+					ch->ch_flag |= CH_PARAM;
+				}
+			} else if (dlen <= 3) {
+				if (ch->ch_rtime != 10) {
+					ch->ch_rtime = 10;
+					ch->ch_flag |= CH_PARAM;
+				}
+			} else {
+				if (ch->ch_rtime != DGRP_RTIME) {
+					ch->ch_rtime = DGRP_RTIME;
+					ch->ch_flag |= CH_PARAM;
+				}
+			}
+
+			/*
+			 *  If a portion of the packet is outside the
+			 *  buffer, shorten the effective length of the
+			 *  data packet to be the amount of data received.
+			 */
+
+			if (remain < plen)
+				dlen -= plen - remain;
+
+			/*
+			 *  Detect if receive flush is now complete.
+			 */
+
+			if ((ch->ch_flag & CH_RX_FLUSH) != 0 &&
+			    ((ch->ch_flush_seq - nd->nd_seq_out) & SEQ_MASK) >=
+			    ((nd->nd_seq_in    - nd->nd_seq_out) & SEQ_MASK)) {
+				ch->ch_flag &= ~CH_RX_FLUSH;
+			}
+
+			/*
+			 *  If we are ready to receive, move the data into
+			 *  the receive buffer.
+			 */
+
+			ch->ch_s_rin = (ch->ch_s_rin + dlen) & 0xffff;
+
+			if (ch->ch_state == CS_READY &&
+			    (ch->ch_tun.un_open_count != 0) &&
+			    (ch->ch_tun.un_flag & UN_CLOSING) == 0 &&
+			    (ch->ch_cflag & CF_CREAD) != 0 &&
+			    (ch->ch_flag & (CH_BAUD0 | CH_RX_FLUSH)) == 0 &&
+			    (ch->ch_send & RR_RX_FLUSH) == 0) {
+
+				if (ch->ch_rin + dlen >= RBUF_MAX) {
+					n = RBUF_MAX - ch->ch_rin;
+
+					memcpy(ch->ch_rbuf + ch->ch_rin, dbuf, n);
+
+					ch->ch_rin = 0;
+					dbuf += n;
+					dlen -= n;
+				}
+
+				memcpy(ch->ch_rbuf + ch->ch_rin, dbuf, dlen);
+
+				ch->ch_rin += dlen;
+
+
+				/*
+				 *  If we are not in fastcook mode, or
+				 *  if there is a fastcook thread
+				 *  waiting for data, send the data to
+				 *  the line discipline.
+				 */
+
+				if ((ch->ch_flag & CH_FAST_READ) == 0 ||
+				    ch->ch_inwait != 0) {
+					dgrp_input(ch);
+				}
+
+				/*
+				 *  If there is a read thread waiting
+				 *  in select, and we are in fastcook
+				 *  mode, wake him up.
+				 */
+
+				if (waitqueue_active(&ch->ch_tun.un_tty->read_wait) &&
+				    (ch->ch_flag & CH_FAST_READ) != 0)
+					wake_up_interruptible(&ch->ch_tun.un_tty->read_wait);
+
+				/*
+				 * Wake any thread waiting in the
+				 * fastcook loop.
+				 */
+
+				if ((ch->ch_flag & CH_INPUT) != 0) {
+					ch->ch_flag &= ~CH_INPUT;
+
+					wake_up_interruptible(&ch->ch_flag_wait);
+				}
+			}
+
+			/*
+			 *  Fabricate and insert a data packet header to
+			 *  preceed the remaining data when it comes in.
+			 */
+
+			if (remain < plen) {
+				dlen = plen - remain;
+				b = buf;
+
+				b[0] = 0x90 + n1;
+				put_unaligned_be16(dlen, b + 1);
+
+				remain = 3;
+				goto done;
+			}
+			break;
+
+		/*
+		 *  Handle Window Sequence packets.
+		 */
+
+		case 10:
+			plen = 3;
+			if (remain < plen)
+				goto done;
+
+			nd->nd_tx_work = 1;
+
+			{
+				ushort tpos   = get_unaligned_be16(b + 1);
+
+				ushort ack    = (tpos          - ch->ch_s_tpos) & 0xffff;
+				ushort unack  = (ch->ch_s_tin  - ch->ch_s_tpos) & 0xffff;
+				ushort notify = (ch->ch_s_treq - ch->ch_s_tpos) & 0xffff;
+
+				if (ch->ch_state < CS_READY || ack > unack) {
+					error = "Improper Window Sequence";
+					goto prot_error;
+				}
+
+				ch->ch_s_tpos = tpos;
+
+				if (notify <= ack)
+					ch->ch_s_treq = tpos;
+			}
+			break;
+
+		/*
+		 *  Handle Command response packets.
+		 */
+
+		case 11:
+
+			/*
+			 * RealPort engine fix - 03/11/2004
+			 *
+			 * This check did not used to be here.
+			 *
+			 * We were using b[1] without verifying that the data
+			 * is actually there and valid. On a split packet, it
+			 * might not be yet.
+			 *
+			 * NOTE:  I have never actually seen the failure happen
+			 *        under Linux,  but since I have seen it occur
+			 *        under both Solaris and HP-UX,  the assumption
+			 *        is that it *could* happen here as well...
+			 */
+			if (remain < 2)
+				goto done;
+
+
+			switch (b[1]) {
+
+			/*
+			 *  Handle Open Response.
+			 */
+
+			case 11:
+				plen = 6;
+				if (remain < plen)
+					goto done;
+
+				nd->nd_tx_work = 1;
+
+				{
+					int req = b[2];
+					int resp = b[3];
+					port = get_unaligned_be16(b + 4);
+
+					if (port >= nd->nd_chan_count) {
+						error = "Open channel number out of range";
+						goto prot_error;
+					}
+
+					ch = nd->nd_chan + port;
+
+					/*
+					 *  How we handle an open response depends primarily
+					 *  on our current channel state.
+					 */
+
+					switch (ch->ch_state) {
+					case CS_IDLE:
+
+						/*
+						 *  Handle a delayed open.
+						 */
+
+						if (ch->ch_otype_waiting != 0 &&
+						    req == ch->ch_otype_waiting &&
+						    resp == 0) {
+							ch->ch_otype = req;
+							ch->ch_otype_waiting = 0;
+							ch->ch_state = CS_SEND_QUERY;
+							break;
+						}
+						goto open_error;
+
+					case CS_WAIT_OPEN:
+
+						/*
+						 *  Handle the open response.
+						 */
+
+						if (req == ch->ch_otype) {
+							switch (resp) {
+
+							/*
+							 *  On successful response, open the
+							 *  port and proceed normally.
+							 */
+
+							case 0:
+								ch->ch_state = CS_SEND_QUERY;
+								break;
+
+							/*
+							 *  On a busy response to a persistent open,
+							 *  remember that the open is pending.
+							 */
+
+							case 1:
+							case 2:
+								if (req != OTYPE_IMMEDIATE) {
+									ch->ch_otype_waiting = req;
+									ch->ch_state = CS_IDLE;
+									break;
+								}
+
+							/*
+							 *  Otherwise the server open failed.  If
+							 *  the Unix port is open, hang it up.
+							 */
+
+							default:
+								if (ch->ch_open_count != 0) {
+									ch->ch_flag |= CH_HANGUP;
+									dgrp_carrier(ch);
+									ch->ch_state = CS_IDLE;
+									break;
+								}
+
+								ch->ch_open_error = resp;
+								ch->ch_state = CS_IDLE;
+
+								wake_up_interruptible(&ch->ch_flag_wait);
+							}
+							break;
+						}
+
+						/*
+						 *  Handle delayed response arrival preceeding
+						 *  the open response we are waiting for.
+						 */
+
+						if (ch->ch_otype_waiting != 0 &&
+						    req == ch->ch_otype_waiting &&
+						    resp == 0) {
+							ch->ch_otype = ch->ch_otype_waiting;
+							ch->ch_otype_waiting = 0;
+							ch->ch_state = CS_WAIT_FAIL;
+							break;
+						}
+						goto open_error;
+
+
+					case CS_WAIT_FAIL:
+
+						/*
+						 *  Handle response to immediate open arriving
+						 *  after a delayed open success.
+						 */
+
+						if (req == OTYPE_IMMEDIATE) {
+							ch->ch_state = CS_SEND_QUERY;
+							break;
+						}
+						goto open_error;
+
+
+					case CS_WAIT_CANCEL:
+						/*
+						 *  Handle delayed open response arriving before
+						 *  the cancel response.
+						 */
+
+						if (req == ch->ch_otype_waiting &&
+						    resp == 0) {
+							ch->ch_otype_waiting = 0;
+							break;
+						}
+
+						/*
+						 *  Handle cancel response.
+						 */
+
+						if (req == 4 && resp == 0) {
+							ch->ch_otype_waiting = 0;
+							ch->ch_state = CS_IDLE;
+							break;
+						}
+						goto open_error;
+
+
+					case CS_WAIT_CLOSE:
+						/*
+						 *  Handle a successful response to a port
+						 *  close.
+						 */
+
+						if (req >= 3) {
+							ch->ch_state = CS_IDLE;
+							break;
+						}
+						goto open_error;
+
+open_error:
+					default:
+						{
+							error = "Improper Open Response";
+							goto prot_error;
+						}
+					}
+				}
+				break;
+
+			/*
+			 *  Handle Synchronize Response.
+			 */
+
+			case 13:
+				plen = 3;
+				if (remain < plen)
+					goto done;
+				{
+					int seq = b[2];
+					int s;
+
+					/*
+					 * If channel was waiting for this sync response,
+					 * unset the flag, and wake up anyone waiting
+					 * on the event.
+					 */
+					if (ch->ch_flag & CH_WAITING_SYNC) {
+						ch->ch_flag &= ~(CH_WAITING_SYNC);
+						wake_up_interruptible(&ch->ch_flag_wait);
+					}
+
+					if (((seq - nd->nd_seq_out) & SEQ_MASK) >=
+					    ((nd->nd_seq_in - nd->nd_seq_out) & SEQ_MASK)) {
+						break;
+					}
+
+					for (s = nd->nd_seq_out;; s = (s + 1) & SEQ_MASK) {
+						if (nd->nd_seq_wait[s] != 0) {
+							nd->nd_seq_wait[s] = 0;
+
+							wake_up_interruptible(&nd->nd_seq_wque[s]);
+						}
+
+						nd->nd_unack -= nd->nd_seq_size[s];
+
+						if (s == seq)
+							break;
+					}
+
+					nd->nd_seq_out = (seq + 1) & SEQ_MASK;
+				}
+				break;
+
+			/*
+			 *  Handle Sequence Response.
+			 */
+
+			case 15:
+				plen = 6;
+				if (remain < plen)
+					goto done;
+
+				{
+				/* Record that we have received the Sequence
+				 * Response, but we aren't interested in the
+				 * sequence numbers.  We were using RIN like it
+				 * was ROUT and that was causing problems,
+				 * fixed 7-13-2001 David Fries. See comment in
+				 * drp.h for ch_s_rin variable.
+					int rin = get_unaligned_be16(b + 2);
+					int tpos = get_unaligned_be16(b + 4);
+				*/
+
+					ch->ch_send   &= ~RR_SEQUENCE;
+					ch->ch_expect &= ~RR_SEQUENCE;
+				}
+				goto check_query;
+
+			/*
+			 *  Handle Status Response.
+			 */
+
+			case 17:
+				plen = 5;
+				if (remain < plen)
+					goto done;
+
+				{
+					ch->ch_s_elast = get_unaligned_be16(b + 2);
+					ch->ch_s_mlast = b[4];
+
+					ch->ch_expect &= ~RR_STATUS;
+					ch->ch_send   &= ~RR_STATUS;
+
+					/*
+					 *  CH_PHYS_CD is cleared because something _could_ be
+					 *  waiting for the initial sense of carrier... and if
+					 *  carrier is high immediately, we want to be sure to
+					 *  wake them as soon as possible.
+					 */
+					ch->ch_flag &= ~CH_PHYS_CD;
+
+					dgrp_carrier(ch);
+				}
+				goto check_query;
+
+			/*
+			 *  Handle Line Error Response.
+			 */
+
+			case 19:
+				plen = 14;
+				if (remain < plen)
+					goto done;
+
+				break;
+
+			/*
+			 *  Handle Buffer Response.
+			 */
+
+			case 21:
+				plen = 6;
+				if (remain < plen)
+					goto done;
+
+				{
+					ch->ch_s_rsize = get_unaligned_be16(b + 2);
+					ch->ch_s_tsize = get_unaligned_be16(b + 4);
+
+					ch->ch_send   &= ~RR_BUFFER;
+					ch->ch_expect &= ~RR_BUFFER;
+				}
+				goto check_query;
+
+			/*
+			 *  Handle Port Capability Response.
+			 */
+
+			case 23:
+				plen = 32;
+				if (remain < plen)
+					goto done;
+
+				{
+					ch->ch_send   &= ~RR_CAPABILITY;
+					ch->ch_expect &= ~RR_CAPABILITY;
+				}
+
+			/*
+			 *  When all queries are complete, set those parameters
+			 *  derived from the query results, then transition
+			 *  to the READY state.
+			 */
+
+check_query:
+				if (ch->ch_state == CS_WAIT_QUERY &&
+				    (ch->ch_expect & (RR_SEQUENCE |
+							RR_STATUS |
+							RR_BUFFER |
+							RR_CAPABILITY)) == 0) {
+					ch->ch_tmax  = ch->ch_s_tsize / 4;
+
+					if (ch->ch_edelay == DGRP_TTIME)
+						ch->ch_ttime = DGRP_TTIME;
+					else
+						ch->ch_ttime = ch->ch_edelay;
+
+					ch->ch_rmax = ch->ch_s_rsize / 4;
+
+					if (ch->ch_edelay == DGRP_RTIME)
+						ch->ch_rtime = DGRP_RTIME;
+					else
+						ch->ch_rtime = ch->ch_edelay;
+
+					ch->ch_rlow  = 2 * ch->ch_s_rsize / 8;
+					ch->ch_rhigh = 6 * ch->ch_s_rsize / 8;
+
+					ch->ch_state = CS_READY;
+
+					nd->nd_tx_work = 1;
+					wake_up_interruptible(&ch->ch_flag_wait);
+
+				}
+				break;
+
+			default:
+				goto decode_error;
+			}
+			break;
+
+		/*
+		 *  Handle Events.
+		 */
+
+		case 12:
+			plen = 4;
+			if (remain < plen)
+				goto done;
+
+			mlast = ch->ch_s_mlast;
+			elast = ch->ch_s_elast;
+
+			mstat = ch->ch_s_mlast = b[1];
+			estat = ch->ch_s_elast = get_unaligned_be16(b + 2);
+
+			/*
+			 *  Handle modem changes.
+			 */
+
+			if (((mstat ^ mlast) & DM_CD) != 0)
+				dgrp_carrier(ch);
+
+
+			/*
+			 *  Handle received break.
+			 */
+
+			if ((estat & ~elast & EV_RXB) != 0 &&
+			    (ch->ch_tun.un_open_count != 0) &&
+			    I_BRKINT(ch->ch_tun.un_tty) &&
+			    !(I_IGNBRK(ch->ch_tun.un_tty))) {
+
+				tty_buffer_request_room(ch->ch_tun.un_tty, 1);
+				tty_insert_flip_char(ch->ch_tun.un_tty, 0, TTY_BREAK);
+				tty_flip_buffer_push(ch->ch_tun.un_tty);
+
+			}
+
+			/*
+			 *  On transmit break complete, if more break traffic
+			 *  is waiting then send it.  Otherwise wake any threads
+			 *  waiting for transmitter empty.
+			 */
+
+			if ((~estat & elast & EV_TXB) != 0 &&
+			    (ch->ch_expect & RR_TX_BREAK) != 0) {
+
+				nd->nd_tx_work = 1;
+
+				ch->ch_expect &= ~RR_TX_BREAK;
+
+				if (ch->ch_break_time != 0) {
+					ch->ch_send |= RR_TX_BREAK;
+				} else {
+					ch->ch_send &= ~RR_TX_BREAK;
+					ch->ch_flag &= ~CH_TX_BREAK;
+					wake_up_interruptible(&ch->ch_flag_wait);
+				}
+			}
+			break;
+
+		case 13:
+		case 14:
+			error = "Unrecognized command";
+			goto prot_error;
+
+		/*
+		 *  Decode Special Codes.
+		 */
+
+		case 15:
+			switch (n1) {
+			/*
+			 *  One byte module select.
+			 */
+
+			case 0:
+			case 1:
+			case 2:
+			case 3:
+			case 4:
+			case 5:
+			case 6:
+			case 7:
+				plen = 1;
+				nd->nd_rx_module = n1;
+				break;
+
+			/*
+			 *  Two byte module select.
+			 */
+
+			case 8:
+				plen = 2;
+				if (remain < plen)
+					goto done;
+
+				nd->nd_rx_module = b[1];
+				break;
+
+			/*
+			 *  ID Request packet.
+			 */
+
+			case 11:
+				if (remain < 4)
+					goto done;
+
+				plen = get_unaligned_be16(b + 2);
+
+				if (plen < 12 || plen > 1000) {
+					error = "Response Packet length error";
+					goto prot_error;
+				}
+
+				nd->nd_tx_work = 1;
+
+				switch (b[1]) {
+				/*
+				 *  Echo packet.
+				 */
+
+				case 0:
+					nd->nd_send |= NR_ECHO;
+					break;
+
+				/*
+				 *  ID Response packet.
+				 */
+
+				case 1:
+					nd->nd_send |= NR_IDENT;
+					break;
+
+				/*
+				 *  ID Response packet.
+				 */
+
+				case 32:
+					nd->nd_send |= NR_PASSWORD;
+					break;
+
+				}
+				break;
+
+			/*
+			 *  Various node-level response packets.
+			 */
+
+			case 12:
+				if (remain < 4)
+					goto done;
+
+				plen = get_unaligned_be16(b + 2);
+
+				if (plen < 4 || plen > 1000) {
+					error = "Response Packet length error";
+					goto prot_error;
+				}
+
+				nd->nd_tx_work = 1;
+
+				switch (b[1]) {
+				/*
+				 *  Echo packet.
+				 */
+
+				case 0:
+					nd->nd_expect &= ~NR_ECHO;
+					break;
+
+				/*
+				 *  Product Response Packet.
+				 */
+
+				case 1:
+					{
+						int desclen;
+
+						nd->nd_hw_ver = (b[8] << 8) | b[9];
+						nd->nd_sw_ver = (b[10] << 8) | b[11];
+						nd->nd_hw_id = b[6];
+						desclen = ((plen - 12) > MAX_DESC_LEN) ? MAX_DESC_LEN :
+							plen - 12;
+
+						if (desclen <= 0) {
+							error = "Response Packet desclen error";
+							goto prot_error;
+						}
+
+						strncpy(nd->nd_ps_desc, b + 12, desclen);
+						nd->nd_ps_desc[desclen] = 0;
+					}
+
+					nd->nd_expect &= ~NR_IDENT;
+					break;
+
+				/*
+				 *  Capability Response Packet.
+				 */
+
+				case 2:
+					{
+						int nn = get_unaligned_be16(b + 4);
+
+						if (nn > CHAN_MAX)
+							nn = CHAN_MAX;
+
+						dgrp_chan_count(nd, nn);
+					}
+
+					nd->nd_expect &= ~NR_CAPABILITY;
+					break;
+
+				/*
+				 *  VPD Response Packet.
+				 */
+
+				case 15:
+					/*
+					 * NOTE: case 15 is here ONLY because the EtherLite
+					 * is broken, and sends a response to 24 back as 15.
+					 * To resolve this, the EtherLite firmware is now
+					 * fixed to send back 24 correctly, but, for backwards
+					 * compatibility, we now have reserved 15 for the
+					 * bad EtherLite response to 24 as well.
+					 */
+
+					/* Fallthru! */
+
+				case 24:
+
+					/*
+					 * If the product doesn't support VPD,
+					 * it will send back a null IDRESP,
+					 * which is a length of 4 bytes.
+					 */
+					if (plen > 4) {
+						memcpy(nd->nd_vpd, b + 4, min(plen - 4, (long) VPDSIZE));
+						nd->nd_vpd_len = min(plen - 4, (long) VPDSIZE);
+					}
+
+					nd->nd_expect &= ~NR_VPD;
+					break;
+
+				default:
+					goto decode_error;
+				}
+
+				if (nd->nd_expect == 0 &&
+				    nd->nd_state == NS_WAIT_QUERY) {
+					nd->nd_state = NS_READY;
+				}
+				break;
+
+			/*
+			 *  Debug packet.
+			 */
+
+			case 14:
+				if (remain < 4)
+					goto done;
+
+				plen = get_unaligned_be16(b + 2) + 4;
+
+				if (plen > 1000) {
+					error = "Debug Packet too large";
+					goto prot_error;
+				}
+
+				if (remain < plen)
+					goto done;
+				break;
+
+			/*
+			 *  Handle reset packet.
+			 */
+
+			case 15:
+				if (remain < 2)
+					goto done;
+
+				plen = 2 + b[1];
+
+				if (remain < plen)
+					goto done;
+
+				nd->nd_tx_work = 1;
+
+				n = b[plen];
+				b[plen] = 0;
+
+				b[plen] = n;
+
+				error = "Client Reset Acknowledge";
+				goto prot_error;
+
+			default:
+				goto decode_error;
+			}
+			break;
+
+		default:
+			goto decode_error;
+		}
+
+		b += plen;
+		remain -= plen;
+	}
+
+	/*
+	 *  When the buffer is exhausted, copy any data left at the
+	 *  top of the buffer back down to the bottom for the next
+	 *  read request.
+	 */
+
+done:
+	if (remain > 0 && b != buf)
+		memcpy(buf, b, remain);
+
+	nd->nd_remain = remain;
+	return;
+
+/*
+ *  Handle a decode error.
+ */
+
+decode_error:
+	error = "Protocol decode error";
+
+/*
+ *  Handle a general protocol error.
+ */
+
+prot_error:
+	nd->nd_remain = 0;
+	nd->nd_state = NS_SEND_ERROR;
+	nd->nd_error = error;
+}
+
+/*
+ * dgrp_net_write() -- write data to the network device.
+ *
+ * A zero byte write indicates that the connection to the RealPort
+ * device has been broken.
+ *
+ * A non-zero write indicates data from the RealPort device.
+ */
+static ssize_t dgrp_net_write(struct file *file, const char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	struct nd_struct *nd;
+	ssize_t rtn = 0;
+	long n;
+	long total = 0;
+
+	/*
+	 *  Get the node pointer, and quit if it doesn't exist.
+	 */
+	nd = (struct nd_struct *)(file->private_data);
+	if (!nd)
+		return -ENXIO;
+
+	/*
+	 *  Grab the NET lock.
+	 */
+	down(&nd->nd_net_semaphore);
+
+	nd->nd_write_count++;
+
+	/*
+	 *  Handle disconnect.
+	 */
+
+	if (count == 0) {
+		dgrp_net_idle(nd);
+		/*
+		 *  Set the active port count to zero.
+		 */
+		dgrp_chan_count(nd, 0);
+		goto unlock;
+	}
+
+	/*
+	 *  Loop to process entire receive packet.
+	 */
+
+	while (count > 0) {
+		n = UIO_MAX - nd->nd_remain;
+
+		if (n > count)
+			n = count;
+
+		nd->nd_rx_byte += n + nd->nd_link.lk_header_size;
+
+		rtn = copy_from_user(nd->nd_iobuf + nd->nd_remain,
+				     (void __user *) buf + total, n);
+		if (rtn) {
+			rtn = -EFAULT;
+			goto unlock;
+		}
+
+		*ppos += n;
+
+		total += n;
+
+		count -= n;
+
+		if (nd->nd_mon_buf)
+			dgrp_monitor_data(nd, RPDUMP_SERVER,
+					  nd->nd_iobuf + nd->nd_remain, n);
+
+		nd->nd_remain += n;
+
+		dgrp_receive(nd);
+	}
+
+	rtn = total;
+
+unlock:
+	/*
+	 *  Release the NET lock.
+	 */
+	up(&nd->nd_net_semaphore);
+
+	return rtn;
+}
+
+
+/*
+ * dgrp_net_select()
+ *  Determine whether a device is ready to be read or written to, and
+ *  sleep if not.
+ */
+static unsigned int dgrp_net_select(struct file *file,
+				    struct poll_table_struct *table)
+{
+	unsigned int retval = 0;
+	struct nd_struct *nd = file->private_data;
+
+	poll_wait(file, &nd->nd_tx_waitq, table);
+
+	if (nd->nd_tx_ready)
+		retval |= POLLIN | POLLRDNORM; /* Conditionally readable */
+
+	retval |= POLLOUT | POLLWRNORM;        /* Always writeable */
+
+	return retval;
+}
+
+/*
+ * dgrp_net_ioctl
+ *
+ * Implement those functions which allow the network daemon to control
+ * the network parameters in the driver.  The ioctls include ones to
+ * get and set the link speed parameters for the PortServer.
+ */
+static long dgrp_net_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg)
+{
+	struct nd_struct  *nd;
+	int    rtn = 0;
+	long   size = _IOC_SIZE(cmd);
+	struct link_struct link;
+
+	nd = file->private_data;
+
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		rtn = access_ok(VERIFY_WRITE, (void __user *) arg, size);
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		rtn = access_ok(VERIFY_READ,  (void __user *) arg, size);
+
+	if (!rtn)
+		return rtn;
+
+	switch (cmd) {
+	case DIGI_SETLINK:
+		if (size != sizeof(struct link_struct))
+			return -EINVAL;
+
+		if (copy_from_user((void *)(&link), (void __user *) arg, size))
+			return -EFAULT;
+
+		if (link.lk_fast_rate < 9600)
+			link.lk_fast_rate = 9600;
+
+		if (link.lk_slow_rate < 2400)
+			link.lk_slow_rate = 2400;
+
+		if (link.lk_fast_rate > 10000000)
+			link.lk_fast_rate = 10000000;
+
+		if (link.lk_slow_rate > link.lk_fast_rate)
+			link.lk_slow_rate = link.lk_fast_rate;
+
+		if (link.lk_fast_delay > 2000)
+			link.lk_fast_delay = 2000;
+
+		if (link.lk_slow_delay > 10000)
+			link.lk_slow_delay = 10000;
+
+		if (link.lk_fast_delay < 60)
+			link.lk_fast_delay = 60;
+
+		if (link.lk_slow_delay < link.lk_fast_delay)
+			link.lk_slow_delay = link.lk_fast_delay;
+
+		if (link.lk_header_size < 2)
+			link.lk_header_size = 2;
+
+		if (link.lk_header_size > 128)
+			link.lk_header_size = 128;
+
+		link.lk_fast_rate /= 8 * 1000 / dgrp_poll_tick;
+		link.lk_slow_rate /= 8 * 1000 / dgrp_poll_tick;
+
+		link.lk_fast_delay /= dgrp_poll_tick;
+		link.lk_slow_delay /= dgrp_poll_tick;
+
+		nd->nd_link = link;
+
+		break;
+
+	case DIGI_GETLINK:
+		if (size != sizeof(struct link_struct))
+			return -EINVAL;
+
+		if (copy_to_user((void __user *)arg, (void *)(&nd->nd_link),
+				 size))
+			return -EFAULT;
+
+		break;
+
+	default:
+		return -EINVAL;
+
+	}
+
+	return 0;
+}
+
+/**
+ * dgrp_poll_handler() -- handler for poll timer
+ *
+ * As each timer expires, it determines (a) whether the "transmit"
+ * waiter needs to be woken up, and (b) whether the poller needs to
+ * be rescheduled.
+ */
+void dgrp_poll_handler(unsigned long arg)
+{
+	struct dgrp_poll_data *poll_data;
+	struct nd_struct *nd;
+	struct link_struct *lk;
+	ulong time;
+	ulong poll_time;
+	ulong freq;
+	ulong lock_flags;
+
+	poll_data = (struct dgrp_poll_data *) arg;
+	freq = 1000 / poll_data->poll_tick;
+	poll_data->poll_round += 17;
+
+	if (poll_data->poll_round >= freq)
+		poll_data->poll_round -= freq;
+
+	/*
+	 * Loop to process all open nodes.
+	 *
+	 * For each node, determine the rate at which it should
+	 * be transmitting data.  Then if the node should wake up
+	 * and transmit data now, enable the net receive select
+	 * to get the transmit going.
+	 */
+
+	list_for_each_entry(nd, &nd_struct_list, list) {
+
+		lk = &nd->nd_link;
+
+		/*
+		 * Decrement statistics.  These are only for use with
+		 * KME, so don't worry that the operations are done
+		 * unlocked, and so the results are occassionally wrong.
+		 */
+
+		nd->nd_read_count -= (nd->nd_read_count +
+				      poll_data->poll_round) / freq;
+		nd->nd_write_count -= (nd->nd_write_count +
+				       poll_data->poll_round) / freq;
+		nd->nd_send_count -= (nd->nd_send_count +
+				      poll_data->poll_round) / freq;
+		nd->nd_tx_byte -= (nd->nd_tx_byte +
+				   poll_data->poll_round) / freq;
+		nd->nd_rx_byte -= (nd->nd_rx_byte +
+				   poll_data->poll_round) / freq;
+
+		/*
+		 * Wake the daemon to transmit data only when there is
+		 * enough byte credit to send data.
+		 *
+		 * The results are approximate because the operations
+		 * are performed unlocked, and we are inspecting
+		 * data asynchronously updated elsewhere.  The whole
+		 * thing is just approximation anyway, so that should
+		 * be okay.
+		 */
+
+		if (lk->lk_slow_rate >= UIO_MAX) {
+
+			nd->nd_delay = 0;
+			nd->nd_rate = UIO_MAX;
+
+			nd->nd_tx_deposit = nd->nd_tx_charge + 3 * UIO_MAX;
+			nd->nd_tx_credit  = 3 * UIO_MAX;
+
+		} else {
+
+			long rate;
+			long delay;
+			long deposit;
+			long charge;
+			long size;
+			long excess;
+
+			long seq_in = nd->nd_seq_in;
+			long seq_out = nd->nd_seq_out;
+
+			/*
+			 * If there are no outstanding packets, run at the
+			 * fastest rate.
+			 */
+
+			if (seq_in == seq_out) {
+				delay = 0;
+				rate = lk->lk_fast_rate;
+			}
+
+			/*
+			 * Otherwise compute the transmit rate based on the
+			 * delay since the oldest packet.
+			 */
+
+			else {
+				/*
+				 * The actual delay is computed as the
+				 * time since the oldest unacknowledged
+				 * packet was sent, minus the time it
+				 * took to send that packet to the server.
+				 */
+
+				delay = ((jiffies - nd->nd_seq_time[seq_out])
+					- (nd->nd_seq_size[seq_out] /
+					lk->lk_fast_rate));
+
+				/*
+				 * If the delay is less than the "fast"
+				 * delay, transmit full speed.  If greater
+				 * than the "slow" delay, transmit at the
+				 * "slow" speed.   In between, interpolate
+				 * between the fast and slow speeds.
+				 */
+
+				rate =
+				  (delay <= lk->lk_fast_delay ?
+				    lk->lk_fast_rate :
+				    delay >= lk->lk_slow_delay ?
+				      lk->lk_slow_rate :
+				      (lk->lk_slow_rate +
+				       (lk->lk_slow_delay - delay) *
+				       (lk->lk_fast_rate - lk->lk_slow_rate) /
+				       (lk->lk_slow_delay - lk->lk_fast_delay)
+				      )
+				  );
+			}
+
+			nd->nd_delay = delay;
+			nd->nd_rate = rate;
+
+			/*
+			 * Increase the transmit credit by depositing the
+			 * current transmit rate.
+			 */
+
+			deposit = nd->nd_tx_deposit;
+			charge  = nd->nd_tx_charge;
+
+			deposit += rate;
+
+			/*
+			 * If the available transmit credit becomes too large,
+			 * reduce the deposit to correct the value.
+			 *
+			 * Too large is the max of:
+			 *		6 times the header size
+			 *		3 times the current transmit rate.
+			 */
+
+			size = 2 * nd->nd_link.lk_header_size;
+
+			if (size < rate)
+				size = rate;
+
+			size *= 3;
+
+			excess = deposit - charge - size;
+
+			if (excess > 0)
+				deposit -= excess;
+
+			nd->nd_tx_deposit = deposit;
+			nd->nd_tx_credit  = deposit - charge;
+
+			/*
+			 * Wake the transmit task only if the transmit credit
+			 * is at least 3 times the transmit header size.
+			 */
+
+			size = 3 * lk->lk_header_size;
+
+			if (nd->nd_tx_credit < size)
+				continue;
+		}
+
+
+		/*
+		 * Enable the READ select to wake the daemon if there
+		 * is useful work for the drp_read routine to perform.
+		 */
+
+		if (waitqueue_active(&nd->nd_tx_waitq) &&
+		    (nd->nd_tx_work != 0 ||
+		    (ulong)(jiffies - nd->nd_tx_time) >= IDLE_MAX)) {
+			nd->nd_tx_ready = 1;
+
+			wake_up_interruptible(&nd->nd_tx_waitq);
+
+			/* not needed */
+			/* nd->nd_flag &= ~ND_SELECT; */
+		}
+	}
+
+
+	/*
+	 * Schedule ourself back at the nominal wakeup interval.
+	 */
+	spin_lock_irqsave(&poll_data->poll_lock, lock_flags);
+
+	poll_data->node_active_count--;
+	if (poll_data->node_active_count > 0) {
+		poll_data->node_active_count++;
+		poll_time = poll_data->timer.expires +
+			poll_data->poll_tick * HZ / 1000;
+
+		time = poll_time - jiffies;
+
+		if (time >= 2 * poll_data->poll_tick)
+			poll_time = jiffies + dgrp_poll_tick * HZ / 1000;
+
+		poll_data->timer.expires = poll_time;
+		add_timer(&poll_data->timer);
+	}
+
+	spin_unlock_irqrestore(&poll_data->poll_lock, lock_flags);
+}
diff --git a/drivers/staging/dgrp/dgrp_ports_ops.c b/drivers/staging/dgrp/dgrp_ports_ops.c
new file mode 100644
index 000000000000..cd1fc2088624
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_ports_ops.c
@@ -0,0 +1,170 @@
+/*
+ *
+ * Copyright 1999-2000 Digi International (www.digi.com)
+ *     James Puzzo <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_ports_ops.c
+ *
+ *  Description:
+ *
+ *     Handle the file operations required for the /proc/dgrp/ports/...
+ *     devices.  Basically gathers tty status for the node and returns it.
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/proc_fs.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+
+#include "dgrp_common.h"
+
+/* File operation declarations */
+static int dgrp_ports_open(struct inode *, struct file *);
+
+static const struct file_operations ports_ops = {
+	.owner   = THIS_MODULE,
+	.open    = dgrp_ports_open,
+	.read    = seq_read,
+	.llseek	 = seq_lseek,
+	.release = seq_release
+};
+
+static struct inode_operations ports_inode_ops = {
+	.permission = dgrp_inode_permission
+};
+
+
+void dgrp_register_ports_hook(struct proc_dir_entry *de)
+{
+	struct nd_struct *node = de->data;
+
+	de->proc_iops = &ports_inode_ops;
+	de->proc_fops = &ports_ops;
+	node->nd_ports_de = de;
+}
+
+static void *dgrp_ports_seq_start(struct seq_file *seq, loff_t *pos)
+{
+	if (*pos == 0)
+		seq_puts(seq, "#num tty_open pr_open tot_wait MSTAT  IFLAG  OFLAG  CFLAG  BPS    DIGIFLAGS\n");
+
+	return pos;
+}
+
+static void *dgrp_ports_seq_next(struct seq_file *seq, void *v, loff_t *pos)
+{
+	struct nd_struct *nd = seq->private;
+
+	if (*pos >= nd->nd_chan_count)
+		return NULL;
+
+	*pos += 1;
+
+	return pos;
+}
+
+static void dgrp_ports_seq_stop(struct seq_file *seq, void *v)
+{
+}
+
+static int dgrp_ports_seq_show(struct seq_file *seq, void *v)
+{
+	loff_t *pos = v;
+	struct nd_struct *nd;
+	struct ch_struct *ch;
+	struct un_struct *tun, *pun;
+	unsigned int totcnt;
+
+	nd = seq->private;
+	if (!nd)
+		return 0;
+
+	if (*pos >= nd->nd_chan_count)
+		return 0;
+
+	ch = &nd->nd_chan[*pos];
+	tun = &ch->ch_tun;
+	pun = &ch->ch_pun;
+
+	/*
+	 * If port is not open and no one is waiting to
+	 * open it, the modem signal values can't be
+	 * trusted, and will be zeroed.
+	 */
+	totcnt = tun->un_open_count +
+		pun->un_open_count +
+		ch->ch_wait_count[0] +
+		ch->ch_wait_count[1] +
+		ch->ch_wait_count[2];
+
+	seq_printf(seq, "%02d      %02d      %02d      %02d     0x%04X 0x%04X 0x%04X 0x%04X %-6d 0x%04X\n",
+		   (int) *pos,
+		   tun->un_open_count,
+		   pun->un_open_count,
+		   ch->ch_wait_count[0] +
+		   ch->ch_wait_count[1] +
+		   ch->ch_wait_count[2],
+		   (totcnt ? ch->ch_s_mlast : 0),
+		   ch->ch_s_iflag,
+		   ch->ch_s_oflag,
+		   ch->ch_s_cflag,
+		   (ch->ch_s_brate ? (1843200 / ch->ch_s_brate) : 0),
+		   ch->ch_digi.digi_flags);
+
+	return 0;
+}
+
+static const struct seq_operations ports_seq_ops = {
+	.start = dgrp_ports_seq_start,
+	.next  = dgrp_ports_seq_next,
+	.stop  = dgrp_ports_seq_stop,
+	.show  = dgrp_ports_seq_show,
+};
+
+/**
+ * dgrp_ports_open -- open the /proc/dgrp/ports/... device
+ * @inode: struct inode *
+ * @file: struct file *
+ *
+ * Open function to open the /proc/dgrp/ports device for a PortServer.
+ * This is the open function for struct file_operations
+ */
+static int dgrp_ports_open(struct inode *inode, struct file *file)
+{
+	struct seq_file *seq;
+	int rtn;
+
+	rtn = seq_open(file, &ports_seq_ops);
+	if (!rtn) {
+		seq = file->private_data;
+		seq->private = PDE(inode)->data;
+	}
+
+	return rtn;
+}
diff --git a/drivers/staging/dgrp/dgrp_specproc.c b/drivers/staging/dgrp/dgrp_specproc.c
new file mode 100644
index 000000000000..28f5c9ab6b43
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_specproc.c
@@ -0,0 +1,822 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     James Puzzo  <jamesp 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_specproc.c
+ *
+ *  Description:
+ *
+ *     Handle the "config" proc entry for the linux realport device driver
+ *     and provide slots for the "net" and "mon" devices
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+#include <linux/cred.h>
+#include <linux/proc_fs.h>
+#include <linux/ctype.h>
+#include <linux/seq_file.h>
+#include <linux/vmalloc.h>
+
+#include "dgrp_common.h"
+
+static struct dgrp_proc_entry dgrp_table[];
+static struct proc_dir_entry *dgrp_proc_dir_entry;
+
+static int dgrp_add_id(long id);
+static int dgrp_remove_nd(struct nd_struct *nd);
+static void unregister_dgrp_device(struct proc_dir_entry *de);
+static void register_dgrp_device(struct nd_struct *node,
+				 struct proc_dir_entry *root,
+				 void (*register_hook)(struct proc_dir_entry *de));
+
+/* File operation declarations */
+static int dgrp_gen_proc_open(struct inode *, struct file *);
+static int dgrp_gen_proc_close(struct inode *, struct file *);
+static int parse_write_config(char *);
+
+
+static const struct file_operations dgrp_proc_file_ops = {
+	.owner   = THIS_MODULE,
+	.open    = dgrp_gen_proc_open,
+	.release = dgrp_gen_proc_close,
+};
+
+static struct inode_operations proc_inode_ops = {
+	.permission = dgrp_inode_permission
+};
+
+
+static void register_proc_table(struct dgrp_proc_entry *,
+				struct proc_dir_entry *);
+static void unregister_proc_table(struct dgrp_proc_entry *,
+				  struct proc_dir_entry *);
+
+static struct dgrp_proc_entry dgrp_net_table[];
+static struct dgrp_proc_entry dgrp_mon_table[];
+static struct dgrp_proc_entry dgrp_ports_table[];
+static struct dgrp_proc_entry dgrp_dpa_table[];
+
+static ssize_t config_proc_write(struct file *file, const char __user *buffer,
+				 size_t count, loff_t *pos);
+
+static int nodeinfo_proc_open(struct inode *inode, struct file *file);
+static int info_proc_open(struct inode *inode, struct file *file);
+static int config_proc_open(struct inode *inode, struct file *file);
+
+static struct file_operations config_proc_file_ops = {
+	.owner	 = THIS_MODULE,
+	.open	 = config_proc_open,
+	.read	 = seq_read,
+	.llseek	 = seq_lseek,
+	.release = seq_release,
+	.write   = config_proc_write
+};
+
+static struct file_operations info_proc_file_ops = {
+	.owner	 = THIS_MODULE,
+	.open	 = info_proc_open,
+	.read	 = seq_read,
+	.llseek	 = seq_lseek,
+	.release = seq_release,
+};
+
+static struct file_operations nodeinfo_proc_file_ops = {
+	.owner	 = THIS_MODULE,
+	.open	 = nodeinfo_proc_open,
+	.read	 = seq_read,
+	.llseek	 = seq_lseek,
+	.release = seq_release,
+};
+
+static struct dgrp_proc_entry dgrp_table[] = {
+	{
+		.id = DGRP_CONFIG,
+		.name = "config",
+		.mode = 0644,
+		.proc_file_ops = &config_proc_file_ops,
+	},
+	{
+		.id = DGRP_INFO,
+		.name = "info",
+		.mode = 0644,
+		.proc_file_ops = &info_proc_file_ops,
+	},
+	{
+		.id = DGRP_NODEINFO,
+		.name = "nodeinfo",
+		.mode = 0644,
+		.proc_file_ops = &nodeinfo_proc_file_ops,
+	},
+	{
+		.id = DGRP_NETDIR,
+		.name = "net",
+		.mode = 0500,
+		.child = dgrp_net_table
+	},
+	{
+		.id = DGRP_MONDIR,
+		.name = "mon",
+		.mode = 0500,
+		.child = dgrp_mon_table
+	},
+	{
+		.id = DGRP_PORTSDIR,
+		.name = "ports",
+		.mode = 0500,
+		.child = dgrp_ports_table
+	},
+	{
+		.id = DGRP_DPADIR,
+		.name = "dpa",
+		.mode = 0500,
+		.child = dgrp_dpa_table
+	}
+};
+
+static struct proc_dir_entry *net_entry_pointer;
+static struct proc_dir_entry *mon_entry_pointer;
+static struct proc_dir_entry *dpa_entry_pointer;
+static struct proc_dir_entry *ports_entry_pointer;
+
+static struct dgrp_proc_entry dgrp_net_table[] = {
+	{0}
+};
+
+static struct dgrp_proc_entry dgrp_mon_table[] = {
+	{0}
+};
+
+static struct dgrp_proc_entry dgrp_ports_table[] = {
+	{0}
+};
+
+static struct dgrp_proc_entry dgrp_dpa_table[] = {
+	{0}
+};
+
+void dgrp_unregister_proc(void)
+{
+	unregister_proc_table(dgrp_table, dgrp_proc_dir_entry);
+	net_entry_pointer = NULL;
+	mon_entry_pointer = NULL;
+	dpa_entry_pointer = NULL;
+	ports_entry_pointer = NULL;
+
+	if (dgrp_proc_dir_entry) {
+		remove_proc_entry(dgrp_proc_dir_entry->name,
+				  dgrp_proc_dir_entry->parent);
+		dgrp_proc_dir_entry = NULL;
+	}
+
+}
+
+void dgrp_register_proc(void)
+{
+	/*
+	 *	Register /proc/dgrp
+	 */
+	dgrp_proc_dir_entry = proc_create("dgrp", S_IFDIR, NULL,
+					  &dgrp_proc_file_ops);
+	register_proc_table(dgrp_table, dgrp_proc_dir_entry);
+}
+
+/*
+ * /proc/sys support
+ */
+static int dgrp_proc_match(int len, const char *name, struct proc_dir_entry *de)
+{
+	if (!de || !de->low_ino)
+		return 0;
+	if (de->namelen != len)
+		return 0;
+	return !memcmp(name, de->name, len);
+}
+
+
+/*
+ *  Scan the entries in table and add them all to /proc at the position
+ *  referred to by "root"
+ */
+static void register_proc_table(struct dgrp_proc_entry *table,
+				struct proc_dir_entry *root)
+{
+	struct proc_dir_entry *de;
+	int len;
+	mode_t mode;
+
+	for (; table->id; table++) {
+		/* Can't do anything without a proc name. */
+		if (!table->name)
+			continue;
+
+		/* Maybe we can't do anything with it... */
+		if (!table->proc_file_ops &&
+		    !table->child) {
+			pr_warn("dgrp: Can't register %s\n",
+				table->name);
+			continue;
+		}
+
+		len = strlen(table->name);
+		mode = table->mode;
+
+		de = NULL;
+		if (!table->child)
+			mode |= S_IFREG;
+		else {
+			mode |= S_IFDIR;
+			for (de = root->subdir; de; de = de->next) {
+				if (dgrp_proc_match(len, table->name, de))
+					break;
+			}
+			/* If the subdir exists already, de is non-NULL */
+		}
+
+		if (!de) {
+			de = create_proc_entry(table->name, mode, root);
+			if (!de)
+				continue;
+			de->data = (void *) table;
+			if (!table->child) {
+				de->proc_iops = &proc_inode_ops;
+				if (table->proc_file_ops)
+					de->proc_fops = table->proc_file_ops;
+				else
+					de->proc_fops = &dgrp_proc_file_ops;
+			}
+		}
+		table->de = de;
+		if (de->mode & S_IFDIR)
+			register_proc_table(table->child, de);
+
+		if (table->id == DGRP_NETDIR)
+			net_entry_pointer = de;
+
+		if (table->id == DGRP_MONDIR)
+			mon_entry_pointer = de;
+
+		if (table->id == DGRP_DPADIR)
+			dpa_entry_pointer = de;
+
+		if (table->id == DGRP_PORTSDIR)
+			ports_entry_pointer = de;
+	}
+}
+
+/*
+ * Unregister a /proc sysctl table and any subdirectories.
+ */
+static void unregister_proc_table(struct dgrp_proc_entry *table,
+				  struct proc_dir_entry *root)
+{
+	struct proc_dir_entry *de;
+	struct nd_struct *tmp;
+
+	list_for_each_entry(tmp, &nd_struct_list, list) {
+		if ((table == dgrp_net_table) && (tmp->nd_net_de)) {
+			unregister_dgrp_device(tmp->nd_net_de);
+			dgrp_remove_node_class_sysfs_files(tmp);
+		}
+
+		if ((table == dgrp_mon_table) && (tmp->nd_mon_de))
+			unregister_dgrp_device(tmp->nd_mon_de);
+
+		if ((table == dgrp_dpa_table) && (tmp->nd_dpa_de))
+			unregister_dgrp_device(tmp->nd_dpa_de);
+
+		if ((table == dgrp_ports_table) && (tmp->nd_ports_de))
+			unregister_dgrp_device(tmp->nd_ports_de);
+	}
+
+	for (; table->id; table++) {
+		de = table->de;
+
+		if (!de)
+			continue;
+		if (de->mode & S_IFDIR) {
+			if (!table->child) {
+				pr_alert("dgrp: malformed sysctl tree on free\n");
+				continue;
+			}
+			unregister_proc_table(table->child, de);
+
+	/* Don't unregister directories which still have entries */
+			if (de->subdir)
+				continue;
+		}
+
+		/* Don't unregister proc entries that are still being used.. */
+		if ((atomic_read(&de->count)) != 1) {
+			pr_alert("proc entry %s in use, not removing\n",
+				de->name);
+			continue;
+		}
+
+		remove_proc_entry(de->name, de->parent);
+		table->de = NULL;
+	}
+}
+
+static int dgrp_gen_proc_open(struct inode *inode, struct file *file)
+{
+	struct proc_dir_entry *de;
+	struct dgrp_proc_entry *entry;
+	int ret = 0;
+
+	de = (struct proc_dir_entry *) PDE(file->f_dentry->d_inode);
+	if (!de || !de->data) {
+		ret = -ENXIO;
+		goto done;
+	}
+
+	entry = (struct dgrp_proc_entry *) de->data;
+	if (!entry) {
+		ret = -ENXIO;
+		goto done;
+	}
+
+	down(&entry->excl_sem);
+
+	if (entry->excl_cnt)
+		ret = -EBUSY;
+	else
+		entry->excl_cnt++;
+
+	up(&entry->excl_sem);
+
+done:
+	return ret;
+}
+
+static int dgrp_gen_proc_close(struct inode *inode, struct file *file)
+{
+	struct proc_dir_entry *de;
+	struct dgrp_proc_entry *entry;
+
+	de = (struct proc_dir_entry *) PDE(file->f_dentry->d_inode);
+	if (!de || !de->data)
+		goto done;
+
+	entry = (struct dgrp_proc_entry *) de->data;
+	if (!entry)
+		goto done;
+
+	down(&entry->excl_sem);
+
+	if (entry->excl_cnt)
+		entry->excl_cnt = 0;
+
+	up(&entry->excl_sem);
+
+done:
+	return 0;
+}
+
+static void *config_proc_start(struct seq_file *m, loff_t *pos)
+{
+	return seq_list_start_head(&nd_struct_list, *pos);
+}
+
+static void *config_proc_next(struct seq_file *p, void *v, loff_t *pos)
+{
+	return seq_list_next(v, &nd_struct_list, pos);
+}
+
+static void config_proc_stop(struct seq_file *m, void *v)
+{
+}
+
+static int config_proc_show(struct seq_file *m, void *v)
+{
+	struct nd_struct *nd;
+	char tmp_id[4];
+
+	if (v == &nd_struct_list) {
+		seq_puts(m, "#-----------------------------------------------------------------------------\n");
+		seq_puts(m, "#                        Avail\n");
+		seq_puts(m, "# ID  Major  State       Ports\n");
+		return 0;
+	}
+
+	nd = list_entry(v, struct nd_struct, list);
+
+	ID_TO_CHAR(nd->nd_ID, tmp_id);
+
+	seq_printf(m, "  %-2.2s  %-5ld  %-10.10s  %-5d\n",
+		   tmp_id,
+		   nd->nd_major,
+		   ND_STATE_STR(nd->nd_state),
+		   nd->nd_chan_count);
+
+	return 0;
+}
+
+static const struct seq_operations proc_config_ops = {
+	.start = config_proc_start,
+	.next  = config_proc_next,
+	.stop  = config_proc_stop,
+	.show  = config_proc_show
+};
+
+static int config_proc_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &proc_config_ops);
+}
+
+
+/*
+ *  When writing configuration information, each "record" (i.e. each
+ *  write) is treated as an independent request.  See the "parse"
+ *  description for more details.
+ */
+static ssize_t config_proc_write(struct file *file, const char __user *buffer,
+				 size_t count, loff_t *pos)
+{
+	ssize_t retval;
+	char *inbuf, *sp;
+	char *line, *ldelim;
+
+	if (count > 32768)
+		return -EINVAL;
+
+	inbuf = sp = vzalloc(count + 1);
+	if (!inbuf)
+		return -ENOMEM;
+
+	if (copy_from_user(inbuf, buffer, count)) {
+		retval = -EFAULT;
+		goto done;
+	}
+
+	inbuf[count] = 0;
+
+	ldelim = "\n";
+
+	line = strpbrk(sp, ldelim);
+	while (line) {
+		*line = 0;
+		retval = parse_write_config(sp);
+		if (retval)
+			goto done;
+
+		sp = line + 1;
+		line = strpbrk(sp, ldelim);
+	}
+
+	retval = count;
+done:
+	vfree(inbuf);
+	return retval;
+}
+
+/*
+ *  ------------------------------------------------------------------------
+ *
+ *  The following are the functions to parse input
+ *
+ *  ------------------------------------------------------------------------
+ */
+static inline char *skip_past_ws(const char *str)
+{
+	while ((*str) && !isspace(*str))
+		++str;
+
+	return skip_spaces(str);
+}
+
+static int parse_id(char **c, char *cID)
+{
+	int tmp = **c;
+
+	if (isalnum(tmp) || (tmp == '_'))
+		cID[0] = tmp;
+	else
+		return -EINVAL;
+
+	(*c)++; tmp = **c;
+
+	if (isalnum(tmp) || (tmp == '_')) {
+		cID[1] = tmp;
+		(*c)++;
+	} else
+		cID[1] = 0;
+
+	return 0;
+}
+
+static int parse_add_config(char *buf)
+{
+	char *c = buf;
+	int  retval;
+	char cID[2];
+	long ID;
+
+	c = skip_past_ws(c);
+
+	retval = parse_id(&c, cID);
+	if (retval < 0)
+		return retval;
+
+	ID = CHAR_TO_ID(cID);
+
+	c = skip_past_ws(c);
+
+	return dgrp_add_id(ID);
+}
+
+static int parse_del_config(char *buf)
+{
+	char *c = buf;
+	int  retval;
+	struct nd_struct *nd;
+	char cID[2];
+	long ID;
+	long major;
+
+	c = skip_past_ws(c);
+
+	retval = parse_id(&c, cID);
+	if (retval < 0)
+		return retval;
+
+	ID = CHAR_TO_ID(cID);
+
+	c = skip_past_ws(c);
+
+	retval = kstrtol(c, 10, &major);
+	if (retval)
+		return retval;
+
+	nd = nd_struct_get(major);
+	if (!nd)
+		return -EINVAL;
+
+	if ((nd->nd_major != major) || (nd->nd_ID != ID))
+		return -EINVAL;
+
+	return dgrp_remove_nd(nd);
+}
+
+static int parse_chg_config(char *buf)
+{
+	return -EINVAL;
+}
+
+/*
+ *  The passed character buffer represents a single configuration request.
+ *  If the first character is a "+", it is parsed as a request to add a
+ *     PortServer
+ *  If the first character is a "-", it is parsed as a request to delete a
+ *     PortServer
+ *  If the first character is a "*", it is parsed as a request to change a
+ *     PortServer
+ *  Any other character (including whitespace) causes the record to be
+ *     ignored.
+ */
+static int parse_write_config(char *buf)
+{
+	int retval;
+
+	switch (buf[0]) {
+	case '+':
+		retval = parse_add_config(buf);
+		break;
+	case '-':
+		retval = parse_del_config(buf);
+		break;
+	case '*':
+		retval = parse_chg_config(buf);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	return retval;
+}
+
+static int info_proc_show(struct seq_file *m, void *v)
+{
+	seq_printf(m, "version: %s\n", DIGI_VERSION);
+	seq_puts(m, "register_with_sysfs: 1\n");
+	seq_printf(m, "rawreadok: 0x%08x\t(%d)\n",
+		   dgrp_rawreadok, dgrp_rawreadok);
+	seq_printf(m, "pollrate: 0x%08x\t(%d)\n",
+		   dgrp_poll_tick, dgrp_poll_tick);
+
+	return 0;
+}
+
+static int info_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, info_proc_show, NULL);
+}
+
+
+static void *nodeinfo_start(struct seq_file *m, loff_t *pos)
+{
+	return seq_list_start_head(&nd_struct_list, *pos);
+}
+
+static void *nodeinfo_next(struct seq_file *p, void *v, loff_t *pos)
+{
+	return seq_list_next(v, &nd_struct_list, pos);
+}
+
+static void nodeinfo_stop(struct seq_file *m, void *v)
+{
+}
+
+static int nodeinfo_show(struct seq_file *m, void *v)
+{
+	struct nd_struct *nd;
+	char hwver[8];
+	char swver[8];
+	char tmp_id[4];
+
+	if (v == &nd_struct_list) {
+		seq_puts(m, "#-----------------------------------------------------------------------------\n");
+		seq_puts(m, "#                 HW       HW   SW\n");
+		seq_puts(m, "# ID  State       Version  ID   Version  Description\n");
+		return 0;
+	}
+
+	nd = list_entry(v, struct nd_struct, list);
+
+	ID_TO_CHAR(nd->nd_ID, tmp_id);
+
+	if (nd->nd_state == NS_READY) {
+		sprintf(hwver, "%d.%d", (nd->nd_hw_ver >> 8) & 0xff,
+			nd->nd_hw_ver & 0xff);
+		sprintf(swver, "%d.%d", (nd->nd_sw_ver >> 8) & 0xff,
+			nd->nd_sw_ver & 0xff);
+		seq_printf(m, "  %-2.2s  %-10.10s  %-7.7s  %-3d  %-7.7s  %-35.35s\n",
+			   tmp_id,
+			   ND_STATE_STR(nd->nd_state),
+			   hwver,
+			   nd->nd_hw_id,
+			   swver,
+			   nd->nd_ps_desc);
+
+	} else {
+		seq_printf(m, "  %-2.2s  %-10.10s\n",
+			   tmp_id,
+			   ND_STATE_STR(nd->nd_state));
+	}
+
+	return 0;
+}
+
+
+static const struct seq_operations nodeinfo_ops = {
+	.start = nodeinfo_start,
+	.next  = nodeinfo_next,
+	.stop  = nodeinfo_stop,
+	.show  = nodeinfo_show
+};
+
+static int nodeinfo_proc_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &nodeinfo_ops);
+}
+
+/**
+ * dgrp_add_id() -- creates new nd struct and adds it to list
+ * @id: id of device to add
+ */
+static int dgrp_add_id(long id)
+{
+	struct nd_struct *nd;
+	int ret;
+	int i;
+
+	nd = kzalloc(sizeof(struct nd_struct), GFP_KERNEL);
+	if (!nd)
+		return -ENOMEM;
+
+	nd->nd_major = 0;
+	nd->nd_ID = id;
+
+	spin_lock_init(&nd->nd_lock);
+
+	init_waitqueue_head(&nd->nd_tx_waitq);
+	init_waitqueue_head(&nd->nd_mon_wqueue);
+	init_waitqueue_head(&nd->nd_dpa_wqueue);
+	for (i = 0; i < SEQ_MAX; i++)
+		init_waitqueue_head(&nd->nd_seq_wque[i]);
+
+	/* setup the structures to get the major number */
+	ret = dgrp_tty_init(nd);
+	if (ret)
+		goto error_out;
+
+	nd->nd_major = nd->nd_serial_ttdriver->major;
+
+	ret = nd_struct_add(nd);
+	if (ret)
+		goto error_out;
+
+	register_dgrp_device(nd, net_entry_pointer, dgrp_register_net_hook);
+	register_dgrp_device(nd, mon_entry_pointer, dgrp_register_mon_hook);
+	register_dgrp_device(nd, dpa_entry_pointer, dgrp_register_dpa_hook);
+	register_dgrp_device(nd, ports_entry_pointer,
+			      dgrp_register_ports_hook);
+
+	return 0;
+
+error_out:
+	kfree(nd);
+	return ret;
+
+}
+
+static int dgrp_remove_nd(struct nd_struct *nd)
+{
+	int ret;
+
+	/* Check to see if the selected structure is in use */
+	if (nd->nd_tty_ref_cnt)
+		return -EBUSY;
+
+	if (nd->nd_net_de) {
+		unregister_dgrp_device(nd->nd_net_de);
+		dgrp_remove_node_class_sysfs_files(nd);
+	}
+
+	if (nd->nd_mon_de)
+		unregister_dgrp_device(nd->nd_mon_de);
+
+	if (nd->nd_ports_de)
+		unregister_dgrp_device(nd->nd_ports_de);
+
+	if (nd->nd_dpa_de)
+		unregister_dgrp_device(nd->nd_dpa_de);
+
+	dgrp_tty_uninit(nd);
+
+	ret = nd_struct_del(nd);
+	if (ret)
+		return ret;
+
+	kfree(nd);
+	return 0;
+}
+
+static void register_dgrp_device(struct nd_struct *node,
+				 struct proc_dir_entry *root,
+				 void (*register_hook)(struct proc_dir_entry *de))
+{
+	char buf[3];
+	struct proc_dir_entry *de;
+
+	ID_TO_CHAR(node->nd_ID, buf);
+
+	de = create_proc_entry(buf, 0600 | S_IFREG, root);
+	if (!de)
+		return;
+
+	de->data = (void *) node;
+
+	if (register_hook)
+		register_hook(de);
+
+}
+
+static void unregister_dgrp_device(struct proc_dir_entry *de)
+{
+	if (!de)
+		return;
+
+	/* Don't unregister proc entries that are still being used.. */
+	if ((atomic_read(&de->count)) != 1) {
+		pr_alert("%s - proc entry %s in use. Not removing.\n",
+			 __func__, de->name);
+		return;
+	}
+
+	remove_proc_entry(de->name, de->parent);
+	de = NULL;
+}
diff --git a/drivers/staging/dgrp/dgrp_sysfs.c b/drivers/staging/dgrp/dgrp_sysfs.c
new file mode 100644
index 000000000000..e5a3c88d016e
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_sysfs.c
@@ -0,0 +1,555 @@
+/*
+ * 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.
+ *
+ */
+
+#include "dgrp_common.h"
+
+#include <linux/kernel.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/ctype.h>
+#include <linux/string.h>
+#include <linux/serial_reg.h>
+#include <linux/pci.h>
+#include <linux/kdev_t.h>
+
+
+#define PORTSERVER_DIVIDEND 1843200
+#define SERIAL_TYPE_NORMAL      1
+#define SERIAL_TYPE_CALLOUT     2
+#define SERIAL_TYPE_XPRINT      3
+
+
+static struct class *dgrp_class;
+static struct device *dgrp_class_nodes_dev;
+static struct device *dgrp_class_global_settings_dev;
+
+
+static ssize_t dgrp_class_version_show(struct class *class,
+				       struct class_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", DIGI_VERSION);
+}
+static CLASS_ATTR(driver_version, 0400, dgrp_class_version_show, NULL);
+
+
+static ssize_t dgrp_class_register_with_sysfs_show(struct device *c,
+						   struct device_attribute *attr,
+						   char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "1\n");
+}
+static DEVICE_ATTR(register_with_sysfs, 0400,
+		   dgrp_class_register_with_sysfs_show, NULL);
+
+
+static ssize_t dgrp_class_rawreadok_show(struct device *c,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", dgrp_rawreadok);
+}
+static ssize_t dgrp_class_rawreadok_store(struct device *c,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count)
+{
+	sscanf(buf, "0x%x\n", &dgrp_rawreadok);
+	return count;
+}
+static DEVICE_ATTR(rawreadok, 0600, dgrp_class_rawreadok_show,
+		   dgrp_class_rawreadok_store);
+
+
+static ssize_t dgrp_class_pollrate_show(struct device *c,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", dgrp_poll_tick);
+}
+
+static ssize_t dgrp_class_pollrate_store(struct device *c,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	sscanf(buf, "0x%x\n", &dgrp_poll_tick);
+	return count;
+}
+static DEVICE_ATTR(pollrate, 0600, dgrp_class_pollrate_show,
+		   dgrp_class_pollrate_store);
+
+static struct attribute *dgrp_sysfs_global_settings_entries[] = {
+	&dev_attr_pollrate.attr,
+	&dev_attr_rawreadok.attr,
+	&dev_attr_register_with_sysfs.attr,
+	NULL
+};
+
+
+static struct attribute_group dgrp_global_settings_attribute_group = {
+	.name = NULL,
+	.attrs = dgrp_sysfs_global_settings_entries,
+};
+
+
+
+void dgrp_create_class_sysfs_files(void)
+{
+	int ret = 0;
+	int max_majors = 1U << (32 - MINORBITS);
+
+	dgrp_class = class_create(THIS_MODULE, "digi_realport");
+	ret = class_create_file(dgrp_class, &class_attr_driver_version);
+
+	dgrp_class_global_settings_dev = device_create(dgrp_class, NULL,
+		MKDEV(0, max_majors + 1), NULL, "driver_settings");
+
+	ret = sysfs_create_group(&dgrp_class_global_settings_dev->kobj,
+		&dgrp_global_settings_attribute_group);
+	if (ret) {
+		pr_alert("%s: failed to create sysfs global settings device attributes.\n",
+			__func__);
+		sysfs_remove_group(&dgrp_class_global_settings_dev->kobj,
+			&dgrp_global_settings_attribute_group);
+		return;
+	}
+
+	dgrp_class_nodes_dev = device_create(dgrp_class, NULL,
+		MKDEV(0, max_majors + 2), NULL, "nodes");
+
+}
+
+
+void dgrp_remove_class_sysfs_files(void)
+{
+	struct nd_struct *nd;
+	int max_majors = 1U << (32 - MINORBITS);
+
+	list_for_each_entry(nd, &nd_struct_list, list)
+		dgrp_remove_node_class_sysfs_files(nd);
+
+	sysfs_remove_group(&dgrp_class_global_settings_dev->kobj,
+		&dgrp_global_settings_attribute_group);
+
+	class_remove_file(dgrp_class, &class_attr_driver_version);
+
+	device_destroy(dgrp_class, MKDEV(0, max_majors + 1));
+	device_destroy(dgrp_class, MKDEV(0, max_majors + 2));
+	class_destroy(dgrp_class);
+}
+
+static ssize_t dgrp_node_state_show(struct device *c,
+				    struct device_attribute *attr, char *buf)
+{
+	struct nd_struct *nd;
+
+	if (!c)
+		return 0;
+	nd = (struct nd_struct *) dev_get_drvdata(c);
+	if (!nd)
+		return 0;
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", ND_STATE_STR(nd->nd_state));
+}
+
+static DEVICE_ATTR(state, 0600, dgrp_node_state_show, NULL);
+
+static ssize_t dgrp_node_description_show(struct device *c,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct nd_struct *nd;
+
+	if (!c)
+		return 0;
+	nd = (struct nd_struct *) dev_get_drvdata(c);
+	if (!nd)
+		return 0;
+
+	if (nd->nd_state == NS_READY && nd->nd_ps_desc)
+		return snprintf(buf, PAGE_SIZE, "%s\n", nd->nd_ps_desc);
+	return 0;
+}
+static DEVICE_ATTR(description_info, 0600, dgrp_node_description_show, NULL);
+
+static ssize_t dgrp_node_hw_version_show(struct device *c,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct nd_struct *nd;
+
+	if (!c)
+		return 0;
+	nd = (struct nd_struct *) dev_get_drvdata(c);
+	if (!nd)
+		return 0;
+
+	if (nd->nd_state == NS_READY)
+		return snprintf(buf, PAGE_SIZE, "%d.%d\n",
+				(nd->nd_hw_ver >> 8) & 0xff,
+				nd->nd_hw_ver & 0xff);
+
+	return 0;
+}
+static DEVICE_ATTR(hw_version_info, 0600, dgrp_node_hw_version_show, NULL);
+
+static ssize_t dgrp_node_hw_id_show(struct device *c,
+				    struct device_attribute *attr, char *buf)
+{
+	struct nd_struct *nd;
+
+	if (!c)
+		return 0;
+	nd = (struct nd_struct *) dev_get_drvdata(c);
+	if (!nd)
+		return 0;
+
+
+	if (nd->nd_state == NS_READY)
+		return snprintf(buf, PAGE_SIZE, "%d\n", nd->nd_hw_id);
+	return 0;
+}
+static DEVICE_ATTR(hw_id_info, 0600, dgrp_node_hw_id_show, NULL);
+
+static ssize_t dgrp_node_sw_version_show(struct device *c,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct nd_struct *nd;
+
+	if (!c)
+		return 0;
+
+	nd = (struct nd_struct *) dev_get_drvdata(c);
+	if (!nd)
+		return 0;
+
+	if (nd->nd_state == NS_READY)
+		return snprintf(buf, PAGE_SIZE, "%d.%d\n",
+				(nd->nd_sw_ver >> 8) & 0xff,
+				nd->nd_sw_ver & 0xff);
+
+	return 0;
+}
+static DEVICE_ATTR(sw_version_info, 0600, dgrp_node_sw_version_show, NULL);
+
+
+static struct attribute *dgrp_sysfs_node_entries[] = {
+	&dev_attr_state.attr,
+	&dev_attr_description_info.attr,
+	&dev_attr_hw_version_info.attr,
+	&dev_attr_hw_id_info.attr,
+	&dev_attr_sw_version_info.attr,
+	NULL
+};
+
+
+static struct attribute_group dgrp_node_attribute_group = {
+	.name = NULL,
+	.attrs = dgrp_sysfs_node_entries,
+};
+
+
+void dgrp_create_node_class_sysfs_files(struct nd_struct *nd)
+{
+	int ret;
+	char name[10];
+
+	if (nd->nd_ID)
+		ID_TO_CHAR(nd->nd_ID, name);
+	else
+		sprintf(name, "node%ld", nd->nd_major);
+
+	nd->nd_class_dev = device_create(dgrp_class, dgrp_class_nodes_dev,
+		MKDEV(0, nd->nd_major), NULL, name);
+
+	ret = sysfs_create_group(&nd->nd_class_dev->kobj,
+				 &dgrp_node_attribute_group);
+
+	if (ret) {
+		pr_alert("%s: failed to create sysfs node device attributes.\n",
+			__func__);
+		sysfs_remove_group(&nd->nd_class_dev->kobj,
+				   &dgrp_node_attribute_group);
+		return;
+	}
+
+	dev_set_drvdata(nd->nd_class_dev, nd);
+
+}
+
+
+void dgrp_remove_node_class_sysfs_files(struct nd_struct *nd)
+{
+	if (nd->nd_class_dev) {
+		sysfs_remove_group(&nd->nd_class_dev->kobj,
+				   &dgrp_node_attribute_group);
+
+		device_destroy(dgrp_class, MKDEV(0, nd->nd_major));
+		nd->nd_class_dev = NULL;
+	}
+}
+
+
+
+static ssize_t dgrp_tty_state_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
+{
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			un->un_open_count ? "Open" : "Closed");
+}
+static DEVICE_ATTR(state_info, 0600, dgrp_tty_state_show, NULL);
+
+static ssize_t dgrp_tty_baud_show(struct device *d,
+				  struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+		un->un_open_count ? (PORTSERVER_DIVIDEND / ch->ch_s_brate) : 0);
+}
+static DEVICE_ATTR(baud_info, 0400, dgrp_tty_baud_show, NULL);
+
+
+static ssize_t dgrp_tty_msignals_show(struct device *d,
+				      struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+
+	if (ch->ch_open_count) {
+		return snprintf(buf, PAGE_SIZE, "%s %s %s %s %s %s\n",
+			(ch->ch_s_mlast & DM_RTS) ? "RTS" : "",
+			(ch->ch_s_mlast & DM_CTS) ? "CTS" : "",
+			(ch->ch_s_mlast & DM_DTR) ? "DTR" : "",
+			(ch->ch_s_mlast & DM_DSR) ? "DSR" : "",
+			(ch->ch_s_mlast & DM_CD) ? "DCD" : "",
+			(ch->ch_s_mlast & DM_RI)  ? "RI"  : "");
+	}
+	return 0;
+}
+static DEVICE_ATTR(msignals_info, 0400, dgrp_tty_msignals_show, NULL);
+
+
+static ssize_t dgrp_tty_iflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%x\n", ch->ch_s_iflag);
+}
+static DEVICE_ATTR(iflag_info, 0600, dgrp_tty_iflag_show, NULL);
+
+
+static ssize_t dgrp_tty_cflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%x\n", ch->ch_s_cflag);
+}
+static DEVICE_ATTR(cflag_info, 0600, dgrp_tty_cflag_show, NULL);
+
+
+static ssize_t dgrp_tty_oflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%x\n", ch->ch_s_oflag);
+}
+static DEVICE_ATTR(oflag_info, 0600, dgrp_tty_oflag_show, NULL);
+
+
+static ssize_t dgrp_tty_digi_flag_show(struct device *d,
+				       struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%x\n", ch->ch_digi.digi_flags);
+}
+static DEVICE_ATTR(digi_flag_info, 0600, dgrp_tty_digi_flag_show, NULL);
+
+
+static ssize_t dgrp_tty_rxcount_show(struct device *d,
+				     struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%d\n", ch->ch_rxcount);
+}
+static DEVICE_ATTR(rxcount_info, 0600, dgrp_tty_rxcount_show, NULL);
+
+
+static ssize_t dgrp_tty_txcount_show(struct device *d,
+				     struct device_attribute *attr, char *buf)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%d\n", ch->ch_txcount);
+}
+static DEVICE_ATTR(txcount_info, 0600, dgrp_tty_txcount_show, NULL);
+
+
+static ssize_t dgrp_tty_name_show(struct device *d,
+				  struct device_attribute *attr, char *buf)
+{
+	struct nd_struct *nd;
+	struct ch_struct *ch;
+	struct un_struct *un;
+	char name[10];
+
+	if (!d)
+		return 0;
+	un = (struct un_struct *) dev_get_drvdata(d);
+	if (!un)
+		return 0;
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+	nd = ch->ch_nd;
+	if (!nd)
+		return 0;
+
+	ID_TO_CHAR(nd->nd_ID, name);
+
+	return snprintf(buf, PAGE_SIZE, "%s%s%02d\n",
+		un->un_type == SERIAL_TYPE_XPRINT ? "pr" : "tty",
+		name, ch->ch_portnum);
+}
+static DEVICE_ATTR(custom_name, 0600, dgrp_tty_name_show, NULL);
+
+
+static struct attribute *dgrp_sysfs_tty_entries[] = {
+	&dev_attr_state_info.attr,
+	&dev_attr_baud_info.attr,
+	&dev_attr_msignals_info.attr,
+	&dev_attr_iflag_info.attr,
+	&dev_attr_cflag_info.attr,
+	&dev_attr_oflag_info.attr,
+	&dev_attr_digi_flag_info.attr,
+	&dev_attr_rxcount_info.attr,
+	&dev_attr_txcount_info.attr,
+	&dev_attr_custom_name.attr,
+	NULL
+};
+
+
+static struct attribute_group dgrp_tty_attribute_group = {
+	.name = NULL,
+	.attrs = dgrp_sysfs_tty_entries,
+};
+
+
+void dgrp_create_tty_sysfs(struct un_struct *un, struct device *c)
+{
+	int ret;
+
+	ret = sysfs_create_group(&c->kobj, &dgrp_tty_attribute_group);
+	if (ret) {
+		pr_alert("%s: failed to create sysfs tty device attributes.\n",
+			__func__);
+		sysfs_remove_group(&c->kobj, &dgrp_tty_attribute_group);
+		return;
+	}
+
+	dev_set_drvdata(c, un);
+
+}
+
+
+void dgrp_remove_tty_sysfs(struct device *c)
+{
+	sysfs_remove_group(&c->kobj, &dgrp_tty_attribute_group);
+}
diff --git a/drivers/staging/dgrp/dgrp_tty.c b/drivers/staging/dgrp/dgrp_tty.c
new file mode 100644
index 000000000000..7d7de873870c
--- /dev/null
+++ b/drivers/staging/dgrp/dgrp_tty.c
@@ -0,0 +1,3331 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     Gene Olson    <Gene_Olson at digi dot com>
+ *     James Puzzo   <jamesp at digi dot com>
+ *     Jeff Randall
+ *     Scott Kilau   <scottk 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.
+ *
+ */
+
+/*
+ *
+ *  Filename:
+ *
+ *     dgrp_tty.c
+ *
+ *  Description:
+ *
+ *     This file implements the tty driver functionality for the
+ *     RealPort driver software.
+ *
+ *  Author:
+ *
+ *     James A. Puzzo
+ *     Ann-Marie Westgate
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/sched.h>
+
+#include "dgrp_common.h"
+
+#ifndef _POSIX_VDISABLE
+#define   _POSIX_VDISABLE ('\0')
+#endif
+
+/*
+ *	forward declarations
+ */
+
+static void drp_param(struct ch_struct *);
+static void dgrp_tty_close(struct tty_struct *, struct file *);
+
+/* ioctl helper functions */
+static int set_modem_info(struct ch_struct *, unsigned int, unsigned int *);
+static int get_modem_info(struct ch_struct *, unsigned int *);
+static void dgrp_set_custom_speed(struct ch_struct *, int);
+static int dgrp_tty_digigetedelay(struct tty_struct *, int *);
+static int dgrp_tty_digisetedelay(struct tty_struct *, int *);
+static int dgrp_send_break(struct ch_struct *, int);
+
+static ushort  tty_to_ch_flags(struct tty_struct *, char);
+static tcflag_t ch_to_tty_flags(unsigned short, char);
+
+static void dgrp_tty_input_start(struct tty_struct *);
+static void dgrp_tty_input_stop(struct tty_struct *);
+
+static void drp_wmove(struct ch_struct *, int, void*, int);
+
+static int dgrp_tty_open(struct tty_struct *, struct file *);
+static void dgrp_tty_close(struct tty_struct *, struct file *);
+static int dgrp_tty_write(struct tty_struct *, const unsigned char *, int);
+static int dgrp_tty_write_room(struct tty_struct *);
+static void dgrp_tty_flush_buffer(struct tty_struct *);
+static int dgrp_tty_chars_in_buffer(struct tty_struct *);
+static int dgrp_tty_ioctl(struct tty_struct *, unsigned int, unsigned long);
+static void dgrp_tty_set_termios(struct tty_struct *, struct ktermios *);
+static void dgrp_tty_stop(struct tty_struct *);
+static void dgrp_tty_start(struct tty_struct *);
+static void dgrp_tty_throttle(struct tty_struct *);
+static void dgrp_tty_unthrottle(struct tty_struct *);
+static void dgrp_tty_hangup(struct tty_struct *);
+static int dgrp_tty_put_char(struct tty_struct *, unsigned char);
+static int dgrp_tty_tiocmget(struct tty_struct *);
+static int dgrp_tty_tiocmset(struct tty_struct *, unsigned int, unsigned int);
+static int dgrp_tty_send_break(struct tty_struct *, int);
+static void dgrp_tty_send_xchar(struct tty_struct *, char);
+
+/*
+ *	tty defines
+ */
+#define	SERIAL_TYPE_NORMAL	1
+#define	SERIAL_TYPE_CALLOUT	2
+#define	SERIAL_TYPE_XPRINT	3
+
+
+/*
+ *	tty globals/statics
+ */
+
+
+#define PORTSERVER_DIVIDEND	1843200
+
+/*
+ *  Default transparent print information.
+ */
+static struct digi_struct digi_init = {
+	.digi_flags   = DIGI_COOK,	/* Flags			*/
+	.digi_maxcps  = 100,		/* Max CPS			*/
+	.digi_maxchar = 50,		/* Max chars in print queue	*/
+	.digi_bufsize = 100,		/* Printer buffer size		*/
+	.digi_onlen   = 4,		/* size of printer on string	*/
+	.digi_offlen  = 4,		/* size of printer off string	*/
+	.digi_onstr   = "\033[5i",	/* ANSI printer on string	*/
+	.digi_offstr  = "\033[4i",	/* ANSI printer off string	*/
+	.digi_term    = "ansi"		/* default terminal type	*/
+};
+
+/*
+ *	Define a local default termios struct. All ports will be created
+ *	with this termios initially.
+ *
+ *	This defines a raw port at 9600 baud, 8 data bits, no parity,
+ *	1 stop bit.
+ */
+static struct ktermios DefaultTermios = {
+	.c_iflag = (ICRNL | IXON),
+	.c_oflag = (OPOST | ONLCR),
+	.c_cflag = (B9600 | CS8 | CREAD | HUPCL | CLOCAL),
+	.c_lflag = (ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHOCTL
+		    | ECHOKE | IEXTEN),
+	.c_cc    = INIT_C_CC,
+	.c_line  = 0,
+};
+
+/* Define our tty operations struct */
+static const struct tty_operations dgrp_tty_ops = {
+	.open            = dgrp_tty_open,
+	.close           = dgrp_tty_close,
+	.write           = dgrp_tty_write,
+	.write_room      = dgrp_tty_write_room,
+	.flush_buffer    = dgrp_tty_flush_buffer,
+	.chars_in_buffer = dgrp_tty_chars_in_buffer,
+	.flush_chars     = NULL,
+	.ioctl           = dgrp_tty_ioctl,
+	.set_termios     = dgrp_tty_set_termios,
+	.stop            = dgrp_tty_stop,
+	.start           = dgrp_tty_start,
+	.throttle        = dgrp_tty_throttle,
+	.unthrottle      = dgrp_tty_unthrottle,
+	.hangup          = dgrp_tty_hangup,
+	.put_char        = dgrp_tty_put_char,
+	.tiocmget        = dgrp_tty_tiocmget,
+	.tiocmset        = dgrp_tty_tiocmset,
+	.break_ctl       = dgrp_tty_send_break,
+	.send_xchar      = dgrp_tty_send_xchar
+};
+
+
+static int calc_baud_rate(struct un_struct *un)
+{
+	int i;
+	int brate;
+
+	struct baud_rates {
+		unsigned int rate;
+		unsigned int cflag;
+	};
+
+	static struct baud_rates baud_rates[] = {
+		{ 921600, B921600 },
+		{ 460800, B460800 },
+		{ 230400, B230400 },
+		{ 115200, B115200 },
+		{  57600, B57600  },
+		{  38400, B38400  },
+		{  19200, B19200  },
+		{   9600, B9600   },
+		{   4800, B4800   },
+		{   2400, B2400   },
+		{   1200, B1200   },
+		{    600, B600    },
+		{    300, B300    },
+		{    200, B200    },
+		{    150, B150    },
+		{    134, B134    },
+		{    110, B110    },
+		{     75, B75     },
+		{     50, B50     },
+		{      0, B9600  }
+	};
+
+	brate = C_BAUD(un->un_tty);
+
+	for (i = 0; baud_rates[i].rate; i++) {
+		if (baud_rates[i].cflag == brate)
+			break;
+	}
+
+	return baud_rates[i].rate;
+}
+
+static int calc_fastbaud_rate(struct un_struct *un, struct ktermios *uts)
+{
+	int i;
+	int brate;
+
+	ulong bauds[2][16] = {
+		{ /* fastbaud*/
+			0,      57600,	 76800,	115200,
+			131657, 153600, 230400, 460800,
+			921600, 1200,   1800,	2400,
+			4800,   9600,	19200,	38400 },
+		{ /* fastbaud & CBAUDEX */
+			0,      57600,	115200,	230400,
+			460800, 150,    200,    921600,
+			600,    1200,   1800,	2400,
+			4800,   9600,	19200,	38400 }
+	};
+
+	brate = C_BAUD(un->un_tty) & 0xff;
+
+	i = (uts->c_cflag & CBAUDEX) ? 1 : 0;
+
+
+	if ((i >= 0) && (i < 2) && (brate >= 0) && (brate < 16))
+		brate = bauds[i][brate];
+	else
+		brate = 0;
+
+	return brate;
+}
+
+/**
+ * drp_param() -- send parameter values to be sent to the node
+ * @ch: channel structure of port to modify
+ *
+ * Interprets the tty and modem changes made by an application
+ * program (by examining the termios structures) and sets up
+ * parameter values to be sent to the node.
+ */
+static void drp_param(struct ch_struct *ch)
+{
+	struct nd_struct *nd;
+	struct un_struct *un;
+	int   brate;
+	int   mflow;
+	int   xflag;
+	int   iflag;
+	struct ktermios *tts, *pts, *uts;
+
+	nd = ch->ch_nd;
+
+	/*
+	 *  If the terminal device is open, use it to set up all tty
+	 *  modes and functions.  Otherwise use the printer device.
+	 */
+
+	if (ch->ch_tun.un_open_count) {
+
+		un = &ch->ch_tun;
+		tts = &ch->ch_tun.un_tty->termios;
+
+		/*
+		 *  If both devices are open, copy critical line
+		 *  parameters from the tty device to the printer,
+		 *  so that if the tty is closed, the printer will
+		 *  continue without disruption.
+		 */
+
+		if (ch->ch_pun.un_open_count) {
+
+			pts = &ch->ch_pun.un_tty->termios;
+
+			pts->c_cflag ^=
+				(pts->c_cflag ^ tts->c_cflag) &
+				(CBAUD  | CSIZE | CSTOPB | CREAD | PARENB |
+				 PARODD | HUPCL | CLOCAL);
+
+			pts->c_iflag ^=
+				(pts->c_iflag ^ tts->c_iflag) &
+				(IGNBRK | BRKINT | IGNPAR | PARMRK | INPCK |
+				 ISTRIP | IXON   | IXANY  | IXOFF);
+
+			pts->c_cc[VSTART] = tts->c_cc[VSTART];
+			pts->c_cc[VSTOP] = tts->c_cc[VSTOP];
+		}
+	} else if (ch->ch_pun.un_open_count == 0) {
+		pr_warn("%s - ch_pun.un_open_count shouldn't be 0\n",
+		       __func__);
+		return;
+	} else {
+		un = &ch->ch_pun;
+	}
+
+	uts = &un->un_tty->termios;
+
+	/*
+	 * Determine if FAST writes can be performed.
+	 */
+
+	if ((ch->ch_digi.digi_flags & DIGI_COOK) != 0 &&
+	    (ch->ch_tun.un_open_count != 0)  &&
+	    !((un->un_tty)->ldisc->ops->flags & LDISC_FLAG_DEFINED) &&
+	    !(L_XCASE(un->un_tty))) {
+		ch->ch_flag |= CH_FAST_WRITE;
+	} else {
+		ch->ch_flag &= ~CH_FAST_WRITE;
+	}
+
+	/*
+	 *  If FAST writes can be performed, and OPOST is on in the
+	 *  terminal device, do OPOST handling in the server.
+	 */
+
+	if ((ch->ch_flag & CH_FAST_WRITE) &&
+	      O_OPOST(un->un_tty) != 0) {
+		int oflag = tty_to_ch_flags(un->un_tty, 'o');
+
+		/* add to ch_ocook any processing flags set in the termio */
+		ch->ch_ocook |= oflag & (OF_OLCUC |
+					 OF_ONLCR |
+					 OF_OCRNL |
+					 OF_ONLRET |
+					 OF_TABDLY);
+
+		/*
+		 * the hpux driver clears any flags set in ch_ocook
+		 * from the termios oflag.  It is STILL reported though
+		 * by a TCGETA
+		 */
+
+		oflag = ch_to_tty_flags(ch->ch_ocook, 'o');
+		uts->c_oflag &= ~oflag;
+
+	} else {
+		/* clear the ch->ch_ocook flag */
+		int oflag = ch_to_tty_flags(ch->ch_ocook, 'o');
+		uts->c_oflag |= oflag;
+		ch->ch_ocook = 0;
+	}
+
+	ch->ch_oflag = ch->ch_ocook;
+
+
+	ch->ch_flag &= ~CH_FAST_READ;
+
+	/*
+	 *  Generate channel flags
+	 */
+
+	if (C_BAUD(un->un_tty) == B0) {
+		if (!(ch->ch_flag & CH_BAUD0)) {
+			/* TODO : the HPUX driver flushes line */
+			/* TODO : discipline, I assume I don't have to */
+
+			ch->ch_tout = ch->ch_tin;
+			ch->ch_rout = ch->ch_rin;
+
+			ch->ch_break_time = 0;
+
+			ch->ch_send |= RR_TX_FLUSH | RR_RX_FLUSH;
+
+			ch->ch_mout &= ~(DM_DTR | DM_RTS);
+
+			ch->ch_flag |= CH_BAUD0;
+		}
+	} else if (ch->ch_custom_speed) {
+		ch->ch_brate = PORTSERVER_DIVIDEND / ch->ch_custom_speed ;
+
+		if (ch->ch_flag & CH_BAUD0) {
+			ch->ch_mout |= DM_DTR | DM_RTS;
+
+			ch->ch_flag &= ~CH_BAUD0;
+		}
+	} else {
+		/*
+		 * Baud rate mapping.
+		 *
+		 * If FASTBAUD isn't on, we can scan the new baud rate list
+		 * as required.
+		 *
+		 * However, if FASTBAUD is on, we must go to the old
+		 * baud rate mapping that existed many many moons ago,
+		 * for compatibility reasons.
+		 */
+
+		if (!(ch->ch_digi.digi_flags & DIGI_FAST))
+			brate = calc_baud_rate(un);
+		else
+			brate = calc_fastbaud_rate(un, uts);
+
+		if (brate == 0)
+			brate = 9600;
+
+		ch->ch_brate = PORTSERVER_DIVIDEND / brate;
+
+		if (ch->ch_flag & CH_BAUD0) {
+			ch->ch_mout |= DM_DTR | DM_RTS;
+
+			ch->ch_flag &= ~CH_BAUD0;
+		}
+	}
+
+	/*
+	 *  Generate channel cflags from the termio.
+	 */
+
+	ch->ch_cflag = tty_to_ch_flags(un->un_tty, 'c');
+
+	/*
+	 *  Generate channel iflags from the termio.
+	 */
+
+	iflag = (int) tty_to_ch_flags(un->un_tty, 'i');
+
+	if (START_CHAR(un->un_tty) == _POSIX_VDISABLE ||
+	    STOP_CHAR(un->un_tty) == _POSIX_VDISABLE) {
+		iflag &= ~(IF_IXON | IF_IXANY | IF_IXOFF);
+	}
+
+	ch->ch_iflag = iflag;
+
+	/*
+	 *  Generate flow control characters
+	 */
+
+	/*
+	 * From the POSIX.1 spec (7.1.2.6): "If {_POSIX_VDISABLE}
+	 * is defined for the terminal device file, and the value
+	 * of one of the changable special control characters (see
+	 * 7.1.1.9) is {_POSIX_VDISABLE}, that function shall be
+	 * disabled, that is, no input data shall be recognized as
+	 * the disabled special character."
+	 *
+	 * OK, so we don't ever assign S/DXB XON or XOFF to _POSIX_VDISABLE.
+	 */
+
+	if (uts->c_cc[VSTART] != _POSIX_VDISABLE)
+		ch->ch_xon = uts->c_cc[VSTART];
+	if (uts->c_cc[VSTOP] != _POSIX_VDISABLE)
+		ch->ch_xoff = uts->c_cc[VSTOP];
+
+	ch->ch_lnext = (uts->c_cc[VLNEXT] == _POSIX_VDISABLE ? 0 :
+			uts->c_cc[VLNEXT]);
+
+	/*
+	 * Also, if either c_cc[START] or c_cc[STOP] is set to
+	 * _POSIX_VDISABLE, we can't really do software flow
+	 * control--in either direction--so we turn it off as
+	 * far as S/DXB is concerned.  In essence, if you disable
+	 * one, you disable the other too.
+	 */
+	if ((uts->c_cc[VSTART] == _POSIX_VDISABLE) ||
+	    (uts->c_cc[VSTOP] == _POSIX_VDISABLE))
+		ch->ch_iflag &= ~(IF_IXOFF | IF_IXON);
+
+	/*
+	 *  Update xflags.
+	 */
+
+	xflag = 0;
+
+	if (ch->ch_digi.digi_flags & DIGI_AIXON)
+		xflag = XF_XIXON;
+
+	if ((ch->ch_xxon == _POSIX_VDISABLE) ||
+	    (ch->ch_xxoff == _POSIX_VDISABLE))
+		xflag &= ~XF_XIXON;
+
+	ch->ch_xflag = xflag;
+
+
+	/*
+	 *  Figure effective DCD value.
+	 */
+
+	if (C_CLOCAL(un->un_tty))
+		ch->ch_flag |= CH_CLOCAL;
+	else
+		ch->ch_flag &= ~CH_CLOCAL;
+
+	/*
+	 *  Check modem signals
+	 */
+
+	dgrp_carrier(ch);
+
+	/*
+	 *  Get hardware handshake value.
+	 */
+
+	mflow = 0;
+
+	if (C_CRTSCTS(un->un_tty))
+		mflow |= (DM_RTS | DM_CTS);
+
+	if (ch->ch_digi.digi_flags & RTSPACE)
+		mflow |= DM_RTS;
+
+	if (ch->ch_digi.digi_flags & DTRPACE)
+		mflow |= DM_DTR;
+
+	if (ch->ch_digi.digi_flags & CTSPACE)
+		mflow |= DM_CTS;
+
+	if (ch->ch_digi.digi_flags & DSRPACE)
+		mflow |= DM_DSR;
+
+	if (ch->ch_digi.digi_flags & DCDPACE)
+		mflow |= DM_CD;
+
+	if (ch->ch_digi.digi_flags & DIGI_RTS_TOGGLE)
+		mflow |= DM_RTS_TOGGLE;
+
+	ch->ch_mflow = mflow;
+
+	/*
+	 *  Send the changes to the server.
+	 */
+
+	ch->ch_flag |= CH_PARAM;
+	(ch->ch_nd)->nd_tx_work = 1;
+
+	if (waitqueue_active(&ch->ch_flag_wait))
+		wake_up_interruptible(&ch->ch_flag_wait);
+}
+
+/*
+ * This function is just used as a callback for timeouts
+ * waiting on the ch_sleep flag.
+ */
+static void wake_up_drp_sleep_timer(unsigned long ptr)
+{
+	struct ch_struct *ch = (struct ch_struct *) ptr;
+	if (ch)
+		wake_up(&ch->ch_sleep);
+}
+
+
+/*
+ * Set up our own sleep that can't be cancelled
+ * until our timeout occurs.
+ */
+static void drp_my_sleep(struct ch_struct *ch)
+{
+	struct timer_list drp_wakeup_timer;
+	DECLARE_WAITQUEUE(wait, current);
+
+	/*
+	 * First make sure we're ready to receive the wakeup.
+	 */
+
+	add_wait_queue(&ch->ch_sleep, &wait);
+	current->state = TASK_UNINTERRUPTIBLE;
+
+	/*
+	 * Since we are uninterruptible, set a timer to
+	 * unset the uninterruptable state in 1 second.
+	 */
+
+	init_timer(&drp_wakeup_timer);
+	drp_wakeup_timer.function = wake_up_drp_sleep_timer;
+	drp_wakeup_timer.data = (unsigned long) ch;
+	drp_wakeup_timer.expires = jiffies + (1 * HZ);
+	add_timer(&drp_wakeup_timer);
+
+	schedule();
+
+	del_timer(&drp_wakeup_timer);
+
+	remove_wait_queue(&ch->ch_sleep, &wait);
+}
+
+/*
+ * dgrp_tty_open()
+ *
+ * returns:
+ *    -EBUSY    - this is a callout device and the normal device is active
+ *              - there is an error in opening the tty
+ *    -ENODEV   - the channel does not exist
+ *    -EAGAIN   - we are in the middle of hanging up or closing
+ *              - IMMEDIATE_OPEN fails
+ *    -ENXIO or -EAGAIN
+ *              - if the port is outside physical range
+ *    -EINTR    - the open is interrupted
+ *
+ */
+static int dgrp_tty_open(struct tty_struct *tty, struct file *file)
+{
+	int    retval = 0;
+	struct nd_struct  *nd;
+	struct ch_struct *ch;
+	struct un_struct  *un;
+	int    port;
+	int    delay_error;
+	int    otype;
+	int    unf;
+	int    wait_carrier;
+	int    category;
+	int    counts_were_incremented = 0;
+	ulong lock_flags;
+	DECLARE_WAITQUEUE(wait, current);
+
+	/*
+	 * Do some initial checks to see if the node and port exist
+	 */
+
+	nd = nd_struct_get(MAJOR(tty_devnum(tty)));
+	port = PORT_NUM(MINOR(tty_devnum(tty)));
+	category = OPEN_CATEGORY(MINOR(tty_devnum(tty)));
+
+	if (!nd)
+		return -ENODEV;
+
+	if (port >= CHAN_MAX)
+		return -ENODEV;
+
+	/*
+	 *  The channel exists.
+	 */
+
+	ch = nd->nd_chan + port;
+
+	un = IS_PRINT(MINOR(tty_devnum(tty))) ? &ch->ch_pun : &ch->ch_tun;
+	un->un_tty = tty;
+	tty->driver_data = un;
+
+	/*
+	 * If we are in the middle of hanging up,
+	 * then return an error
+	 */
+	if (tty_hung_up_p(file)) {
+		retval = ((un->un_flag & UN_HUP_NOTIFY) ?
+			   -EAGAIN : -ERESTARTSYS);
+		goto done;
+	}
+
+	/*
+	 * If the port is in the middle of closing, then block
+	 * until it is done, then try again.
+	 */
+	retval = wait_event_interruptible(un->un_close_wait,
+			((un->un_flag & UN_CLOSING) == 0));
+
+	if (retval)
+		goto done;
+
+	/*
+	 * If the port is in the middle of a reopen after a network disconnect,
+	 * wait until it is done, then try again.
+	 */
+	retval = wait_event_interruptible(ch->ch_flag_wait,
+			((ch->ch_flag & CH_PORT_GONE) == 0));
+
+	if (retval)
+		goto done;
+
+	/*
+	 * If this is a callout device, then just make sure the normal
+	 * device isn't being used.
+	 */
+
+	if (tty->driver->subtype == SERIAL_TYPE_CALLOUT) {
+		if (un->un_flag & UN_NORMAL_ACTIVE) {
+			retval = -EBUSY;
+			goto done;
+		} else {
+			un->un_flag |= UN_CALLOUT_ACTIVE;
+		}
+	}
+
+	/*
+	 *  Loop waiting until the open can be successfully completed.
+	 */
+
+	spin_lock_irqsave(&nd->nd_lock, lock_flags);
+
+	nd->nd_tx_work = 1;
+
+	for (;;) {
+		wait_carrier = 0;
+
+		/*
+		 * Determine the open type from the flags provided.
+		 */
+
+		/*
+		 * If the port is not enabled, then exit
+		 */
+		if (test_bit(TTY_IO_ERROR, &tty->flags)) {
+			/* there was an error in opening the tty */
+			if (un->un_flag & UN_CALLOUT_ACTIVE)
+				retval = -EBUSY;
+			else
+				un->un_flag |= UN_NORMAL_ACTIVE;
+			goto unlock;
+		}
+
+		if (file->f_flags & O_NONBLOCK) {
+
+			/*
+			 * if the O_NONBLOCK is set, errors on read and write
+			 * must return -EAGAIN immediately and NOT sleep
+			 * on the waitqs.
+			 */
+			otype = OTYPE_IMMEDIATE;
+			delay_error = -EAGAIN;
+
+		} else if (!OPEN_WAIT_AVAIL(category) ||
+			  (file->f_flags & O_NDELAY) != 0) {
+			otype = OTYPE_IMMEDIATE;
+			delay_error = -EBUSY;
+
+		} else if (!OPEN_WAIT_CARRIER(category) ||
+			  ((ch->ch_digi.digi_flags & DIGI_FORCEDCD) != 0) ||
+			  C_CLOCAL(tty)) {
+			otype = OTYPE_PERSISTENT;
+			delay_error = 0;
+
+		} else {
+			otype = OTYPE_INCOMING;
+			delay_error = 0;
+		}
+
+		/*
+		 * Handle port currently outside physical port range.
+		 */
+
+		if (port >= nd->nd_chan_count) {
+			if (otype == OTYPE_IMMEDIATE) {
+				retval = (nd->nd_state == NS_READY) ?
+						-ENXIO : -EAGAIN;
+				goto unlock;
+			}
+		}
+
+		/*
+		 *  Handle port not currently open.
+		 */
+
+		else if (ch->ch_open_count == 0) {
+			/*
+			 * Return an error when an Incoming Open
+			 * response indicates the port is busy.
+			 */
+
+			if (ch->ch_open_error != 0 && otype == ch->ch_otype) {
+				retval = (ch->ch_open_error <= 2) ?
+					  delay_error : -ENXIO ;
+				goto unlock;
+			}
+
+			/*
+			 * Fail any new Immediate open if we do not have
+			 * a normal connection to the server.
+			 */
+
+			if (nd->nd_state != NS_READY &&
+			    otype == OTYPE_IMMEDIATE) {
+				retval = -EAGAIN;
+				goto unlock;
+			}
+
+			/*
+			 * If a Realport open of the correct type has
+			 * succeeded, complete the open.
+			 */
+
+			if (ch->ch_state == CS_READY && ch->ch_otype == otype)
+				break;
+		}
+
+		/*
+		 * Handle port already open and active as a device
+		 * of same category.
+		 */
+
+		else if ((ch->ch_category == category) ||
+			  IS_PRINT(MINOR(tty_devnum(tty)))) {
+			/*
+			 * Fail if opening the device now would
+			 * violate exclusive use.
+			 */
+			unf = ch->ch_tun.un_flag | ch->ch_pun.un_flag;
+
+			if ((file->f_flags & O_EXCL) || (unf & UN_EXCL)) {
+				retval = -EBUSY;
+				goto unlock;
+			}
+
+			/*
+			 * If the open device is in the hangup state, all
+			 * system calls fail except close().
+			 */
+
+			/* TODO : check on hangup_p calls */
+
+			if (ch->ch_flag & CH_HANGUP) {
+				retval = -ENXIO;
+				goto unlock;
+			}
+
+			/*
+			 * If the port is ready, and carrier is ignored
+			 * or present, then complete the open.
+			 */
+
+			if (ch->ch_state == CS_READY &&
+			    (otype != OTYPE_INCOMING ||
+			    ch->ch_flag & CH_VIRT_CD))
+				break;
+
+			wait_carrier = 1;
+		}
+
+		/*
+		 *  Handle port active with a different category device.
+		 */
+
+		else {
+			if (otype == OTYPE_IMMEDIATE) {
+				retval = delay_error;
+				goto unlock;
+			}
+		}
+
+		/*
+		 * Wait until conditions change, then take another
+		 * try at the open.
+		 */
+
+		ch->ch_wait_count[otype]++;
+
+		if (wait_carrier)
+			ch->ch_wait_carrier++;
+
+		/*
+		 * Prepare the task to accept the wakeup, then
+		 * release our locks and release control.
+		 */
+
+		add_wait_queue(&ch->ch_flag_wait, &wait);
+		current->state = TASK_INTERRUPTIBLE;
+
+		spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+
+		/*
+		 * Give up control, we'll come back if we're
+		 * interrupted or are woken up.
+		 */
+		schedule();
+		remove_wait_queue(&ch->ch_flag_wait, &wait);
+
+		spin_lock_irqsave(&nd->nd_lock, lock_flags);
+
+		current->state = TASK_RUNNING;
+
+		ch->ch_wait_count[otype]--;
+
+		if (wait_carrier)
+			ch->ch_wait_carrier--;
+
+		nd->nd_tx_work = 1;
+
+		if (signal_pending(current)) {
+			retval = -EINTR;
+			goto unlock;
+		}
+	} /* end for(;;) */
+
+	/*
+	 *  The open has succeeded.  No turning back.
+	 */
+	counts_were_incremented = 1;
+	un->un_open_count++;
+	ch->ch_open_count++;
+
+	/*
+	 * Initialize the channel, if it's not already open.
+	 */
+
+	if (ch->ch_open_count == 1) {
+		ch->ch_flag = 0;
+		ch->ch_inwait = 0;
+		ch->ch_category = category;
+		ch->ch_pscan_state = 0;
+
+		/* TODO : find out what PS-1 bug Gene was referring to */
+		/* TODO : in the following comment. */
+
+		ch->ch_send = RR_TX_START | RR_RX_START;  /* PS-1 bug */
+
+		if (C_CLOCAL(tty) ||
+		    ch->ch_s_mlast & DM_CD ||
+		    ch->ch_digi.digi_flags & DIGI_FORCEDCD)
+			ch->ch_flag |= CH_VIRT_CD;
+		else if (OPEN_FORCES_CARRIER(category))
+			ch->ch_flag |= CH_VIRT_CD;
+
+	}
+
+	/*
+	 *  Initialize the unit, if it is not already open.
+	 */
+
+	if (un->un_open_count == 1) {
+		/*
+		 *  Since all terminal options are always sticky in Linux,
+		 *  we don't need the UN_STICKY flag to be handled specially.
+		 */
+		/* clears all the digi flags, leaves serial flags */
+		un->un_flag &= ~UN_DIGI_MASK;
+
+		if (file->f_flags & O_EXCL)
+			un->un_flag |= UN_EXCL;
+
+		/* TODO : include "session" and "pgrp" */
+
+		/*
+		 *  In Linux, all terminal parameters are intended to be sticky.
+		 *  as a result, we "remove" the code which once reset the ports
+		 *  to sane values.
+		 */
+
+		drp_param(ch);
+
+	}
+
+	un->un_flag |= UN_INITIALIZED;
+
+	retval = 0;
+
+unlock:
+
+	spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+
+done:
+	/*
+	 * Linux does a close for every open, even failed ones!
+	 */
+	if (!counts_were_incremented) {
+		un->un_open_count++;
+		ch->ch_open_count++;
+	}
+
+	if (retval)
+		dev_err(tty->dev, "tty open bad return (%i)\n", retval);
+
+	return retval;
+}
+
+
+
+
+/*
+ * dgrp_tty_close() -- close function for tty_operations
+ */
+static void dgrp_tty_close(struct tty_struct *tty, struct file *file)
+{
+	struct ch_struct *ch;
+	struct un_struct *un;
+	struct nd_struct *nd;
+	int	tpos;
+	int	port;
+	int	err = 0;
+	int	s = 0;
+	ulong  waketime;
+	ulong  lock_flags;
+	int sent_printer_offstr = 0;
+
+	port = PORT_NUM(MINOR(tty_devnum(tty)));
+
+	un = tty->driver_data;
+
+	if (!un)
+		return;
+
+	ch = un->un_ch;
+
+	if (!ch)
+		return;
+
+	nd = ch->ch_nd;
+
+	if (!nd)
+		return;
+
+	spin_lock_irqsave(&nd->nd_lock, lock_flags);
+
+
+	/* Used to be on channel basis, now we check on a unit basis. */
+	if (un->un_open_count != 1)
+		goto unlock;
+
+	/*
+	 * OK, its the last close on the unit
+	 */
+	un->un_flag |= UN_CLOSING;
+
+	/*
+	 * Notify the discipline to only process XON/XOFF characters.
+	 */
+	tty->closing = 1;
+
+	/*
+	 * Wait for output to drain only if this is
+	 * the last close against the channel
+	 */
+
+	if (ch->ch_open_count == 1) {
+		/*
+		 * If its the print device, we need to ensure at all costs that
+		 * the offstr will fit. If it won't, flush our tbuf.
+		 */
+		if (IS_PRINT(MINOR(tty_devnum(tty))) &&
+		    (((ch->ch_tout - ch->ch_tin - 1) & TBUF_MASK) <
+		    ch->ch_digi.digi_offlen))
+			ch->ch_tin = ch->ch_tout;
+
+		/*
+		 * Turn off the printer.  Don't bother checking to see if its
+		 * IS_PRINT... Since this is the last close the flag is going
+		 * to be cleared, so we MUST make sure the offstr gets inserted
+		 * into tbuf.
+		 */
+
+		if ((ch->ch_flag & CH_PRON) != 0) {
+			drp_wmove(ch, 0, ch->ch_digi.digi_offstr,
+				  ch->ch_digi.digi_offlen);
+			ch->ch_flag &= ~CH_PRON;
+			sent_printer_offstr = 1;
+		}
+	}
+
+	/*
+	 *  Wait until either the output queue has drained, or we see
+	 *  absolutely no progress for 15 seconds.
+	 */
+
+	tpos = ch->ch_s_tpos;
+
+	waketime = jiffies + 15 * HZ;
+
+	for (;;) {
+
+		/*
+		 *  Make sure the port still exists.
+		 */
+
+		if (port >= nd->nd_chan_count) {
+			err = 1;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			err = 1;
+			break;
+		}
+
+		/*
+		 * If the port is idle (not opened on the server), we have
+		 * no way of draining/flushing/closing the port on that server.
+		 * So break out of loop.
+		 */
+		if (ch->ch_state == CS_IDLE)
+			break;
+
+		nd->nd_tx_work = 1;
+
+		/*
+		 *  Exit if the queues for this unit are empty,
+		 *  and either the other unit is still open or all
+		 *  data has drained.
+		 */
+
+		if ((un->un_tty)->ops->chars_in_buffer ?
+		    ((un->un_tty)->ops->chars_in_buffer)(un->un_tty) == 0 : 1) {
+
+			/*
+			 * We don't need to wait for a buffer to drain
+			 * if the other unit is open.
+			 */
+
+			if (ch->ch_open_count != un->un_open_count)
+				break;
+
+			/*
+			 *  The wait is complete when all queues are
+			 *  drained, and any break in progress is complete.
+			 */
+
+			if (ch->ch_tin == ch->ch_tout &&
+			    ch->ch_s_tin == ch->ch_s_tpos &&
+			    (ch->ch_send & RR_TX_BREAK) == 0) {
+				break;
+			}
+		}
+
+		/*
+		 * Flush TX data and exit the wait if NDELAY is set,
+		 * or this is not a DIGI printer, and the close timeout
+		 * expires.
+		 */
+
+		if ((file->f_flags & (O_NDELAY | O_NONBLOCK)) ||
+		    ((long)(jiffies - waketime) >= 0 &&
+		      (ch->ch_digi.digi_flags & DIGI_PRINTER) == 0)) {
+
+				/*
+				 * If we sent the printer off string, we cannot
+				 * flush our internal buffers, or we might lose
+				 * the offstr.
+				 */
+				if (!sent_printer_offstr)
+					dgrp_tty_flush_buffer(tty);
+
+				tty_ldisc_flush(tty);
+				break;
+		}
+
+		/*
+		 *  Otherwise take a short nap.
+		 */
+
+		ch->ch_flag |= CH_DRAIN;
+
+		spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+
+		schedule_timeout_interruptible(1);
+		s = signal_pending(current);
+
+		spin_lock_irqsave(&nd->nd_lock, lock_flags);
+
+		if (s) {
+			/*
+			 * If we had sent the printer off string, we now have
+			 * some problems.
+			 *
+			 * The system won't let us sleep since we got an error
+			 * back from sleep, presumably because the user did
+			 * a ctrl-c...
+			 * But we need to ensure that the offstr gets sent!
+			 * Thus, we have to do something else besides sleeping.
+			 * The plan:
+			 * 1) Make this task uninterruptable.
+			 * 2) Set up a timer to go off in 1 sec.
+			 * 3) Act as tho we just got out of the sleep above.
+			 *
+			 * Thankfully, in the real world, this just
+			 * never happens.
+			 */
+
+			if (sent_printer_offstr) {
+				spin_unlock_irqrestore(&nd->nd_lock,
+						       lock_flags);
+				drp_my_sleep(ch);
+				spin_lock_irqsave(&nd->nd_lock, lock_flags);
+			} else {
+				err = 1;
+				break;
+			}
+		}
+
+		/*
+		 *  Restart the wait if any progress is seen.
+		 */
+
+		if (ch->ch_s_tpos != tpos) {
+			tpos = ch->ch_s_tpos;
+
+			/* TODO:  this gives us timeout problems with nist ?? */
+			waketime = jiffies + 15 * HZ;
+		}
+	}
+
+	/*
+	 *  Close the line discipline
+	 */
+
+	/* this is done in tty_io.c */
+	/* if ((un->un_tty)->ldisc.close)
+	 *	((un->un_tty)->ldisc.close)(un->un_tty);
+	 */
+
+	/* don't do this here */
+	/* un->un_flag = 0; */
+
+	/*
+	 *  Flush the receive buffer on terminal unit close only.
+	 */
+
+	if (!IS_PRINT(MINOR(tty_devnum(tty))))
+		ch->ch_rout = ch->ch_rin;
+
+
+	/*
+	 * Don't permit the close to happen until we get any pending
+	 * sync request responses.
+	 * There could be other ports depending upon the response as well.
+	 *
+	 * Also, don't permit the close to happen until any parameter
+	 * changes have been sent out from the state machine as well.
+	 * This is required because of a ditty -a race with -HUPCL
+	 * We MUST make sure all channel parameters have been sent to the
+	 * Portserver before sending a close.
+	 */
+
+	if ((err != 1) && (ch->ch_state != CS_IDLE)) {
+		spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+		s = wait_event_interruptible(ch->ch_flag_wait,
+			((ch->ch_flag & (CH_WAITING_SYNC | CH_PARAM)) == 0));
+		spin_lock_irqsave(&nd->nd_lock, lock_flags);
+	}
+
+	/*
+	 * Cleanup the channel if last unit open.
+	 */
+
+	if (ch->ch_open_count == 1) {
+		ch->ch_flag = 0;
+		ch->ch_category = 0;
+		ch->ch_send = 0;
+		ch->ch_expect = 0;
+		ch->ch_tout = ch->ch_tin;
+		/* (un->un_tty)->device = 0; */
+
+		if (ch->ch_state == CS_READY)
+			ch->ch_state = CS_SEND_CLOSE;
+	}
+
+	/*
+	 * Send the changes to the server
+	 */
+	if (ch->ch_state != CS_IDLE) {
+		ch->ch_flag |= CH_PARAM;
+		wake_up_interruptible(&ch->ch_flag_wait);
+	}
+
+	nd->nd_tx_work = 1;
+	nd->nd_tx_ready = 1;
+
+unlock:
+	tty->closing = 0;
+
+	if (ch->ch_open_count <= 0)
+		dev_info(tty->dev,
+			 "%s - unexpected value for ch->ch_open_count: %i\n",
+			 __func__, ch->ch_open_count);
+	else
+		ch->ch_open_count--;
+
+	if (un->un_open_count <= 0)
+		dev_info(tty->dev,
+			 "%s - unexpected value for un->un_open_count: %i\n",
+			 __func__, un->un_open_count);
+	else
+		un->un_open_count--;
+
+	un->un_flag &= ~(UN_NORMAL_ACTIVE | UN_CALLOUT_ACTIVE | UN_CLOSING);
+	if (waitqueue_active(&un->un_close_wait))
+		wake_up_interruptible(&un->un_close_wait);
+
+	spin_unlock_irqrestore(&nd->nd_lock, lock_flags);
+
+	return;
+
+}
+
+static void drp_wmove(struct ch_struct *ch, int from_user, void *buf, int count)
+{
+	int n;
+	int ret = 0;
+
+	ch->ch_nd->nd_tx_work = 1;
+
+	n = TBUF_MAX - ch->ch_tin;
+
+	if (count >= n) {
+		if (from_user)
+			ret = copy_from_user(ch->ch_tbuf + ch->ch_tin,
+					     (void __user *) buf, n);
+		else
+			memcpy(ch->ch_tbuf + ch->ch_tin, buf, n);
+
+		buf = (char *) buf + n;
+		count -= n;
+		ch->ch_tin = 0;
+	}
+
+	if (from_user)
+		ret = copy_from_user(ch->ch_tbuf + ch->ch_tin,
+				     (void __user *) buf, count);
+	else
+		memcpy(ch->ch_tbuf + ch->ch_tin, buf, count);
+
+	ch->ch_tin += count;
+}
+
+
+static int dgrp_calculate_txprint_bounds(struct ch_struct *ch, int space,
+					 int *un_flag)
+{
+	clock_t tt;
+	clock_t mt;
+	unsigned short tmax = 0;
+
+	/*
+	 * If the terminal device is busy, reschedule when
+	 * the terminal device becomes idle.
+	 */
+
+	if (ch->ch_tun.un_open_count != 0 &&
+	    ch->ch_tun.un_tty->ops->chars_in_buffer &&
+	    ((ch->ch_tun.un_tty->ops->chars_in_buffer)(ch->ch_tun.un_tty) != 0)) {
+		*un_flag = UN_PWAIT;
+		return 0;
+	}
+
+	/*
+	 * Assure that whenever there is printer data in the output
+	 * buffer, there always remains enough space after it to
+	 * turn the printer off.
+	 */
+	space -= ch->ch_digi.digi_offlen;
+
+	if (space <= 0) {
+		*un_flag = UN_EMPTY;
+		return 0;
+	}
+
+	/*
+	 * We measure printer CPS speed by incrementing
+	 * ch_cpstime by (HZ / digi_maxcps) for every
+	 * character we output, restricting output so
+	 * that ch_cpstime never exceeds lbolt.
+	 *
+	 * However if output has not been done for some
+	 * time, lbolt will grow to very much larger than
+	 * ch_cpstime, which would allow essentially
+	 * unlimited amounts of output until ch_cpstime
+	 * finally caught up.   To avoid this, we adjust
+	 * cps_time when necessary so the difference
+	 * between lbolt and ch_cpstime never results
+	 * in sending more than digi_bufsize characters.
+	 *
+	 * This nicely models a printer with an internal
+	 * buffer of digi_bufsize characters.
+	 *
+	 * Get the time between lbolt and ch->ch_cpstime;
+	 */
+
+	tt = jiffies - ch->ch_cpstime;
+
+	/*
+	 * Compute the time required to send digi_bufsize
+	 * characters.
+	 */
+
+	mt = HZ * ch->ch_digi.digi_bufsize / ch->ch_digi.digi_maxcps;
+
+	/*
+	 * Compute the number of characters that can be sent
+	 * without violating the time constraint.   If the
+	 * direct calculation of this number is bigger than
+	 * digi_bufsize, limit the number to digi_bufsize,
+	 * and adjust cpstime to match.
+	 */
+
+	if ((clock_t)(tt + HZ) > (clock_t)(mt + HZ)) {
+		tmax = ch->ch_digi.digi_bufsize;
+		ch->ch_cpstime = jiffies - mt;
+	} else {
+		tmax = ch->ch_digi.digi_maxcps * tt / HZ;
+	}
+
+	/*
+	 * If the time constraint now binds, limit the transmit
+	 * count accordingly, and tentatively arrange to be
+	 * rescheduled based on time.
+	 */
+
+	if (tmax < space) {
+		*un_flag = UN_TIME;
+		space = tmax;
+	}
+
+	/*
+	 * Compute the total number of characters we can
+	 * output before the total number of characters known
+	 * to be in the output queue exceeds digi_maxchar.
+	 */
+
+	tmax = (ch->ch_digi.digi_maxchar -
+		((ch->ch_tin - ch->ch_tout) & TBUF_MASK) -
+		((ch->ch_s_tin - ch->ch_s_tpos) & 0xffff));
+
+
+	/*
+	 * If the digi_maxchar constraint now holds, limit
+	 * the transmit count accordingly, and arrange to
+	 * be rescheduled when the queue becomes empty.
+	 */
+
+	if (space > tmax) {
+		*un_flag = UN_EMPTY;
+		space = tmax;
+	}
+
+	if (space <= 0)
+		*un_flag |= UN_EMPTY;
+
+	return space;
+}
+
+
+static int dgrp_tty_write(struct tty_struct *tty,
+			  const unsigned char *buf,
+			  int count)
+{
+	struct nd_struct *nd;
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int	space;
+	int	n;
+	int	t;
+	int sendcount;
+	int un_flag;
+	ulong lock_flags;
+
+	if (tty == NULL)
+		return 0;
+
+	un = tty->driver_data;
+	if (!un)
+		return 0;
+
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+
+	nd = ch->ch_nd;
+	if (!nd)
+		return 0;
+
+	/*
+	 * Ignore the request if the channel is not ready.
+	 */
+	if (ch->ch_state != CS_READY)
+		return 0;
+
+	spin_lock_irqsave(&dgrp_poll_data.poll_lock, lock_flags);
+
+	/*
+	 * Ignore the request if output is blocked.
+	 */
+	if ((un->un_flag & (UN_EMPTY | UN_LOW | UN_TIME | UN_PWAIT)) != 0) {
+		count = 0;
+		goto out;
+	}
+
+	/*
+	 * Also ignore the request if DPA has this port open,
+	 * and is flow controlled on reading more data.
+	 */
+	if (nd->nd_dpa_debug && nd->nd_dpa_flag & DPA_WAIT_SPACE &&
+		nd->nd_dpa_port == MINOR(tty_devnum(ch->ch_tun.un_tty))) {
+		count = 0;
+		goto out;
+	}
+
+	/*
+	 *	Limit amount we will write to the amount of space
+	 *	available in the channel buffer.
+	 */
+	sendcount = 0;
+
+	space = (ch->ch_tout - ch->ch_tin - 1) & TBUF_MASK;
+
+	/*
+	 * Handle the printer device.
+	 */
+
+	un_flag = UN_LOW;
+
+	if (IS_PRINT(MINOR(tty_devnum(tty)))) {
+		clock_t tt;
+		clock_t mt;
+		unsigned short tmax = 0;
+
+		/*
+		 * If the terminal device is busy, reschedule when
+		 * the terminal device becomes idle.
+		 */
+
+		if (ch->ch_tun.un_open_count != 0 &&
+		    ((ch->ch_tun.un_tty->ops->chars_in_buffer)(ch->ch_tun.un_tty) != 0)) {
+			un->un_flag |= UN_PWAIT;
+			count = 0;
+			goto out;
+		}
+
+		/*
+		 * Assure that whenever there is printer data in the output
+		 * buffer, there always remains enough space after it to
+		 * turn the printer off.
+		 */
+		space -= ch->ch_digi.digi_offlen;
+
+		/*
+		 * Output the printer on string.
+		 */
+
+		if ((ch->ch_flag & CH_PRON) == 0) {
+			space -= ch->ch_digi.digi_onlen;
+
+			if (space < 0) {
+				un->un_flag |= UN_EMPTY;
+				(ch->ch_nd)->nd_tx_work = 1;
+				count = 0;
+				goto out;
+			}
+
+			drp_wmove(ch, 0, ch->ch_digi.digi_onstr,
+				ch->ch_digi.digi_onlen);
+
+			ch->ch_flag |= CH_PRON;
+		}
+
+		/*
+		 * We measure printer CPS speed by incrementing
+		 * ch_cpstime by (HZ / digi_maxcps) for every
+		 * character we output, restricting output so
+		 * that ch_cpstime never exceeds lbolt.
+		 *
+		 * However if output has not been done for some
+		 * time, lbolt will grow to very much larger than
+		 * ch_cpstime, which would allow essentially
+		 * unlimited amounts of output until ch_cpstime
+		 * finally caught up.   To avoid this, we adjust
+		 * cps_time when necessary so the difference
+		 * between lbolt and ch_cpstime never results
+		 * in sending more than digi_bufsize characters.
+		 *
+		 * This nicely models a printer with an internal
+		 * buffer of digi_bufsize characters.
+		 *
+		 * Get the time between lbolt and ch->ch_cpstime;
+		 */
+
+		tt = jiffies - ch->ch_cpstime;
+
+		/*
+		 * Compute the time required to send digi_bufsize
+		 * characters.
+		 */
+
+		mt = HZ * ch->ch_digi.digi_bufsize / ch->ch_digi.digi_maxcps;
+
+		/*
+		 * Compute the number of characters that can be sent
+		 * without violating the time constraint.   If the
+		 * direct calculation of this number is bigger than
+		 * digi_bufsize, limit the number to digi_bufsize,
+		 * and adjust cpstime to match.
+		 */
+
+		if ((clock_t)(tt + HZ) > (clock_t)(mt + HZ)) {
+			tmax = ch->ch_digi.digi_bufsize;
+			ch->ch_cpstime = jiffies - mt;
+		} else {
+			tmax = ch->ch_digi.digi_maxcps * tt / HZ;
+		}
+
+		/*
+		 * If the time constraint now binds, limit the transmit
+		 * count accordingly, and tentatively arrange to be
+		 * rescheduled based on time.
+		 */
+
+		if (tmax < space) {
+			space = tmax;
+			un_flag = UN_TIME;
+		}
+
+		/*
+		 * Compute the total number of characters we can
+		 * output before the total number of characters known
+		 * to be in the output queue exceeds digi_maxchar.
+		 */
+
+		tmax = (ch->ch_digi.digi_maxchar -
+			((ch->ch_tin - ch->ch_tout) & TBUF_MASK) -
+			((ch->ch_s_tin - ch->ch_s_tpos) & 0xffff));
+
+
+		/*
+		 * If the digi_maxchar constraint now holds, limit
+		 * the transmit count accordingly, and arrange to
+		 * be rescheduled when the queue becomes empty.
+		 */
+
+		if (space > tmax) {
+			space = tmax;
+			un_flag = UN_EMPTY;
+		}
+
+	}
+	/*
+	 * Handle the terminal device.
+	 */
+	else {
+
+		/*
+		 * If the printer device is on, turn it off.
+		 */
+
+		if ((ch->ch_flag & CH_PRON) != 0) {
+
+			space -= ch->ch_digi.digi_offlen;
+
+			drp_wmove(ch, 0, ch->ch_digi.digi_offstr,
+				  ch->ch_digi.digi_offlen);
+
+			ch->ch_flag &= ~CH_PRON;
+		}
+	}
+
+	/*
+	 *	If space is 0 and its because the ch->tbuf
+	 *	is full, then Linux will handle a callback when queue
+	 *	space becomes available.
+	 *	tty_write returns count = 0
+	 */
+
+	if (space <= 0) {
+		/* the linux tty_io.c handles this if we return 0 */
+		/* if (fp->flags & O_NONBLOCK) return -EAGAIN; */
+
+		un->un_flag |= UN_EMPTY;
+		(ch->ch_nd)->nd_tx_work = 1;
+		count = 0;
+		goto out;
+	}
+
+	count = min(count, space);
+
+	if (count > 0) {
+
+		un->un_tbusy++;
+
+		/*
+		 *	Copy the buffer contents to the ch_tbuf
+		 *	being careful to wrap around the circular queue
+		 */
+
+		t = TBUF_MAX - ch->ch_tin;
+		n = count;
+
+		if (n >= t) {
+			memcpy(ch->ch_tbuf + ch->ch_tin, buf, t);
+			if (nd->nd_dpa_debug && nd->nd_dpa_port == PORT_NUM(MINOR(tty_devnum(un->un_tty))))
+				dgrp_dpa_data(nd, 0, (char *) buf, t);
+			buf += t;
+			n -= t;
+			ch->ch_tin = 0;
+			sendcount += n;
+		}
+
+		memcpy(ch->ch_tbuf + ch->ch_tin, buf, n);
+		if (nd->nd_dpa_debug && nd->nd_dpa_port == PORT_NUM(MINOR(tty_devnum(un->un_tty))))
+			dgrp_dpa_data(nd, 0, (char *) buf, n);
+		buf += n;
+		ch->ch_tin += n;
+		sendcount += n;
+
+		un->un_tbusy--;
+		(ch->ch_nd)->nd_tx_work = 1;
+		if (ch->ch_edelay != DGRP_RTIME) {
+			(ch->ch_nd)->nd_tx_ready = 1;
+			wake_up_interruptible(&nd->nd_tx_waitq);
+		}
+	}
+
+	ch->ch_txcount += count;
+
+	if (IS_PRINT(MINOR(tty_devnum(tty)))) {
+
+		/*
+		 * Adjust ch_cpstime to account
+		 * for the characters just output.
+		 */
+
+		if (sendcount > 0) {
+			int cc = HZ * sendcount + ch->ch_cpsrem;
+
+			ch->ch_cpstime += cc / ch->ch_digi.digi_maxcps;
+			ch->ch_cpsrem   = cc % ch->ch_digi.digi_maxcps;
+		}
+
+		/*
+		 * If we are now waiting on time, schedule ourself
+		 * back when we'll be able to send a block of
+		 * digi_maxchar characters.
+		 */
+
+		if ((un_flag & UN_TIME) != 0) {
+			ch->ch_waketime = (ch->ch_cpstime +
+				(ch->ch_digi.digi_maxchar * HZ /
+				ch->ch_digi.digi_maxcps));
+		}
+	}
+
+	/*
+	 * If the printer unit is waiting for completion
+	 * of terminal output, get him going again.
+	 */
+
+	if ((ch->ch_pun.un_flag & UN_PWAIT) != 0)
+		(ch->ch_nd)->nd_tx_work = 1;
+
+out:
+	spin_unlock_irqrestore(&dgrp_poll_data.poll_lock, lock_flags);
+
+	return count;
+}
+
+
+/*
+ *	Put a character into ch->ch_buf
+ *
+ *	- used by the line discipline for OPOST processing
+ */
+
+static int dgrp_tty_put_char(struct tty_struct *tty, unsigned char new_char)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	ulong  lock_flags;
+	int space;
+	int retval = 0;
+
+	if (tty == NULL)
+		return 0;
+
+	un = tty->driver_data;
+	if (!un)
+		return 0;
+
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+
+	if (ch->ch_state != CS_READY)
+		return 0;
+
+	spin_lock_irqsave(&dgrp_poll_data.poll_lock, lock_flags);
+
+
+	/*
+	 *	If space is 0 and its because the ch->tbuf
+	 *	Warn and dump the character, there isn't anything else
+	 *	we can do about it.  David_Fries@digi.com
+	 */
+
+	space = (ch->ch_tout - ch->ch_tin - 1) & TBUF_MASK;
+
+	un->un_tbusy++;
+
+	/*
+	 * Output the printer on string if device is TXPrint.
+	 */
+	if (IS_PRINT(MINOR(tty_devnum(tty))) && (ch->ch_flag & CH_PRON) == 0) {
+		if (space < ch->ch_digi.digi_onlen) {
+			un->un_tbusy--;
+			goto out;
+		}
+		space -= ch->ch_digi.digi_onlen;
+		drp_wmove(ch, 0, ch->ch_digi.digi_onstr,
+			  ch->ch_digi.digi_onlen);
+		ch->ch_flag |= CH_PRON;
+	}
+
+	/*
+	 * Output the printer off string if device is NOT TXPrint.
+	 */
+
+	if (!IS_PRINT(MINOR(tty_devnum(tty))) &&
+	    ((ch->ch_flag & CH_PRON) != 0)) {
+		if (space < ch->ch_digi.digi_offlen) {
+			un->un_tbusy--;
+			goto out;
+		}
+
+		space -= ch->ch_digi.digi_offlen;
+		drp_wmove(ch, 0, ch->ch_digi.digi_offstr,
+			  ch->ch_digi.digi_offlen);
+		ch->ch_flag &= ~CH_PRON;
+	}
+
+	if (!space) {
+		un->un_tbusy--;
+		goto out;
+	}
+
+	/*
+	 *	Copy the character to the ch_tbuf being
+	 *	careful to wrap around the circular queue
+	 */
+	ch->ch_tbuf[ch->ch_tin] = new_char;
+	ch->ch_tin = (1 + ch->ch_tin) & TBUF_MASK;
+
+	if (IS_PRINT(MINOR(tty_devnum(tty)))) {
+
+		/*
+		 * Adjust ch_cpstime to account
+		 * for the character just output.
+		 */
+
+		int cc = HZ + ch->ch_cpsrem;
+
+		ch->ch_cpstime += cc / ch->ch_digi.digi_maxcps;
+		ch->ch_cpsrem   = cc % ch->ch_digi.digi_maxcps;
+
+		/*
+		 * If we are now waiting on time, schedule ourself
+		 * back when we'll be able to send a block of
+		 * digi_maxchar characters.
+		 */
+
+		ch->ch_waketime = (ch->ch_cpstime +
+			(ch->ch_digi.digi_maxchar * HZ /
+			ch->ch_digi.digi_maxcps));
+	}
+
+
+	un->un_tbusy--;
+	(ch->ch_nd)->nd_tx_work = 1;
+
+	retval = 1;
+out:
+	spin_unlock_irqrestore(&dgrp_poll_data.poll_lock, lock_flags);
+	return retval;
+}
+
+
+
+/*
+ *	Flush TX buffer (make in == out)
+ *
+ *	check tty_ioctl.c  -- this is called after TCOFLUSH
+ */
+static void dgrp_tty_flush_buffer(struct tty_struct *tty)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+	un = tty->driver_data;
+	if (!un)
+		return;
+
+	ch = un->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_tout = ch->ch_tin;
+	/* do NOT do this here! */
+	/* ch->ch_s_tpos = ch->ch_s_tin = 0; */
+
+	/* send the flush output command now */
+	ch->ch_send |= RR_TX_FLUSH;
+	(ch->ch_nd)->nd_tx_ready = 1;
+	(ch->ch_nd)->nd_tx_work = 1;
+	wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+
+	if (waitqueue_active(&tty->write_wait))
+		wake_up_interruptible(&tty->write_wait);
+
+	tty_wakeup(tty);
+
+}
+
+/*
+ *	Return space available in Tx buffer
+ *	count = ( ch->ch_tout - ch->ch_tin ) mod (TBUF_MAX - 1)
+ */
+static int dgrp_tty_write_room(struct tty_struct *tty)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int	count;
+
+	if (!tty)
+		return 0;
+
+	un = tty->driver_data;
+	if (!un)
+		return 0;
+
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+
+	count = (ch->ch_tout - ch->ch_tin - 1) & TBUF_MASK;
+
+	/* We *MUST* check this, and return 0 if the Printer Unit cannot
+	 * take any more data within its time constraints...  If we don't
+	 * return 0 and the printer has hit it time constraint, the ld will
+	 * call us back doing a put_char, which cannot be rejected!!!
+	 */
+	if (IS_PRINT(MINOR(tty_devnum(tty)))) {
+		int un_flag = 0;
+		count = dgrp_calculate_txprint_bounds(ch, count, &un_flag);
+		if (count <= 0)
+			count = 0;
+
+		ch->ch_pun.un_flag |= un_flag;
+		(ch->ch_nd)->nd_tx_work = 1;
+	}
+
+	return count;
+}
+
+/*
+ *	Return number of characters that have not been transmitted yet.
+ *	chars_in_buffer = ( ch->ch_tin - ch->ch_tout ) mod (TBUF_MAX - 1)
+ *			+ ( ch->ch_s_tin - ch->ch_s_tout ) mod (0xffff)
+ *			= number of characters "in transit"
+ *
+ * Remember that sequence number math is always with a sixteen bit
+ * mask, not the TBUF_MASK.
+ */
+
+static int dgrp_tty_chars_in_buffer(struct tty_struct *tty)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int	count;
+	int	count1;
+
+	if (!tty)
+		return 0;
+
+	un = tty->driver_data;
+	if (!un)
+		return 0;
+
+	ch = un->un_ch;
+	if (!ch)
+		return 0;
+
+	count1 = count = (ch->ch_tin - ch->ch_tout) & TBUF_MASK;
+	count += (ch->ch_s_tin - ch->ch_s_tpos) & 0xffff;
+	/* one for tbuf, one for the PS */
+
+	/*
+	 * If we are busy transmitting add 1
+	 */
+	count += un->un_tbusy;
+
+	return count;
+}
+
+
+/*****************************************************************************
+ *
+ * Helper applications for dgrp_tty_ioctl()
+ *
+ *****************************************************************************
+ */
+
+
+/**
+ * ch_to_tty_flags() -- convert channel flags to termio flags
+ * @ch_flag: Digi channel flags
+ * @flagtype: type of ch_flag (iflag, oflag or cflag)
+ *
+ * take the channel flags of the specified type and return the
+ * corresponding termio flag
+ */
+static tcflag_t ch_to_tty_flags(ushort ch_flag, char flagtype)
+{
+	tcflag_t retval = 0;
+
+	switch (flagtype) {
+	case 'i':
+		retval = ((ch_flag & IF_IGNBRK) ? IGNBRK : 0)
+		     | ((ch_flag & IF_BRKINT) ? BRKINT : 0)
+		     | ((ch_flag & IF_IGNPAR) ? IGNPAR : 0)
+		     | ((ch_flag & IF_PARMRK) ? PARMRK : 0)
+		     | ((ch_flag & IF_INPCK) ? INPCK  : 0)
+		     | ((ch_flag & IF_ISTRIP) ? ISTRIP : 0)
+		     | ((ch_flag & IF_IXON) ? IXON   : 0)
+		     | ((ch_flag & IF_IXANY) ? IXANY  : 0)
+		     | ((ch_flag & IF_IXOFF) ? IXOFF  : 0);
+		break;
+
+	case 'o':
+		retval = ((ch_flag & OF_OLCUC) ? OLCUC : 0)
+		     | ((ch_flag & OF_ONLCR) ? ONLCR  : 0)
+		     | ((ch_flag & OF_OCRNL) ? OCRNL  : 0)
+		     | ((ch_flag & OF_ONOCR) ? ONOCR  : 0)
+		     | ((ch_flag & OF_ONLRET) ? ONLRET : 0)
+		  /* | ((ch_flag & OF_OTAB3) ? OFILL  : 0) */
+		     | ((ch_flag & OF_TABDLY) ? TABDLY : 0);
+		break;
+
+	case 'c':
+		retval = ((ch_flag & CF_CSTOPB) ? CSTOPB : 0)
+		     | ((ch_flag & CF_CREAD) ? CREAD  : 0)
+		     | ((ch_flag & CF_PARENB) ? PARENB : 0)
+		     | ((ch_flag & CF_PARODD) ? PARODD : 0)
+		     | ((ch_flag & CF_HUPCL) ? HUPCL  : 0);
+
+		switch (ch_flag & CF_CSIZE) {
+		case CF_CS5:
+			retval |= CS5;
+			break;
+		case CF_CS6:
+			retval |= CS6;
+			break;
+		case CF_CS7:
+			retval |= CS7;
+			break;
+		case CF_CS8:
+			retval |= CS8;
+			break;
+		default:
+			retval |= CS8;
+			break;
+		}
+		break;
+	case 'x':
+		break;
+	case 'l':
+		break;
+	default:
+		return 0;
+	}
+
+	return retval;
+}
+
+
+/**
+ * tty_to_ch_flags() -- convert termio flags to digi channel flags
+ * @tty: pointer to a TTY structure holding flag to be converted
+ * @flagtype: identifies which flag (iflags, oflags, or cflags) should
+ *                 be converted
+ *
+ * take the termio flag of the specified type and return the
+ * corresponding Digi version of the flags
+ */
+static ushort tty_to_ch_flags(struct tty_struct *tty, char flagtype)
+{
+	ushort retval = 0;
+	tcflag_t tflag = 0;
+
+	switch (flagtype) {
+	case 'i':
+		tflag  = tty->termios.c_iflag;
+		retval = (I_IGNBRK(tty) ? IF_IGNBRK : 0)
+		      | (I_BRKINT(tty) ? IF_BRKINT : 0)
+		      | (I_IGNPAR(tty) ? IF_IGNPAR : 0)
+		      | (I_PARMRK(tty) ? IF_PARMRK : 0)
+		      | (I_INPCK(tty)  ? IF_INPCK  : 0)
+		      | (I_ISTRIP(tty) ? IF_ISTRIP : 0)
+		      | (I_IXON(tty)   ? IF_IXON   : 0)
+		      | (I_IXANY(tty)  ? IF_IXANY  : 0)
+		      | (I_IXOFF(tty)  ? IF_IXOFF  : 0);
+		break;
+	case 'o':
+		tflag  = tty->termios.c_oflag;
+		/*
+		 * If OPOST is set, then do the post processing in the
+		 * firmware by setting all the processing flags on.
+		 * If ~OPOST, then make sure we are not doing any
+		 * output processing!!
+		 */
+		if (!O_OPOST(tty))
+			retval = 0;
+		else
+			retval = (O_OLCUC(tty) ? OF_OLCUC : 0)
+			     | (O_ONLCR(tty)  ? OF_ONLCR  : 0)
+			     | (O_OCRNL(tty)  ? OF_OCRNL  : 0)
+			     | (O_ONOCR(tty)  ? OF_ONOCR  : 0)
+			     | (O_ONLRET(tty) ? OF_ONLRET : 0)
+			  /* | (O_OFILL(tty)  ? OF_TAB3   : 0) */
+			     | (O_TABDLY(tty) ? OF_TABDLY : 0);
+		break;
+	case 'c':
+		tflag  = tty->termios.c_cflag;
+		retval = (C_CSTOPB(tty) ? CF_CSTOPB : 0)
+		     | (C_CREAD(tty)  ? CF_CREAD  : 0)
+		     | (C_PARENB(tty) ? CF_PARENB : 0)
+		     | (C_PARODD(tty) ? CF_PARODD : 0)
+		     | (C_HUPCL(tty)  ? CF_HUPCL  : 0);
+		switch (C_CSIZE(tty)) {
+		case CS8:
+			retval |= CF_CS8;
+			break;
+		case CS7:
+			retval |= CF_CS7;
+			break;
+		case CS6:
+			retval |= CF_CS6;
+			break;
+		case CS5:
+			retval |= CF_CS5;
+			break;
+		default:
+			retval |= CF_CS8;
+			break;
+		}
+		break;
+	case 'x':
+		break;
+	case 'l':
+		break;
+	default:
+		return 0;
+	}
+
+	return retval;
+}
+
+
+static int dgrp_tty_send_break(struct tty_struct *tty, int msec)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int ret = -EIO;
+
+	if (!tty)
+		return ret;
+
+	un = tty->driver_data;
+	if (!un)
+		return ret;
+
+	ch = un->un_ch;
+	if (!ch)
+		return ret;
+
+	dgrp_send_break(ch, msec);
+	return 0;
+}
+
+
+/*
+ * This routine sends a break character out the serial port.
+ *
+ * duration is in 1/1000's of a second
+ */
+static int dgrp_send_break(struct ch_struct *ch, int msec)
+{
+	ulong x;
+
+	wait_event_interruptible(ch->ch_flag_wait,
+		((ch->ch_flag & CH_TX_BREAK) == 0));
+	ch->ch_break_time += max(msec, 250);
+	ch->ch_send |= RR_TX_BREAK;
+	ch->ch_flag |= CH_TX_BREAK;
+	(ch->ch_nd)->nd_tx_work = 1;
+
+	x = (msec * HZ) / 1000;
+	wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+
+	return 0;
+}
+
+
+/*
+ * Return modem signals to ld.
+ */
+static int dgrp_tty_tiocmget(struct tty_struct *tty)
+{
+	unsigned int mlast;
+	struct un_struct *un = tty->driver_data;
+	struct ch_struct *ch;
+
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	mlast = ((ch->ch_s_mlast & ~(DM_RTS | DM_DTR)) |
+		(ch->ch_mout & (DM_RTS | DM_DTR)));
+
+	/* defined in /usr/include/asm/termios.h */
+	mlast =   ((mlast & DM_RTS) ? TIOCM_RTS : 0)
+		| ((mlast & DM_DTR) ? TIOCM_DTR : 0)
+		| ((mlast & DM_CD)  ? TIOCM_CAR : 0)
+		| ((mlast & DM_RI)  ? TIOCM_RNG : 0)
+		| ((mlast & DM_DSR) ? TIOCM_DSR : 0)
+		| ((mlast & DM_CTS) ? TIOCM_CTS : 0);
+
+	return mlast;
+}
+
+
+/*
+ *      Set modem lines
+ */
+static int dgrp_tty_tiocmset(struct tty_struct *tty,
+			     unsigned int set, unsigned int clear)
+{
+	ulong lock_flags;
+	struct un_struct *un = tty->driver_data;
+	struct ch_struct *ch;
+
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	if (set & TIOCM_RTS)
+		ch->ch_mout |= DM_RTS;
+
+	if (set & TIOCM_DTR)
+		ch->ch_mout |= DM_DTR;
+
+	if (clear & TIOCM_RTS)
+		ch->ch_mout &= ~(DM_RTS);
+
+	if (clear & TIOCM_DTR)
+		ch->ch_mout &= ~(DM_DTR);
+
+	spin_lock_irqsave(&(ch->ch_nd)->nd_lock, lock_flags);
+	ch->ch_flag |= CH_PARAM;
+	(ch->ch_nd)->nd_tx_work = 1;
+	wake_up_interruptible(&ch->ch_flag_wait);
+
+	spin_unlock_irqrestore(&(ch->ch_nd)->nd_lock, lock_flags);
+
+	return 0;
+}
+
+
+
+/*
+ *      Get current modem status
+ */
+static int get_modem_info(struct ch_struct *ch, unsigned int *value)
+{
+	unsigned int mlast;
+
+	mlast = ((ch->ch_s_mlast & ~(DM_RTS | DM_DTR)) |
+		(ch->ch_mout    &  (DM_RTS | DM_DTR)));
+
+	/* defined in /usr/include/asm/termios.h */
+	mlast =   ((mlast & DM_RTS) ? TIOCM_RTS : 0)
+		| ((mlast & DM_DTR) ? TIOCM_DTR : 0)
+		| ((mlast & DM_CD)  ? TIOCM_CAR : 0)
+		| ((mlast & DM_RI)  ? TIOCM_RNG : 0)
+		| ((mlast & DM_DSR) ? TIOCM_DSR : 0)
+		| ((mlast & DM_CTS) ? TIOCM_CTS : 0);
+	put_user(mlast, (unsigned int __user *) value);
+
+	return 0;
+}
+
+/*
+ *      Set modem lines
+ */
+static int set_modem_info(struct ch_struct *ch, unsigned int command,
+			  unsigned int *value)
+{
+	int error;
+	unsigned int arg;
+	int mval = 0;
+	ulong lock_flags;
+
+	error = access_ok(VERIFY_READ, (void __user *) value, sizeof(int));
+	if (error == 0)
+		return -EFAULT;
+
+	get_user(arg, (unsigned int __user *) value);
+	mval |= ((arg & TIOCM_RTS) ? DM_RTS : 0)
+		| ((arg & TIOCM_DTR) ? DM_DTR : 0);
+
+	switch (command) {
+	case TIOCMBIS:  /* set flags */
+		ch->ch_mout |= mval;
+		break;
+	case TIOCMBIC:  /* clear flags */
+		ch->ch_mout &= ~mval;
+		break;
+	case TIOCMSET:
+		ch->ch_mout = mval;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&(ch->ch_nd)->nd_lock, lock_flags);
+
+	ch->ch_flag |= CH_PARAM;
+	(ch->ch_nd)->nd_tx_work = 1;
+	wake_up_interruptible(&ch->ch_flag_wait);
+
+	spin_unlock_irqrestore(&(ch->ch_nd)->nd_lock, lock_flags);
+
+	return 0;
+}
+
+
+/*
+ *  Assign the custom baud rate to the channel structure
+ */
+static void dgrp_set_custom_speed(struct ch_struct *ch, int newrate)
+{
+	int testdiv;
+	int testrate_high;
+	int testrate_low;
+
+	int deltahigh, deltalow;
+
+	if (newrate < 0)
+		newrate = 0;
+
+	/*
+	 * Since the divisor is stored in a 16-bit integer, we make sure
+	 * we don't allow any rates smaller than a 16-bit integer would allow.
+	 * And of course, rates above the dividend won't fly.
+	 */
+	if (newrate && newrate < ((PORTSERVER_DIVIDEND / 0xFFFF) + 1))
+		newrate = ((PORTSERVER_DIVIDEND / 0xFFFF) + 1);
+	if (newrate && newrate > PORTSERVER_DIVIDEND)
+		newrate = PORTSERVER_DIVIDEND;
+
+	while (newrate > 0) {
+		testdiv = PORTSERVER_DIVIDEND / newrate;
+
+		/*
+		 * If we try to figure out what rate the PortServer would use
+		 * with the test divisor, it will be either equal or higher
+		 * than the requested baud rate.  If we then determine the
+		 * rate with a divisor one higher, we will get the next lower
+		 * supported rate below the requested.
+		 */
+		testrate_high = PORTSERVER_DIVIDEND / testdiv;
+		testrate_low  = PORTSERVER_DIVIDEND / (testdiv + 1);
+
+		/*
+		 * If the rate for the requested divisor is correct, just
+		 * use it and be done.
+		 */
+		if (testrate_high == newrate)
+			break;
+
+		/*
+		 * Otherwise, pick the rate that is closer (i.e. whichever rate
+		 * has a smaller delta).
+		 */
+		deltahigh = testrate_high - newrate;
+		deltalow = newrate - testrate_low;
+
+		if (deltahigh < deltalow)
+			newrate = testrate_high;
+		else
+			newrate = testrate_low;
+
+		break;
+	}
+
+	ch->ch_custom_speed = newrate;
+
+	drp_param(ch);
+
+	return;
+}
+
+
+/*
+ # dgrp_tty_digiseta()
+ *
+ * Ioctl to set the information from ditty.
+ *
+ * NOTE: DIGI_IXON, DSRPACE, DCDPACE, and DTRPACE are unsupported.  JAR 990922
+ */
+static int dgrp_tty_digiseta(struct tty_struct *tty,
+			     struct digi_struct *new_info)
+{
+	struct un_struct *un = tty->driver_data;
+	struct ch_struct *ch;
+
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	if (copy_from_user(&ch->ch_digi, (void __user *) new_info,
+			   sizeof(struct digi_struct)))
+		return -EFAULT;
+
+	if ((ch->ch_digi.digi_flags & RTSPACE) ||
+	    (ch->ch_digi.digi_flags & CTSPACE))
+		tty->termios.c_cflag |= CRTSCTS;
+	else
+		tty->termios.c_cflag &= ~CRTSCTS;
+
+	if (ch->ch_digi.digi_maxcps < 1)
+		ch->ch_digi.digi_maxcps = 1;
+
+	if (ch->ch_digi.digi_maxcps > 10000)
+		ch->ch_digi.digi_maxcps = 10000;
+
+	if (ch->ch_digi.digi_bufsize < 10)
+		ch->ch_digi.digi_bufsize = 10;
+
+	if (ch->ch_digi.digi_maxchar < 1)
+		ch->ch_digi.digi_maxchar = 1;
+
+	if (ch->ch_digi.digi_maxchar > ch->ch_digi.digi_bufsize)
+		ch->ch_digi.digi_maxchar = ch->ch_digi.digi_bufsize;
+
+	if (ch->ch_digi.digi_onlen > DIGI_PLEN)
+		ch->ch_digi.digi_onlen = DIGI_PLEN;
+
+	if (ch->ch_digi.digi_offlen > DIGI_PLEN)
+		ch->ch_digi.digi_offlen = DIGI_PLEN;
+
+	/* make the changes now */
+	drp_param(ch);
+
+	return 0;
+}
+
+
+
+/*
+ * dgrp_tty_digigetedelay()
+ *
+ * Ioctl to get the current edelay setting.
+ *
+ *
+ *
+ */
+static int dgrp_tty_digigetedelay(struct tty_struct *tty, int *retinfo)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int tmp;
+
+	if (!retinfo)
+		return -EFAULT;
+
+	if (!tty || tty->magic != TTY_MAGIC)
+		return -EFAULT;
+
+	un = tty->driver_data;
+
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	tmp = ch->ch_edelay;
+
+	if (copy_to_user((void __user *) retinfo, &tmp, sizeof(*retinfo)))
+		return -EFAULT;
+
+	return 0;
+}
+
+
+/*
+ * dgrp_tty_digisetedelay()
+ *
+ * Ioctl to set the EDELAY setting
+ *
+ */
+static int dgrp_tty_digisetedelay(struct tty_struct *tty, int *new_info)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int new_digi;
+
+	if (!tty || tty->magic != TTY_MAGIC)
+		return -EFAULT;
+
+	un = tty->driver_data;
+
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	if (copy_from_user(&new_digi, (void __user *)new_info, sizeof(int)))
+		return -EFAULT;
+
+	ch->ch_edelay = new_digi;
+
+	/* make the changes now */
+	drp_param(ch);
+
+	return 0;
+}
+
+
+/*
+ *	The usual assortment of ioctl's
+ *
+ *	note:  use tty_check_change to make sure that we are not
+ *	changing the state of a terminal when we are not a process
+ *	in the forground.  See tty_io.c
+ *		rc = tty_check_change(tty);
+ *		if (rc) return rc;
+ */
+static int dgrp_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
+			  unsigned long arg)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+	int rc;
+	struct digiflow_struct   dflow;
+
+	if (!tty)
+		return -ENODEV;
+
+	un = tty->driver_data;
+	if (!un)
+		return -ENODEV;
+
+	ch = un->un_ch;
+	if (!ch)
+		return -ENODEV;
+
+	switch (cmd) {
+
+	/*
+	 * Here are all the standard ioctl's that we MUST implement
+	 */
+
+	case TCSBRK:
+		/*
+		 * TCSBRK is SVID version: non-zero arg --> no break
+		 * this behaviour is exploited by tcdrain().
+		 *
+		 * According to POSIX.1 spec (7.2.2.1.2) breaks should be
+		 * between 0.25 and 0.5 seconds
+		 */
+
+		rc = tty_check_change(tty);
+		if (rc)
+			return rc;
+		tty_wait_until_sent(tty, 0);
+
+		if (!arg)
+			rc = dgrp_send_break(ch, 250); /* 1/4 second */
+
+		if (dgrp_tty_chars_in_buffer(tty) != 0)
+			return -EINTR;
+
+		return 0;
+
+	case TCSBRKP:
+		/* support for POSIX tcsendbreak()
+		 *
+		 * According to POSIX.1 spec (7.2.2.1.2) breaks should be
+		 * between 0.25 and 0.5 seconds so we'll ask for something
+		 * in the middle: 0.375 seconds.
+		 */
+		rc = tty_check_change(tty);
+		if (rc)
+			return rc;
+		tty_wait_until_sent(tty, 0);
+
+		rc = dgrp_send_break(ch, arg ? arg*250 : 250);
+
+		if (dgrp_tty_chars_in_buffer(tty) != 0)
+			return -EINTR;
+		return 0;
+
+	case TIOCSBRK:
+		rc = tty_check_change(tty);
+		if (rc)
+			return rc;
+		tty_wait_until_sent(tty, 0);
+
+		/*
+		 * RealPort doesn't support turning on a break unconditionally.
+		 * The RealPort device will stop sending a break automatically
+		 * after the specified time value that we send in.
+		 */
+		rc = dgrp_send_break(ch, 250); /* 1/4 second */
+
+		if (dgrp_tty_chars_in_buffer(tty) != 0)
+			return -EINTR;
+		return 0;
+
+	case TIOCCBRK:
+		/*
+		 * RealPort doesn't support turning off a break unconditionally.
+		 * The RealPort device will stop sending a break automatically
+		 * after the specified time value that was sent when turning on
+		 * the break.
+		 */
+		return 0;
+
+	case TIOCGSOFTCAR:
+		rc = access_ok(VERIFY_WRITE, (void __user *) arg,
+			       sizeof(long));
+		if (rc == 0)
+			return -EFAULT;
+		put_user(C_CLOCAL(tty) ? 1 : 0, (unsigned long __user *) arg);
+		return 0;
+
+	case TIOCSSOFTCAR:
+		get_user(arg, (unsigned long __user *) arg);
+		tty->termios.c_cflag =
+			((tty->termios.c_cflag & ~CLOCAL) |
+			 (arg ? CLOCAL : 0));
+		return 0;
+
+	case TIOCMGET:
+		rc = access_ok(VERIFY_WRITE, (void __user *) arg,
+				 sizeof(unsigned int));
+		if (rc == 0)
+			return -EFAULT;
+		return get_modem_info(ch, (unsigned int *) arg);
+
+	case TIOCMBIS:
+	case TIOCMBIC:
+	case TIOCMSET:
+		return set_modem_info(ch, cmd, (unsigned int *) arg);
+
+	/*
+	 * Here are any additional ioctl's that we want to implement
+	 */
+
+	case TCFLSH:
+		/*
+		 * The linux tty driver doesn't have a flush
+		 * input routine for the driver, assuming all backed
+		 * up data is in the line disc. buffers.  However,
+		 * we all know that's not the case.  Here, we
+		 * act on the ioctl, but then lie and say we didn't
+		 * so the line discipline will process the flush
+		 * also.
+		 */
+		rc = tty_check_change(tty);
+		if (rc)
+			return rc;
+
+		switch (arg) {
+		case TCIFLUSH:
+		case TCIOFLUSH:
+			/* only flush input if this is the only open unit */
+			if (!IS_PRINT(MINOR(tty_devnum(tty)))) {
+				ch->ch_rout = ch->ch_rin;
+				ch->ch_send |= RR_RX_FLUSH;
+				(ch->ch_nd)->nd_tx_work = 1;
+				(ch->ch_nd)->nd_tx_ready = 1;
+				wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+			}
+			if (arg == TCIFLUSH)
+				break;
+
+		case TCOFLUSH: /* flush output, or the receive buffer */
+			/*
+			 * This is handled in the tty_ioctl.c code
+			 * calling tty_flush_buffer
+			 */
+			break;
+
+		default:
+			/* POSIX.1 says return EINVAL if we got a bad arg */
+			return -EINVAL;
+		}
+		/* pretend we didn't recognize this IOCTL */
+		return -ENOIOCTLCMD;
+
+#ifdef TIOCGETP
+	case TIOCGETP:
+#endif
+	/*****************************************
+	Linux		HPUX		Function
+	TCSETA		TCSETA		- set the termios
+	TCSETAF		TCSETAF		- wait for drain first, then set termios
+	TCSETAW		TCSETAW		- wait for drain, flush the input queue, then set termios
+	- looking at the tty_ioctl code, these command all call our
+	tty_set_termios at the driver's end, when a TCSETA* is sent,
+	it is expecting the tty to have a termio structure,
+	NOT a termios stucture.  These two structures differ in size
+	and the tty_ioctl code does a conversion before processing them both.
+	- we should treat the TCSETAW TCSETAF ioctls the same, and let
+	the tty_ioctl code do the conversion stuff.
+
+	TCSETS
+	TCSETSF		(none)
+	TCSETSW
+	- the associated tty structure has a termios structure.
+	*****************************************/
+
+	case TCGETS:
+	case TCGETA:
+		return -ENOIOCTLCMD;
+
+	case TCSETAW:
+	case TCSETAF:
+	case TCSETSF:
+	case TCSETSW:
+		/*
+		 * The linux tty driver doesn't have a flush
+		 * input routine for the driver, assuming all backed
+		 * up data is in the line disc. buffers.  However,
+		 * we all know that's not the case.  Here, we
+		 * act on the ioctl, but then lie and say we didn't
+		 * so the line discipline will process the flush
+		 * also.
+		 */
+
+		/*
+		 * Also, now that we have TXPrint, we have to check
+		 * if this is the TXPrint device and the terminal
+		 * device is open. If so, do NOT run check_change,
+		 * as the terminal device is ALWAYS the parent.
+		 */
+		if (!IS_PRINT(MINOR(tty_devnum(tty))) ||
+		    !ch->ch_tun.un_open_count) {
+			rc = tty_check_change(tty);
+			if (rc)
+				return rc;
+		}
+
+		/* wait for all the characters in tbuf to drain */
+		tty_wait_until_sent(tty, 0);
+
+		if ((cmd == TCSETSF) || (cmd == TCSETAF)) {
+			/* flush the contents of the rbuf queue */
+			/* TODO:  check if this is print device? */
+			ch->ch_send |= RR_RX_FLUSH;
+			(ch->ch_nd)->nd_tx_ready = 1;
+			(ch->ch_nd)->nd_tx_work = 1;
+			wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+			/* do we need to do this?  just to be safe! */
+			ch->ch_rout = ch->ch_rin;
+		}
+
+		/* pretend we didn't recognize this */
+		return -ENOIOCTLCMD;
+
+	case TCXONC:
+		/*
+		 * The Linux Line Discipline (LD) would do this for us if we
+		 * let it, but we have the special firmware options to do this
+		 * the "right way" regardless of hardware or software flow
+		 * control so we'll do it outselves instead of letting the LD
+		 * do it.
+		 */
+		rc = tty_check_change(tty);
+		if (rc)
+			return rc;
+
+		switch (arg) {
+		case TCOON:
+			dgrp_tty_start(tty);
+			return 0;
+		case TCOOFF:
+			dgrp_tty_stop(tty);
+			return 0;
+		case TCION:
+			dgrp_tty_input_start(tty);
+			return 0;
+		case TCIOFF:
+			dgrp_tty_input_stop(tty);
+			return 0;
+		default:
+			return -EINVAL;
+		}
+
+	case DIGI_GETA:
+		/* get information for ditty */
+		if (copy_to_user((struct digi_struct __user *) arg,
+				 &ch->ch_digi, sizeof(struct digi_struct)))
+			return -EFAULT;
+		break;
+
+	case DIGI_SETAW:
+	case DIGI_SETAF:
+		/* wait for all the characters in tbuf to drain */
+		tty_wait_until_sent(tty, 0);
+
+		if (cmd == DIGI_SETAF) {
+			/* flush the contents of the rbuf queue */
+			/* send down a packet with RR_RX_FLUSH set */
+			ch->ch_send |= RR_RX_FLUSH;
+			(ch->ch_nd)->nd_tx_ready = 1;
+			(ch->ch_nd)->nd_tx_work = 1;
+			wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+			/* do we need to do this?  just to be safe! */
+			ch->ch_rout = ch->ch_rin;
+		}
+
+		/* pretend we didn't recognize this */
+
+	case DIGI_SETA:
+		return dgrp_tty_digiseta(tty, (struct digi_struct *) arg);
+
+	case DIGI_SEDELAY:
+		return dgrp_tty_digisetedelay(tty, (int *) arg);
+
+	case DIGI_GEDELAY:
+		return dgrp_tty_digigetedelay(tty, (int *) arg);
+
+	case DIGI_GETFLOW:
+	case DIGI_GETAFLOW:
+		if (cmd == (DIGI_GETFLOW)) {
+			dflow.startc = tty->termios.c_cc[VSTART];
+			dflow.stopc = tty->termios.c_cc[VSTOP];
+		} else {
+			dflow.startc = ch->ch_xxon;
+			dflow.stopc = ch->ch_xxoff;
+		}
+
+		if (copy_to_user((char __user *)arg, &dflow, sizeof(dflow)))
+			return -EFAULT;
+		break;
+
+	case DIGI_SETFLOW:
+	case DIGI_SETAFLOW:
+
+		if (copy_from_user(&dflow, (char __user *)arg, sizeof(dflow)))
+			return -EFAULT;
+
+		if (cmd == (DIGI_SETFLOW)) {
+			tty->termios.c_cc[VSTART] = dflow.startc;
+			tty->termios.c_cc[VSTOP] = dflow.stopc;
+		} else {
+			ch->ch_xxon = dflow.startc;
+			ch->ch_xxoff = dflow.stopc;
+		}
+		break;
+
+	case DIGI_GETCUSTOMBAUD:
+		rc = access_ok(VERIFY_WRITE, (void __user *) arg, sizeof(int));
+		if (rc == 0)
+			return -EFAULT;
+		put_user(ch->ch_custom_speed, (unsigned int __user *) arg);
+		break;
+
+	case DIGI_SETCUSTOMBAUD:
+	{
+		int new_rate;
+
+		get_user(new_rate, (unsigned int __user *) arg);
+		dgrp_set_custom_speed(ch, new_rate);
+
+		break;
+	}
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+
+	return 0;
+}
+
+/*
+ *  This routine allows the tty driver to be notified when
+ *  the device's termios setting have changed.  Note that we
+ *  should be prepared to accept the case where old == NULL
+ *  and try to do something rational.
+ *
+ *  So we need to make sure that our copies of ch_oflag,
+ *  ch_clag, and ch_iflag reflect the tty->termios flags.
+ */
+static void dgrp_tty_set_termios(struct tty_struct *tty, struct ktermios *old)
+{
+	struct ktermios *ts;
+	struct ch_struct *ch;
+	struct un_struct *un;
+
+	/* seems silly, but we have to check all these! */
+	if (!tty)
+		return;
+
+	un = tty->driver_data;
+	if (!un)
+		return;
+
+	ts = &tty->termios;
+
+	ch = un->un_ch;
+	if (!ch)
+		return;
+
+	drp_param(ch);
+
+	/* the CLOCAL flag has just been set */
+	if (!(old->c_cflag & CLOCAL) && C_CLOCAL(tty))
+		wake_up_interruptible(&un->un_open_wait);
+}
+
+
+/*
+ *	Throttle receiving data.  We just set a bit and stop reading
+ *	data out of the channel buffer.  It will back up and the
+ *	FEP will do whatever is necessary to stop the far end.
+ */
+static void dgrp_tty_throttle(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_flag |= CH_RXSTOP;
+}
+
+
+static void dgrp_tty_unthrottle(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_flag &= ~CH_RXSTOP;
+}
+
+/*
+ *	Stop the transmitter
+ */
+static void dgrp_tty_stop(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_send |= RR_TX_STOP;
+	ch->ch_send &= ~RR_TX_START;
+
+	/* make the change NOW! */
+	(ch->ch_nd)->nd_tx_ready = 1;
+	if (waitqueue_active(&(ch->ch_nd)->nd_tx_waitq))
+		wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+}
+
+/*
+ *	Start the transmitter
+ */
+static void dgrp_tty_start(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	/* TODO: don't do anything if the transmitter is not stopped */
+
+	ch->ch_send |= RR_TX_START;
+	ch->ch_send &= ~RR_TX_STOP;
+
+	/* make the change NOW! */
+	(ch->ch_nd)->nd_tx_ready = 1;
+	(ch->ch_nd)->nd_tx_work = 1;
+	if (waitqueue_active(&(ch->ch_nd)->nd_tx_waitq))
+		wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+
+}
+
+/*
+ *	Stop the reciever
+ */
+static void dgrp_tty_input_stop(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_send |= RR_RX_STOP;
+	ch->ch_send &= ~RR_RX_START;
+	(ch->ch_nd)->nd_tx_ready = 1;
+	if (waitqueue_active(&(ch->ch_nd)->nd_tx_waitq))
+		wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+
+}
+
+
+static void dgrp_tty_send_xchar(struct tty_struct *tty, char c)
+{
+	struct un_struct *un;
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	un = tty->driver_data;
+	if (!un)
+		return;
+
+	ch = un->un_ch;
+	if (!ch)
+		return;
+	if (c == STOP_CHAR(tty))
+		ch->ch_send |= RR_RX_STOP;
+	else if (c == START_CHAR(tty))
+		ch->ch_send |= RR_RX_START;
+
+	ch->ch_nd->nd_tx_ready = 1;
+	ch->ch_nd->nd_tx_work = 1;
+
+	return;
+}
+
+
+static void dgrp_tty_input_start(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+
+	if (!tty)
+		return;
+
+	ch = ((struct un_struct *) tty->driver_data)->un_ch;
+	if (!ch)
+		return;
+
+	ch->ch_send |= RR_RX_START;
+	ch->ch_send &= ~RR_RX_STOP;
+	(ch->ch_nd)->nd_tx_ready = 1;
+	(ch->ch_nd)->nd_tx_work = 1;
+	if (waitqueue_active(&(ch->ch_nd)->nd_tx_waitq))
+		wake_up_interruptible(&(ch->ch_nd)->nd_tx_waitq);
+
+}
+
+
+/*
+ *	Hangup the port.  Like a close, but don't wait for output
+ *	to drain.
+ *
+ *	How do we close all the channels that are open?
+ */
+static void dgrp_tty_hangup(struct tty_struct *tty)
+{
+	struct ch_struct *ch;
+	struct nd_struct *nd;
+	struct un_struct *un;
+
+	if (!tty)
+		return;
+
+	un = tty->driver_data;
+	if (!un)
+		return;
+
+	ch = un->un_ch;
+	if (!ch)
+		return;
+
+	nd = ch->ch_nd;
+
+	if (C_HUPCL(tty)) {
+		/* LOWER DTR */
+		ch->ch_mout &= ~DM_DTR;
+		/* Don't do this here */
+		/* ch->ch_flag |= CH_HANGUP; */
+		ch->ch_nd->nd_tx_ready = 1;
+		ch->ch_nd->nd_tx_work  = 1;
+		if (waitqueue_active(&ch->ch_flag_wait))
+			wake_up_interruptible(&ch->ch_flag_wait);
+	}
+
+}
+
+/************************************************************************/
+/*                                                                      */
+/*       TTY Initialization/Cleanup Functions                           */
+/*                                                                      */
+/************************************************************************/
+
+/*
+ *	Uninitialize the TTY portion of the supplied node.  Free all
+ *      memory and resources associated with this node.  Do it in reverse
+ *      allocation order: this might possibly result in less fragmentation
+ *      of memory, though I don't know this for sure.
+ */
+void
+dgrp_tty_uninit(struct nd_struct *nd)
+{
+	char id[3];
+
+	ID_TO_CHAR(nd->nd_ID, id);
+
+	if (nd->nd_ttdriver_flags & SERIAL_TTDRV_REG) {
+		tty_unregister_driver(nd->nd_serial_ttdriver);
+
+		kfree(nd->nd_serial_ttdriver->ttys);
+		nd->nd_serial_ttdriver->ttys = NULL;
+
+		put_tty_driver(nd->nd_serial_ttdriver);
+		nd->nd_ttdriver_flags &= ~SERIAL_TTDRV_REG;
+	}
+
+	if (nd->nd_ttdriver_flags & CALLOUT_TTDRV_REG) {
+		tty_unregister_driver(nd->nd_callout_ttdriver);
+
+		kfree(nd->nd_callout_ttdriver->ttys);
+		nd->nd_callout_ttdriver->ttys = NULL;
+
+		put_tty_driver(nd->nd_callout_ttdriver);
+		nd->nd_ttdriver_flags &= ~CALLOUT_TTDRV_REG;
+	}
+
+	if (nd->nd_ttdriver_flags & XPRINT_TTDRV_REG) {
+		tty_unregister_driver(nd->nd_xprint_ttdriver);
+
+		kfree(nd->nd_xprint_ttdriver->ttys);
+		nd->nd_xprint_ttdriver->ttys = NULL;
+
+		put_tty_driver(nd->nd_xprint_ttdriver);
+		nd->nd_ttdriver_flags &= ~XPRINT_TTDRV_REG;
+	}
+}
+
+
+
+/*
+ *     Initialize the TTY portion of the supplied node.
+ */
+int
+dgrp_tty_init(struct nd_struct *nd)
+{
+	char id[3];
+	int  rc;
+	int  i;
+
+	ID_TO_CHAR(nd->nd_ID, id);
+
+	/*
+	 *  Initialize the TTDRIVER structures.
+	 */
+
+	nd->nd_serial_ttdriver = alloc_tty_driver(CHAN_MAX);
+	sprintf(nd->nd_serial_name,  "tty_dgrp_%s_", id);
+
+	nd->nd_serial_ttdriver->owner = THIS_MODULE;
+	nd->nd_serial_ttdriver->name = nd->nd_serial_name;
+	nd->nd_serial_ttdriver->name_base = 0;
+	nd->nd_serial_ttdriver->major = 0;
+	nd->nd_serial_ttdriver->minor_start = 0;
+	nd->nd_serial_ttdriver->type = TTY_DRIVER_TYPE_SERIAL;
+	nd->nd_serial_ttdriver->subtype = SERIAL_TYPE_NORMAL;
+	nd->nd_serial_ttdriver->init_termios = DefaultTermios;
+	nd->nd_serial_ttdriver->driver_name = "dgrp";
+	nd->nd_serial_ttdriver->flags = (TTY_DRIVER_REAL_RAW |
+					 TTY_DRIVER_DYNAMIC_DEV |
+					 TTY_DRIVER_HARDWARE_BREAK);
+
+	/* The kernel wants space to store pointers to tty_structs. */
+	nd->nd_serial_ttdriver->ttys =
+		kzalloc(CHAN_MAX * sizeof(struct tty_struct *), GFP_KERNEL);
+	if (!nd->nd_serial_ttdriver->ttys)
+		return -ENOMEM;
+
+	tty_set_operations(nd->nd_serial_ttdriver, &dgrp_tty_ops);
+
+	if (!(nd->nd_ttdriver_flags & SERIAL_TTDRV_REG)) {
+		/*
+		 *   Register tty devices
+		 */
+		rc = tty_register_driver(nd->nd_serial_ttdriver);
+		if (rc < 0) {
+			/*
+			 * If errno is EBUSY, this means there are no more
+			 * slots available to have us auto-majored.
+			 * (Which is currently supported up to 256)
+			 *
+			 * We can still request majors above 256,
+			 * we just have to do it manually.
+			 */
+			if (rc == -EBUSY) {
+				int i;
+				int max_majors = 1U << (32 - MINORBITS);
+				for (i = 256; i < max_majors; i++) {
+					nd->nd_serial_ttdriver->major = i;
+					rc = tty_register_driver(nd->nd_serial_ttdriver);
+					if (rc >= 0)
+						break;
+				}
+				/* Really fail now, since we ran out
+				 * of majors to try. */
+				if (i == max_majors)
+					return rc;
+
+			} else {
+				return rc;
+			}
+		}
+		nd->nd_ttdriver_flags |= SERIAL_TTDRV_REG;
+	}
+
+	nd->nd_callout_ttdriver = alloc_tty_driver(CHAN_MAX);
+	sprintf(nd->nd_callout_name, "cu_dgrp_%s_",  id);
+
+	nd->nd_callout_ttdriver->owner = THIS_MODULE;
+	nd->nd_callout_ttdriver->name = nd->nd_callout_name;
+	nd->nd_callout_ttdriver->name_base = 0;
+	nd->nd_callout_ttdriver->major = nd->nd_serial_ttdriver->major;
+	nd->nd_callout_ttdriver->minor_start = 0x40;
+	nd->nd_callout_ttdriver->type = TTY_DRIVER_TYPE_SERIAL;
+	nd->nd_callout_ttdriver->subtype = SERIAL_TYPE_CALLOUT;
+	nd->nd_callout_ttdriver->init_termios = DefaultTermios;
+	nd->nd_callout_ttdriver->driver_name = "dgrp";
+	nd->nd_callout_ttdriver->flags = (TTY_DRIVER_REAL_RAW |
+					  TTY_DRIVER_DYNAMIC_DEV |
+					  TTY_DRIVER_HARDWARE_BREAK);
+
+	/* The kernel wants space to store pointers to tty_structs. */
+	nd->nd_callout_ttdriver->ttys =
+		kzalloc(CHAN_MAX * sizeof(struct tty_struct *), GFP_KERNEL);
+	if (!nd->nd_callout_ttdriver->ttys)
+		return -ENOMEM;
+
+	tty_set_operations(nd->nd_callout_ttdriver, &dgrp_tty_ops);
+
+	if (dgrp_register_cudevices) {
+		if (!(nd->nd_ttdriver_flags & CALLOUT_TTDRV_REG)) {
+			/*
+			 *   Register cu devices
+			 */
+			rc = tty_register_driver(nd->nd_callout_ttdriver);
+			if (rc < 0)
+				return rc;
+			nd->nd_ttdriver_flags |= CALLOUT_TTDRV_REG;
+		}
+	}
+
+
+	nd->nd_xprint_ttdriver = alloc_tty_driver(CHAN_MAX);
+	sprintf(nd->nd_xprint_name,  "pr_dgrp_%s_", id);
+
+	nd->nd_xprint_ttdriver->owner = THIS_MODULE;
+	nd->nd_xprint_ttdriver->name = nd->nd_xprint_name;
+	nd->nd_xprint_ttdriver->name_base = 0;
+	nd->nd_xprint_ttdriver->major = nd->nd_serial_ttdriver->major;
+	nd->nd_xprint_ttdriver->minor_start = 0x80;
+	nd->nd_xprint_ttdriver->type = TTY_DRIVER_TYPE_SERIAL;
+	nd->nd_xprint_ttdriver->subtype = SERIAL_TYPE_XPRINT;
+	nd->nd_xprint_ttdriver->init_termios = DefaultTermios;
+	nd->nd_xprint_ttdriver->driver_name = "dgrp";
+	nd->nd_xprint_ttdriver->flags = (TTY_DRIVER_REAL_RAW |
+					 TTY_DRIVER_DYNAMIC_DEV |
+					 TTY_DRIVER_HARDWARE_BREAK);
+
+	/* The kernel wants space to store pointers to tty_structs. */
+	nd->nd_xprint_ttdriver->ttys =
+		kzalloc(CHAN_MAX * sizeof(struct tty_struct *), GFP_KERNEL);
+	if (!nd->nd_xprint_ttdriver->ttys)
+		return -ENOMEM;
+
+	tty_set_operations(nd->nd_xprint_ttdriver, &dgrp_tty_ops);
+
+	if (dgrp_register_prdevices) {
+		if (!(nd->nd_ttdriver_flags & XPRINT_TTDRV_REG)) {
+			/*
+			 *   Register transparent print devices
+			 */
+			rc = tty_register_driver(nd->nd_xprint_ttdriver);
+			if (rc < 0)
+				return rc;
+			nd->nd_ttdriver_flags |= XPRINT_TTDRV_REG;
+		}
+	}
+
+	for (i = 0; i < CHAN_MAX; i++) {
+		struct ch_struct *ch = nd->nd_chan + i;
+
+		ch->ch_nd = nd;
+		ch->ch_digi = digi_init;
+		ch->ch_edelay = 100;
+		ch->ch_custom_speed = 0;
+		ch->ch_portnum = i;
+		ch->ch_tun.un_ch = ch;
+		ch->ch_pun.un_ch = ch;
+		ch->ch_tun.un_type = SERIAL_TYPE_NORMAL;
+		ch->ch_pun.un_type = SERIAL_TYPE_XPRINT;
+
+		init_waitqueue_head(&(ch->ch_flag_wait));
+		init_waitqueue_head(&(ch->ch_sleep));
+
+		init_waitqueue_head(&(ch->ch_tun.un_open_wait));
+		init_waitqueue_head(&(ch->ch_tun.un_close_wait));
+
+		init_waitqueue_head(&(ch->ch_pun.un_open_wait));
+		init_waitqueue_head(&(ch->ch_pun.un_close_wait));
+		tty_port_init(&ch->port);
+		tty_port_init(&ch->port);
+	}
+	return 0;
+}
diff --git a/drivers/staging/dgrp/digirp.h b/drivers/staging/dgrp/digirp.h
new file mode 100644
index 000000000000..33c1394fade7
--- /dev/null
+++ b/drivers/staging/dgrp/digirp.h
@@ -0,0 +1,129 @@
+/************************************************************************
+ * HP-UX Realport Daemon interface file.
+ *
+ * Copyright (C) 1998, by Digi International.  All Rights Reserved.
+ ************************************************************************/
+
+#ifndef _DIGIDRP_H
+#define _DIGIDRP_H
+
+/************************************************************************
+ * This file contains defines for the ioctl() interface to
+ * the realport driver.   This ioctl() interface is used by the
+ * daemon to set speed setup parameters honored by the driver.
+ ************************************************************************/
+
+struct link_struct {
+	int lk_fast_rate;  /* Fast line rate to be used
+			      when the delay is less-equal
+			      to lk_fast_delay */
+
+	int lk_fast_delay; /* Fast line rate delay in
+			      milliseconds */
+
+	int lk_slow_rate;  /* Slow line rate to be used when
+			      the delay is greater-equal
+			      to lk_slow_delay */
+
+	int lk_slow_delay; /* Slow line rate delay in
+			      milliseconds */
+
+	int lk_header_size; /* Estimated packet header size
+			       when sent across the slowest
+			       link.  */
+};
+
+#define DIGI_GETLINK	_IOW('e', 103, struct link_struct)	/* Get link parameters */
+#define DIGI_SETLINK	_IOW('e', 104, struct link_struct)	/* Set link parameters */
+
+
+/************************************************************************
+ * This module provides application access to special Digi
+ * serial line enhancements which are not standard UNIX(tm) features.
+ ************************************************************************/
+
+struct	digiflow_struct {
+	unsigned char	startc;				/* flow cntl start char	*/
+	unsigned char	stopc;				/* flow cntl stop char	*/
+};
+
+/************************************************************************
+ * Values for digi_flags
+ ************************************************************************/
+#define DIGI_IXON	0x0001		/* Handle IXON in the FEP	*/
+#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_422	0x4000		/* Change parallel port to input */
+#define DIGI_RTS_TOGGLE	0x8000		/* Support RTS Toggle		 */
+
+
+/************************************************************************
+ * Values associated with transparent print
+ ************************************************************************/
+#define DIGI_PLEN	8		/* String length */
+#define	DIGI_TSIZ	10		/* Terminal string len */
+
+
+/************************************************************************
+ * Structure used with ioctl commands for DIGI parameters.
+ ************************************************************************/
+struct digi_struct {
+	unsigned short	digi_flags;		/* Flags (see above)	*/
+	unsigned short	digi_maxcps;		/* Max printer CPS	*/
+	unsigned short	digi_maxchar;		/* Max chars in print queue */
+	unsigned short	digi_bufsize;		/* Buffer size		*/
+	unsigned char	digi_onlen;		/* Length of ON string	*/
+	unsigned char	digi_offlen;		/* Length of OFF string	*/
+	char		digi_onstr[DIGI_PLEN];	/* Printer on string	*/
+	char		digi_offstr[DIGI_PLEN];	/* Printer off string	*/
+	char		digi_term[DIGI_TSIZ];	/* terminal string	*/
+};
+
+/************************************************************************
+ * Ioctl command arguments for DIGI parameters.
+ ************************************************************************/
+/* Read params */
+#define DIGI_GETA	_IOR('e', 94, struct digi_struct)
+
+/* Set params */
+#define DIGI_SETA	_IOW('e', 95, struct digi_struct)
+
+/* Drain & set params	*/
+#define DIGI_SETAW	_IOW('e', 96, struct digi_struct)
+
+/* Drain, flush & set params */
+#define DIGI_SETAF	_IOW('e', 97, struct digi_struct)
+
+/* Get startc/stopc flow control characters */
+#define	DIGI_GETFLOW	_IOR('e', 99, struct digiflow_struct)
+
+/* Set startc/stopc flow control characters */
+#define	DIGI_SETFLOW	_IOW('e', 100, struct digiflow_struct)
+
+/* Get Aux. startc/stopc flow control chars */
+#define	DIGI_GETAFLOW	_IOR('e', 101, struct digiflow_struct)
+
+/* Set Aux. startc/stopc flow control chars */
+#define	DIGI_SETAFLOW	_IOW('e', 102, struct digiflow_struct)
+
+/* Set integer baud rate */
+#define	DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)
+
+/* Get integer baud rate */
+#define	DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)
+
+#define	DIGI_GEDELAY	_IOR('d', 246, int)	/* Get edelay */
+#define	DIGI_SEDELAY	_IOW('d', 247, int)	/* Get edelay */
+
+
+#endif /* _DIGIDRP_H */
diff --git a/drivers/staging/dgrp/drp.h b/drivers/staging/dgrp/drp.h
new file mode 100644
index 000000000000..84a1e7be4899
--- /dev/null
+++ b/drivers/staging/dgrp/drp.h
@@ -0,0 +1,693 @@
+/*
+ *
+ * Copyright 1999 Digi International (www.digi.com)
+ *     Gene Olson  <gene at digi dot com>
+ *     James Puzzo <jamesp at digi dot com>
+ *     Scott Kilau <scottk 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.
+ *
+ */
+
+/************************************************************************
+ * Master include file for Linux Realport Driver.
+ ************************************************************************/
+
+#ifndef __DRP_H
+#define __DRP_H
+
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/semaphore.h>
+#include <linux/tty.h>
+
+
+#include "digirp.h"
+
+/************************************************************************
+ * Tuning parameters.
+ ************************************************************************/
+
+#define CHAN_MAX	64		/* Max # ports per server */
+
+#define SEQ_MAX		128		/* Max # transmit sequences (2^n) */
+#define SEQ_MASK	(SEQ_MAX-1)	/* Sequence buffer modulus mask */
+
+#define TBUF_MAX	4096		/* Size of transmit buffer (2^n) */
+#define RBUF_MAX	4096		/* Size of receive buffer (2^n) */
+
+#define TBUF_MASK	(TBUF_MAX-1)	/* Transmit buffer modulus mask */
+#define RBUF_MASK	(RBUF_MAX-1)	/* Receive buffer modulus mask */
+
+#define TBUF_LOW	1000		/* Transmit low water mark */
+
+#define UIO_BASE	1000		/* Base for write operations */
+#define UIO_MIN		2000		/* Minimum size application buffer */
+#define UIO_MAX		8100		/* Unix I/O buffer size */
+
+#define MON_MAX		65536		/* Monitor buffer size (2^n) */
+#define MON_MASK	(MON_MAX-1)	/* Monitor wrap mask */
+
+#define DPA_MAX		65536		/* DPA buffer size (2^n) */
+#define DPA_MASK	(DPA_MAX-1)	/* DPA wrap mask */
+#define DPA_HIGH_WATER	58000		/* Enforce flow control when
+					 * over this amount
+					 */
+
+#define IDLE_MAX	(20 * HZ)	/* Max TCP link idle time */
+
+#define MAX_DESC_LEN	100		/* Maximum length of stored PS
+					 * description
+					 */
+
+#define WRITEBUFLEN	((4096) + 4)    /* 4 extra for alignment play space */
+
+#define VPDSIZE		512
+
+/************************************************************************
+ * Minor device decoding conventions.
+ ************************************************************************
+ *
+ * For Linux, the net and mon devices are handled via "proc", so we
+ * only have to mux the "tty" devices.  Since every PortServer will
+ * have an individual major number, the PortServer number does not
+ * need to be encoded, and in fact, does not need to exist.
+ *
+ */
+
+/*
+ * Port device decoding conventions:
+ *
+ *	Device 00 - 3f        64 dial-in modem devices. (tty)
+ *	Device 40 - 7f        64 dial-out tty devices.  (cu)
+ *	Device 80 - bf        64 dial-out printer devices.
+ *
+ *  IS_PRINT(dev)		This is a printer device.
+ *
+ *  OPEN_CATEGORY(dev)		Specifies the device category.  No two
+ *				devices of different categories may be open
+ *				at the same time.
+ *
+ * The following require the category returned by OPEN_CATEGORY().
+ *
+ *  OPEN_WAIT_AVAIL(cat)	Waits on open until the device becomes
+ *				available.  Fails if NDELAY specified.
+ *
+ *  OPEN_WAIT_CARRIER(cat)	Waits on open if carrier is not present.
+ *				Succeeds if NDELAY is given.
+ *
+ *  OPEN_FORCES_CARRIER(cat)	Carrier is forced high on open.
+ *
+ */
+
+#define PORT_NUM(dev)			((dev) & 0x3f)
+
+#define OPEN_CATEGORY(dev)		((((dev) & 0x80) & 0x40))
+#define IS_PRINT(dev)			(((dev) & 0xff) >= 0x80)
+
+#define OPEN_WAIT_AVAIL(cat)		(((cat) & 0x40) == 0x000)
+#define OPEN_WAIT_CARRIER(cat)		(((cat) & 0x40) == 0x000)
+#define OPEN_FORCES_CARRIER(cat)	(((cat) & 0x40) != 0x000)
+
+
+/************************************************************************
+ * Modem signal defines for 16450/16550 compatible FEP.
+ * set in ch_mout, ch_mflow, ch_mlast etc
+ ************************************************************************/
+
+/* TODO : Re-verify that these modem signal definitions are correct */
+
+#define DM_DTR		0x01
+#define DM_RTS		0x02
+#define DM_RTS_TOGGLE	0x04
+
+#define DM_OUT1		0x04
+#define DM_OUT2		0x08
+
+#define DM_CTS		0x10
+#define DM_DSR		0x20
+#define DM_RI		0x40
+#define DM_CD		0x80		/* This is the DCD flag */
+
+
+/************************************************************************
+ * Realport Event Flags.
+ ************************************************************************/
+
+#define EV_OPU		0x0001		/* Ouput paused by client */
+#define EV_OPS		0x0002		/* Output paused by XOFF */
+#define EV_OPX		0x0004		/* Output paused by XXOFF */
+#define EV_OPH		0x0008		/* Output paused by MFLOW */
+#define EV_IPU		0x0010		/* Input paused by client */
+#define EV_IPS		0x0020		/* Input paused by hi/low water */
+#define EV_TXB		0x0040		/* Transmit break pending */
+#define EV_TXI		0x0080		/* Transmit immediate pending */
+#define EV_TXF		0x0100		/* Transmit flow control pending */
+#define EV_RXB		0x0200		/* Break received */
+
+
+/************************************************************************
+ * Realport CFLAGS.
+ ************************************************************************/
+
+#define CF_CS5		0x0000		/* 5 bit characters */
+#define CF_CS6		0x0010		/* 6 bit characters */
+#define CF_CS7		0x0020		/* 7 bit characters */
+#define CF_CS8		0x0030		/* 8 bit characters */
+#define CF_CSIZE	0x0030		/* Character size */
+#define CF_CSTOPB	0x0040		/* Two stop bits */
+#define CF_CREAD	0x0080		/* Enable receiver */
+#define CF_PARENB	0x0100		/* Enable parity */
+#define CF_PARODD	0x0200		/* Odd parity */
+#define CF_HUPCL	0x0400		/* Drop DTR on close */
+
+
+/************************************************************************
+ * Realport XFLAGS.
+ ************************************************************************/
+
+#define XF_XPAR		0x0001		/* Enable Mark/Space Parity */
+#define XF_XMODEM	0x0002		/* Enable in-band modem signalling */
+#define XF_XCASE	0x0004		/* Convert special characters */
+#define XF_XEDATA	0x0008		/* Error data in stream */
+#define XF_XTOSS	0x0010		/* Toss IXANY characters */
+#define XF_XIXON	0x0020		/* xxon/xxoff enable */
+
+
+/************************************************************************
+ * Realport IFLAGS.
+ ************************************************************************/
+
+#define IF_IGNBRK	0x0001		/* Ignore input break */
+#define IF_BRKINT	0x0002		/* Break interrupt */
+#define IF_IGNPAR	0x0004		/* Ignore error characters */
+#define IF_PARMRK	0x0008		/* Error chars marked with 0xff */
+#define IF_INPCK	0x0010		/* Input parity checking enabled */
+#define IF_ISTRIP	0x0020		/* Input chars masked with 0x7F */
+#define IF_IXON		0x0400		/* Output software flow control */
+#define IF_IXANY	0x0800		/* Restart output on any char */
+#define	IF_IXOFF	0x1000		/* Input software flow control */
+#define IF_DOSMODE	0x8000		/* 16450-compatible errors */
+
+
+/************************************************************************
+ * Realport OFLAGS.
+ ************************************************************************/
+
+#define OF_OLCUC	0x0002		/* Map lower to upper case */
+#define OF_ONLCR	0x0004		/* Map NL to CR-NL */
+#define OF_OCRNL	0x0008		/* Map CR to NL */
+#define OF_ONOCR	0x0010		/* No CR output at column 0 */
+#define OF_ONLRET	0x0020		/* Assume NL does NL/CR */
+#define OF_TAB3		0x1800		/* Tabs expand to 8 spaces */
+#define OF_TABDLY	0x1800		/* Tab delay */
+
+/************************************************************************
+ * Unit flag definitions for un_flag.
+ ************************************************************************/
+
+/* These are the DIGI unit flags */
+#define UN_EXCL		0x00010000	/* Exclusive open */
+#define UN_STICKY	0x00020000	/* TTY Settings are now sticky */
+#define UN_BUSY		0x00040000	/* Some work this channel */
+#define UN_PWAIT	0x00080000	/* Printer waiting for terminal */
+#define UN_TIME		0x00100000	/* Waiting on time */
+#define UN_EMPTY	0x00200000	/* Waiting output queue empty */
+#define UN_LOW		0x00400000	/* Waiting output low water */
+#define UN_DIGI_MASK	0x00FF0000	/* Waiting output low water */
+
+/*
+ * Definitions for async_struct (and serial_struct) flags field
+ *
+ * these are the ASYNC flags copied from serial.h
+ *
+ */
+#define UN_HUP_NOTIFY	0x0001 /* Notify getty on hangups and
+				* closes on the callout port
+				*/
+#define UN_FOURPORT	0x0002	/* Set OU1, OUT2 per AST Fourport settings */
+#define UN_SAK		0x0004	/* Secure Attention Key (Orange book) */
+#define UN_SPLIT_TERMIOS 0x0008 /* Separate termios for dialin/callout */
+
+#define UN_SPD_MASK	0x0030
+#define UN_SPD_HI	0x0010	/* Use 56000 instead of 38400 bps */
+#define UN_SPD_VHI	0x0020	/* Use 115200 instead of 38400 bps */
+#define UN_SPD_CUST	0x0030	/* Use user-specified divisor */
+
+#define UN_SKIP_TEST	0x0040 /* Skip UART test during autoconfiguration */
+#define UN_AUTO_IRQ	0x0080 /* Do automatic IRQ during autoconfiguration */
+
+#define UN_SESSION_LOCKOUT 0x0100 /* Lock out cua opens based on session */
+#define UN_PGRP_LOCKOUT	   0x0200 /* Lock out cua opens based on pgrp */
+#define UN_CALLOUT_NOHUP   0x0400 /* Don't do hangups for cua device */
+
+#define UN_FLAGS	0x0FFF	/* Possible legal async flags */
+#define UN_USR_MASK	0x0430	/* Legal flags that non-privileged
+				 * users can set or reset
+				 */
+
+#define UN_INITIALIZED		0x80000000 /* Serial port was initialized */
+#define UN_CALLOUT_ACTIVE	0x40000000 /* Call out device is active */
+#define UN_NORMAL_ACTIVE	0x20000000 /* Normal device is active */
+#define UN_BOOT_AUTOCONF	0x10000000 /* Autoconfigure port on bootup */
+#define UN_CLOSING		0x08000000 /* Serial port is closing */
+#define UN_CTS_FLOW		0x04000000 /* Do CTS flow control */
+#define UN_CHECK_CD		0x02000000 /* i.e., CLOCAL */
+#define UN_SHARE_IRQ		0x01000000 /* for multifunction cards */
+
+
+/************************************************************************
+ * Structure for terminal or printer unit.  struct un_struct
+ *
+ * Note that in some places the code assumes the "tty_t" is placed
+ * first in the structure.
+ ************************************************************************/
+
+struct un_struct {
+	struct tty_struct *un_tty;		/* System TTY struct */
+	struct ch_struct *un_ch;		/* Associated channel */
+
+	ushort     un_open_count;		/* Successful open count */
+	int		un_flag;		/* Unit flags */
+	ushort     un_tbusy;		/* Busy transmit count */
+
+	wait_queue_head_t  un_open_wait;
+	wait_queue_head_t  un_close_wait;
+	ushort	un_type;
+	struct device *un_sysfs;
+};
+
+
+/************************************************************************
+ * Channel State Numbers for ch_state.
+ ************************************************************************/
+
+/*
+ * The ordering is important.
+ *
+ *    state <= CS_WAIT_CANCEL implies the channel is definitely closed.
+ *
+ *    state >= CS_WAIT_FAIL  implies the channel is definitely open.
+ *
+ *    state >= CS_READY implies data is allowed on the channel.
+ */
+
+enum dgrp_ch_state_t {
+	CS_IDLE = 0,	    /* Channel is idle */
+	CS_WAIT_OPEN = 1,   /* Waiting for Immediate Open Resp */
+	CS_WAIT_CANCEL = 2, /* Waiting for Per/Incom Cancel Resp */
+	CS_WAIT_FAIL = 3,   /* Waiting for Immed Open Failure */
+	CS_SEND_QUERY = 4,  /* Ready to send Port Query */
+	CS_WAIT_QUERY = 5,  /* Waiting for Port Query Response */
+	CS_READY = 6,	    /* Ready to accept commands and data */
+	CS_SEND_CLOSE =	7,  /* Ready to send Close Request */
+	CS_WAIT_CLOSE =	8   /* Waiting for Close Response */
+};
+
+/************************************************************************
+ * Device flag definitions for ch_flag.
+ ************************************************************************/
+
+/*
+ *  Note that the state of the two carrier based flags is key.	When
+ *  we check for carrier state transitions, we look at the current
+ *  physical state of the DCD line and compare it with PHYS_CD (which
+ *  was the state the last time we checked), and we also determine
+ *  a new virtual state (composite of the physical state, FORCEDCD,
+ *  CLOCAL, etc.) and compare it with VIRT_CD.
+ *
+ *  VIRTUAL transitions high will have the side effect of waking blocked
+ *  opens.
+ *
+ *  PHYSICAL transitions low will cause hangups to occur _IF_ the virtual
+ *  state is also low.	We DON'T want to hangup on a PURE virtual drop.
+ */
+
+#define CH_HANGUP	0x00002		/* Server port ready to close */
+
+#define CH_VIRT_CD	0x00004		/* Carrier was virtually present */
+#define CH_PHYS_CD	0x00008		/* Carrier was physically present */
+
+#define CH_CLOCAL	0x00010		/* CLOCAL set in cflags */
+#define CH_BAUD0	0x00020		/* Baud rate zero hangup */
+
+#define CH_FAST_READ	0x00040		/* Fast reads are enabled */
+#define CH_FAST_WRITE	0x00080		/* Fast writes are enabled */
+
+#define CH_PRON		0x00100		/* Printer on string active */
+#define CH_RX_FLUSH	0x00200		/* Flushing receive data */
+#define CH_LOW		0x00400		/* Thread waiting for LOW water */
+#define CH_EMPTY	0x00800		/* Thread waiting for EMPTY */
+#define CH_DRAIN	0x01000		/* Close is waiting to drain */
+#define CH_INPUT	0x02000		/* Thread waiting for INPUT */
+#define CH_RXSTOP	0x04000		/* Stop output to ldisc */
+#define CH_PARAM	0x08000		/* A parameter was updated */
+#define CH_WAITING_SYNC 0x10000		/* A pending sync was assigned
+					 * to this port.
+					 */
+#define CH_PORT_GONE	0x20000		/* Port has disappeared */
+#define CH_TX_BREAK	0x40000		/* TX Break to be sent,
+					 * but has not yet.
+					 */
+
+/************************************************************************
+ * Types of Open Requests for ch_otype.
+ ************************************************************************/
+
+#define OTYPE_IMMEDIATE	  0		/* Immediate Open */
+#define OTYPE_PERSISTENT  1		/* Persistent Open */
+#define OTYPE_INCOMING	  2		/* Incoming Open */
+
+
+/************************************************************************
+ * Request/Response flags.
+ ************************************************************************/
+
+#define RR_SEQUENCE	0x0001		/* Get server RLAST, TIN */
+#define RR_STATUS	0x0002		/* Get server MINT, EINT */
+#define RR_BUFFER	0x0004		/* Get server RSIZE, TSIZE */
+#define RR_CAPABILITY	0x0008		/* Get server port capabilities */
+
+#define RR_TX_FLUSH	0x0040		/* Flush output buffers */
+#define RR_RX_FLUSH	0x0080		/* Flush input buffers */
+
+#define RR_TX_STOP	0x0100		/* Pause output */
+#define RR_RX_STOP	0x0200		/* Pause input */
+#define RR_TX_START	0x0400		/* Start output */
+#define RR_RX_START	0x0800		/* Start input */
+
+#define RR_TX_BREAK	0x1000		/* Send BREAK */
+#define RR_TX_ICHAR	0x2000		/* Send character immediate */
+
+
+/************************************************************************
+ * Channel information structure.   struct ch_struct
+ ************************************************************************/
+
+struct ch_struct {
+	struct digi_struct ch_digi;		/* Digi variables */
+	int	ch_edelay;		/* Digi edelay */
+
+	struct tty_port port;
+	struct un_struct ch_tun;	/* Terminal unit info */
+	struct un_struct ch_pun;	/* Printer unit info */
+
+	struct nd_struct *ch_nd;	/* Node pointer */
+	u8  *ch_tbuf;		/* Local Transmit Buffer */
+	u8  *ch_rbuf;		/* Local Receive Buffer */
+	ulong	ch_cpstime;		/* Printer CPS time */
+	ulong	ch_waketime;		/* Printer wake time */
+
+	ulong	ch_flag;		/* CH_* flags */
+
+	enum dgrp_ch_state_t ch_state;		/* CS_* Protocol state */
+	ushort	ch_send;		/* Bit vector of RR_* requests */
+	ushort	ch_expect;		/* Bit vector of RR_* responses */
+	ushort	ch_wait_carrier;	/* Thread count waiting for carrier */
+	ushort	ch_wait_count[3];	/* Thread count waiting by otype */
+
+	ushort	ch_portnum;		/* Port number */
+	ushort	ch_open_count;		/* Successful open count */
+	ushort	ch_category;		/* Device category */
+	ushort	ch_open_error;		/* Last open error number */
+	ushort	ch_break_time;		/* Pending break request time */
+	ushort	ch_cpsrem;		/* Printer CPS remainder */
+	ushort	ch_ocook;		/* Realport fastcook oflags */
+	ushort	ch_inwait;		/* Thread count in CLIST input */
+
+	ushort	ch_tin;			/* Local transmit buffer in ptr */
+	ushort	ch_tout;		/* Local transmit buffer out ptr */
+	ushort	ch_s_tin;		/* Realport TIN */
+	ushort	ch_s_tpos;		/* Realport TPOS */
+	ushort	ch_s_tsize;		/* Realport TSIZE */
+	ushort	ch_s_treq;		/* Realport TREQ */
+	ushort	ch_s_elast;		/* Realport ELAST */
+
+	ushort	ch_rin;			/* Local receive buffer in ptr */
+	ushort	ch_rout;		/* Local receive buffer out ptr */
+	ushort	ch_s_rin;		/* Realport RIN */
+	/* David Fries 7-13-2001, ch_s_rin should be renamed ch_s_rout because
+	 * the variable we want to represent is the PortServer's ROUT, which is
+	 * the sequence number for the next byte the PortServer will send us.
+	 * RIN is the sequence number for the next byte the PortServer will
+	 * receive from the uart.  The port server will send data as long as
+	 * ROUT is less than RWIN.  What would happen is the port is opened, it
+	 * receives data, it gives the value of RIN, we set the RWIN to
+	 * RIN+RBUF_MAX-1, it sends us RWIN-ROUT bytes which overflows.	 ROUT
+	 * is set to zero when the port is opened, so we start at zero and
+	 * count up as data is received.
+	 */
+	ushort	ch_s_rwin;		/* Realport RWIN */
+	ushort	ch_s_rsize;		/* Realport RSIZE */
+
+	ushort	ch_tmax;		/* Local TMAX */
+	ushort	ch_ttime;		/* Local TTIME */
+	ushort	ch_rmax;		/* Local RMAX */
+	ushort	ch_rtime;		/* Local RTIME */
+	ushort	ch_rlow;		/* Local RLOW */
+	ushort	ch_rhigh;		/* Local RHIGH */
+
+	ushort	ch_s_tmax;		/* Realport TMAX */
+	ushort	ch_s_ttime;		/* Realport TTIME */
+	ushort	ch_s_rmax;		/* Realport RMAX */
+	ushort	ch_s_rtime;		/* Realport RTIME */
+	ushort	ch_s_rlow;		/* Realport RLOW */
+	ushort	ch_s_rhigh;		/* Realport RHIGH */
+
+	ushort	ch_brate;		/* Local baud rate */
+	ushort	ch_cflag;		/* Local tty cflags */
+	ushort	ch_iflag;		/* Local tty iflags */
+	ushort	ch_oflag;		/* Local tty oflags */
+	ushort	ch_xflag;		/* Local tty xflags */
+
+	ushort	ch_s_brate;		/* Realport BRATE */
+	ushort	ch_s_cflag;		/* Realport CFLAG */
+	ushort	ch_s_iflag;		/* Realport IFLAG */
+	ushort	ch_s_oflag;		/* Realport OFLAG */
+	ushort	ch_s_xflag;		/* Realport XFLAG */
+
+	u8	ch_otype;		/* Open request type */
+	u8	ch_pscan_savechar;	/* Last character read by parity scan */
+	u8	ch_pscan_state;		/* PScan State based on last 2 chars */
+	u8	ch_otype_waiting;	/* Type of open pending in server */
+	u8	ch_flush_seq;		/* Receive flush end sequence */
+	u8	ch_s_mlast;		/* Realport MLAST */
+
+	u8	ch_mout;		/* Local MOUT */
+	u8	ch_mflow;		/* Local MFLOW */
+	u8	ch_mctrl;		/* Local MCTRL */
+	u8	ch_xon;			/* Local XON */
+	u8	ch_xoff;		/* Local XOFF */
+	u8	ch_lnext;		/* Local LNEXT */
+	u8	ch_xxon;		/* Local XXON */
+	u8	ch_xxoff;		/* Local XXOFF */
+
+	u8	ch_s_mout;		/* Realport MOUT */
+	u8	ch_s_mflow;		/* Realport MFLOW */
+	u8	ch_s_mctrl;		/* Realport MCTRL */
+	u8	ch_s_xon;		/* Realport XON */
+	u8	ch_s_xoff;		/* Realport XOFF */
+	u8	ch_s_lnext;		/* Realport LNEXT */
+	u8	ch_s_xxon;		/* Realport XXON */
+	u8	ch_s_xxoff;		/* Realport XXOFF */
+
+	wait_queue_head_t ch_flag_wait;	/* Wait queue for ch_flag changes */
+	wait_queue_head_t ch_sleep;	/* Wait queue for my_sleep() */
+
+	int	ch_custom_speed;	/* Realport custom speed */
+	int	ch_txcount;		/* Running TX count */
+	int	ch_rxcount;		/* Running RX count */
+};
+
+
+/************************************************************************
+ * Node State definitions.
+ ************************************************************************/
+
+enum dgrp_nd_state_t {
+	NS_CLOSED = 0,	   /* Network device is closed */
+	NS_IDLE = 1,	   /* Network connection inactive */
+	NS_SEND_QUERY =	2, /* Send server query */
+	NS_WAIT_QUERY =	3, /* Wait for query response */
+	NS_READY = 4,	   /* Network ready */
+	NS_SEND_ERROR =	5  /* Must send error hangup */
+};
+
+#define ND_STATE_STR(x) \
+	((x) == NS_CLOSED     ? "CLOSED"     : \
+	((x) == NS_IDLE	      ? "IDLE"	     : \
+	((x) == NS_SEND_QUERY ? "SEND_QUERY" : \
+	((x) == NS_WAIT_QUERY ? "WAIT_QUERY" : \
+	((x) == NS_READY      ? "READY"	     : \
+	((x) == NS_SEND_ERROR ? "SEND_ERROR" : "UNKNOWN"))))))
+
+/************************************************************************
+ * Node Flag definitions.
+ ************************************************************************/
+
+#define ND_SELECT	0x0001		/* Multiple net read selects */
+#define ND_DEB_WAIT	0x0002		/* Debug Device waiting */
+
+
+/************************************************************************
+ * Monitoring flag definitions.
+ ************************************************************************/
+
+#define MON_WAIT_DATA	0x0001		/* Waiting for buffer data */
+#define MON_WAIT_SPACE	0x0002		/* Waiting for buffer space */
+
+/************************************************************************
+ * DPA flag definitions.
+ ************************************************************************/
+
+#define DPA_WAIT_DATA	0x0001		/* Waiting for buffer data */
+#define DPA_WAIT_SPACE	0x0002		/* Waiting for buffer space */
+
+
+/************************************************************************
+ * Definitions taken from Realport Dump.
+ ************************************************************************/
+
+#define RPDUMP_MAGIC	"Digi-RealPort-1.0"
+
+#define RPDUMP_MESSAGE	0xE2		/* Descriptive message */
+#define RPDUMP_RESET	0xE7		/* Connection reset */
+#define RPDUMP_CLIENT	0xE8		/* Client data */
+#define RPDUMP_SERVER	0xE9		/* Server data */
+
+
+/************************************************************************
+ * Node request/response definitions.
+ ************************************************************************/
+
+#define NR_ECHO		0x0001		/* Server echo packet */
+#define NR_IDENT	0x0002		/* Server Product ID */
+#define NR_CAPABILITY	0x0004		/* Server Capabilties */
+#define NR_VPD		0x0008		/* Server VPD, if any */
+#define NR_PASSWORD	0x0010		/* Server Password */
+
+/************************************************************************
+ * Registration status of the node's Linux struct tty_driver structures.
+ ************************************************************************/
+#define SERIAL_TTDRV_REG   0x0001     /* nd_serial_ttdriver registered	*/
+#define CALLOUT_TTDRV_REG  0x0002     /* nd_callout_ttdriver registered */
+#define XPRINT_TTDRV_REG   0x0004     /* nd_xprint_ttdriver registered	*/
+
+
+/************************************************************************
+ * Node structure.  There exists one of these for each associated
+ * realport server.
+ ************************************************************************/
+
+struct nd_struct {
+	struct list_head	list;
+	long	      nd_major;		   /* Node's major number	    */
+	long	      nd_ID;		   /* Node's ID code		    */
+
+	char	      nd_serial_name[50];   /* "tty_dgrp_<id>_" + null	    */
+	char	      nd_callout_name[50];  /* "cu_dgrp_<id>_" + null	    */
+	char	      nd_xprint_name[50];   /* "pr_dgrp_<id>_" + null	    */
+
+	char	     password[16];	  /* Password for server, if needed */
+	int	     nd_tty_ref_cnt;	  /* Linux tty reference count	   */
+
+	struct proc_dir_entry *nd_net_de; /* Dir entry for /proc/dgrp/net  */
+	struct proc_dir_entry *nd_mon_de; /* Dir entry for /proc/dgrp/mon  */
+	struct proc_dir_entry *nd_ports_de; /* Dir entry for /proc/dgrp/ports*/
+	struct proc_dir_entry *nd_dpa_de; /* Dir entry for /proc/dgrp/dpa  */
+
+	spinlock_t nd_lock;		  /* General node lock		   */
+
+	struct semaphore nd_net_semaphore; /* Net read/write lock	    */
+	struct semaphore nd_mon_semaphore; /* Monitor buffer lock	    */
+	spinlock_t nd_dpa_lock;		/* DPA buffer lock	     */
+
+	enum dgrp_nd_state_t nd_state;	  /* NS_* network state */
+	int	      nd_chan_count;	   /* # active channels		    */
+	int	      nd_flag;		   /* Node flags		    */
+	int	      nd_send;		   /* Responses to send		    */
+	int	      nd_expect;	   /* Responses we expect	    */
+
+	u8	 *nd_iobuf;	       /* Network R/W Buffer		*/
+	wait_queue_head_t nd_tx_waitq;	  /* Network select wait queue	   */
+
+	u8	 *nd_inputbuf;	       /* Input Buffer			*/
+	u8	 *nd_inputflagbuf;     /* Input Flags Buffer		*/
+
+	int	      nd_tx_deposit;	   /* Accumulated transmit deposits */
+	int	      nd_tx_charge;	   /* Accumulated transmit charges  */
+	int	      nd_tx_credit;	   /* Current TX credit		    */
+	int	      nd_tx_ready;	   /* Ready to transmit		    */
+	int	      nd_tx_work;	   /* TX work waiting		    */
+	ulong	     nd_tx_time;	  /* Last transmit time		   */
+	ulong	     nd_poll_time;	  /* Next scheduled poll time	   */
+
+	int	      nd_delay;		   /* Current TX delay		    */
+	int	      nd_rate;		   /* Current TX rate		    */
+	struct link_struct nd_link;		/* Link speed params.		 */
+
+	int	      nd_seq_in;	   /* TX seq in ptr		    */
+	int	      nd_seq_out;	   /* TX seq out ptr		    */
+	int	      nd_unack;		   /* Unacknowledged byte count	    */
+	int	      nd_remain;	   /* Remaining receive bytes	    */
+	int	      nd_tx_module;	   /* Current TX module #	    */
+	int	      nd_rx_module;	   /* Current RX module #	    */
+	char	     *nd_error;		   /* Protocol error message	    */
+
+	int	      nd_write_count;	   /* drp_write() call count	    */
+	int	      nd_read_count;	   /* drp_read() count		    */
+	int	      nd_send_count;	   /* TCP message sent		    */
+	int	      nd_tx_byte;	   /* Transmit byte count	    */
+	int	      nd_rx_byte;	   /* Receive byte count	    */
+
+	ulong	     nd_mon_lbolt;	 /* Monitor start time		   */
+	int	      nd_mon_flag;	  /* Monitor flags		    */
+	int	      nd_mon_in;	  /* Monitor in pointer		    */
+	int	      nd_mon_out;	  /* Monitor out pointer	    */
+	wait_queue_head_t nd_mon_wqueue;  /* Monitor wait queue (on flags)  */
+	u8	 *nd_mon_buf;	      /* Monitor buffer			*/
+
+	ulong	     nd_dpa_lbolt;	/* DPA start time	      */
+	int	     nd_dpa_flag;	/* DPA flags		      */
+	int	     nd_dpa_in;		/* DPA in pointer	      */
+	int	     nd_dpa_out;	/* DPA out pointer	      */
+	wait_queue_head_t nd_dpa_wqueue; /* DPA wait queue (on flags)  */
+	u8	  *nd_dpa_buf;	/* DPA buffer		      */
+
+	uint	     nd_dpa_debug;
+	uint	     nd_dpa_port;
+
+	wait_queue_head_t nd_seq_wque[SEQ_MAX];	  /* TX thread wait queues */
+	u8	  nd_seq_wait[SEQ_MAX];	  /* Transmit thread wait count */
+
+	ushort	     nd_seq_size[SEQ_MAX];   /* Transmit seq packet size   */
+	ulong	     nd_seq_time[SEQ_MAX];   /* Transmit seq packet time   */
+
+	ushort	     nd_hw_ver;		  /* HW version returned from PS   */
+	ushort	     nd_sw_ver;		  /* SW version returned from PS   */
+	uint	     nd_hw_id;		  /* HW ID returned from PS	   */
+	u8	  nd_ps_desc[MAX_DESC_LEN+1];  /* Description from PS	*/
+	uint	     nd_vpd_len;		/* VPD len, if any */
+	u8	     nd_vpd[VPDSIZE];		/* VPD, if any */
+
+	ulong	     nd_ttdriver_flags;	  /* Registration status	    */
+	struct tty_driver *nd_serial_ttdriver;	/* Linux TTYDRIVER structure */
+	struct tty_driver *nd_callout_ttdriver; /* Linux TTYDRIVER structure */
+	struct tty_driver *nd_xprint_ttdriver;	/* Linux TTYDRIVER structure */
+
+	u8	     *nd_writebuf;		/* Used to cache data read
+						 * from user
+						 */
+	struct ch_struct nd_chan[CHAN_MAX];  /* Channel array		    */
+	struct device *nd_class_dev;	/* Hang our sysfs stuff off of here */
+};
+
+#endif /* __DRP_H */
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index 029725c89e58..413da0d6b9f6 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -3955,12 +3955,7 @@ static void et131x_hwaddr_init(struct et131x_adapter *adapter)
 	 * EEPROM then we need to generate the last octet and set it on the
 	 * device
 	 */
-	if (adapter->rom_addr[0] == 0x00 &&
-	    adapter->rom_addr[1] == 0x00 &&
-	    adapter->rom_addr[2] == 0x00 &&
-	    adapter->rom_addr[3] == 0x00 &&
-	    adapter->rom_addr[4] == 0x00 &&
-	    adapter->rom_addr[5] == 0x00) {
+	if (is_zero_ether_addr(adapter->rom_addr)) {
 		/*
 		 * We need to randomly generate the last octet so we
 		 * decrease our chances of setting the mac address to
@@ -3995,16 +3990,14 @@ static void et131x_hwaddr_init(struct et131x_adapter *adapter)
 static int et131x_pci_init(struct et131x_adapter *adapter,
 						struct pci_dev *pdev)
 {
-	int cap = pci_pcie_cap(pdev);
 	u16 max_payload;
-	u16 ctl;
 	int i, rc;
 
 	rc = et131x_init_eeprom(adapter);
 	if (rc < 0)
 		goto out;
 
-	if (!cap) {
+	if (!pci_is_pcie(pdev)) {
 		dev_err(&pdev->dev, "Missing PCIe capabilities\n");
 		goto err_out;
 	}
@@ -4012,7 +4005,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 	/* Let's set up the PORT LOGIC Register.  First we need to know what
 	 * the max_payload_size is
 	 */
-	if (pci_read_config_word(pdev, cap + PCI_EXP_DEVCAP, &max_payload)) {
+	if (pcie_capability_read_word(pdev, PCI_EXP_DEVCAP, &max_payload)) {
 		dev_err(&pdev->dev,
 		    "Could not read PCI config space for Max Payload Size\n");
 		goto err_out;
@@ -4049,17 +4042,10 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 	}
 
 	/* Change the max read size to 2k */
-	if (pci_read_config_word(pdev, cap + PCI_EXP_DEVCTL, &ctl)) {
+	if (pcie_capability_clear_and_set_word(pdev, PCI_EXP_DEVCTL,
+				PCI_EXP_DEVCTL_READRQ, 0x4 << 12)) {
 		dev_err(&pdev->dev,
-			"Could not read PCI config space for Max read size\n");
-		goto err_out;
-	}
-
-	ctl = (ctl & ~PCI_EXP_DEVCTL_READRQ) | (0x04 << 12);
-
-	if (pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL, ctl)) {
-		dev_err(&pdev->dev,
-		      "Could not write PCI config space for Max read size\n");
+			"Couldn't change PCI config space for Max read size\n");
 		goto err_out;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
index f8b8e71284d9..47cc365c630b 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
@@ -94,27 +94,27 @@ void put_request_value(struct net_device *dev, long lvalue);
 u16 hdr_checksum(struct pseudo_hdr *pHdr);
 
 struct dsp_file_hdr {
-	u32  version_id;	// Version ID of this image format.
-	u32  package_id;	// Package ID of code release.
-	u32  build_date;	// Date/time stamp when file was built.
-	u32  commands_offset;	// Offset to attached commands in Pseudo Hdr format.
-	u32  loader_offset;	// Offset to bootloader code.
-	u32  loader_code_address;	// Start address of bootloader.
-	u32  loader_code_end;	// Where bootloader code ends.
+	u32  version_id;	/* Version ID of this image format. */
+	u32  package_id;	/* Package ID of code release. */
+	u32  build_date;	/* Date/time stamp when file was built. */
+	u32  commands_offset;	/* Offset to attached commands in Pseudo Hdr format. */
+	u32  loader_offset;	/* Offset to bootloader code. */
+	u32  loader_code_address;	/* Start address of bootloader. */
+	u32  loader_code_end;	/* Where bootloader code ends. */
 	u32  loader_code_size;
-	u32  version_data_offset;	// Offset were scrambled version data begins.
-	u32  version_data_size;	// Size, in words, of scrambled version data.
-	u32  nDspImages;	// Number of DSP images in file.
+	u32  version_data_offset;	/* Offset were scrambled version data begins. */
+	u32  version_data_size;	/* Size, in words, of scrambled version data. */
+	u32  nDspImages;	/* Number of DSP images in file. */
 } __attribute__ ((packed));
 
 struct dsp_image_info {
-	u32  coff_date;		// Date/time when DSP Coff image was built.
-	u32  begin_offset;	// Offset in file where image begins.
-	u32  end_offset;	// Offset in file where image begins.
-	u32  run_address;	// On chip Start address of DSP code.
-	u32  image_size;	// Size of image.
-	u32  version;		// Embedded version # of DSP code.
-	unsigned short checksum;	// Dsp File checksum
+	u32  coff_date;		/* Date/time when DSP Coff image was built. */
+	u32  begin_offset;	/* Offset in file where image begins. */
+	u32  end_offset;	/* Offset in file where image begins. */
+	u32  run_address;	/* On chip Start address of DSP code. */
+	u32  image_size;	/* Size of image. */
+	u32  version;		/* Embedded version # of DSP code. */
+	unsigned short checksum;	/* Dsp File checksum */
 	unsigned short pad1;
 } __attribute__ ((packed));
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index 31929ef5332d..809fa4886961 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -73,7 +73,7 @@ static int ft1000_control(struct ft1000_device *ft1000dev, unsigned int pipe,
 	}
 
 	ret = usb_control_msg(ft1000dev->dev, pipe, request, requesttype,
-			      value, index, data, size, LARGE_TIMEOUT);
+			      value, index, data, size, timeout);
 
 	if (ret > 0)
 		ret = 0;
@@ -110,7 +110,7 @@ int ft1000_read_register(struct ft1000_device *ft1000dev, u16* Data,
 			     nRegIndx,
 			     Data,
 			     2,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_GET_TIMEOUT);
 
 	return ret;
 }
@@ -143,7 +143,7 @@ int ft1000_write_register(struct ft1000_device *ft1000dev, u16 value,
 			     nRegIndx,
 			     NULL,
 			     0,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_SET_TIMEOUT);
 
 	return ret;
 }
@@ -178,7 +178,7 @@ int ft1000_read_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer,
 			     indx,
 			     buffer,
 			     cnt,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_GET_TIMEOUT);
 
 	return ret;
 }
@@ -215,7 +215,7 @@ int ft1000_write_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer,
 			     indx,
 			     buffer,
 			     cnt,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_SET_TIMEOUT);
 
 	return ret;
 }
@@ -255,7 +255,7 @@ int ft1000_read_dpram16(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer,
 			     indx,
 			     buffer,
 			     2,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_GET_TIMEOUT);
 
 	return ret;
 }
@@ -294,7 +294,7 @@ int ft1000_write_dpram16(struct ft1000_device *ft1000dev, u16 indx, u16 value, u
 			     indx,
 			     NULL,
 			     0,
-			     LARGE_TIMEOUT);
+			     USB_CTRL_SET_TIMEOUT);
 
 	return ret;
 }
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
index 642bb89942f5..2aa6a1c7fd38 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
@@ -36,8 +36,6 @@ struct app_info_block {
 
 #define FT1000_STATUS_CLOSING  0x01
 
-#define LARGE_TIMEOUT   5000
-
 #define DSPBCMSGID              0x10
 
 /* Electrabuzz specific DPRAM mapping */
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 80bde053fbc2..e26c6a8b2627 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -106,8 +106,8 @@ void gdm_qos_init(void *nic_ptr)
 
 	for (i = 0 ; i < QOS_MAX; i++) {
 		INIT_LIST_HEAD(&qcb->qos_list[i]);
-		qcb->csr[i].QoSBufCount = 0;
-		qcb->csr[i].Enabled = 0;
+		qcb->csr[i].qos_buf_count = 0;
+		qcb->csr[i].enabled = 0;
 	}
 
 	qcb->qos_list_cnt = 0;
@@ -133,8 +133,8 @@ void gdm_qos_release_list(void *nic_ptr)
 	spin_lock_irqsave(&qcb->qos_lock, flags);
 
 	for (i = 0; i < QOS_MAX; i++) {
-		qcb->csr[i].QoSBufCount = 0;
-		qcb->csr[i].Enabled = 0;
+		qcb->csr[i].qos_buf_count = 0;
+		qcb->csr[i].enabled = 0;
 	}
 
 	qcb->qos_list_cnt = 0;
@@ -153,42 +153,42 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 {
 	int i;
 
-	if (csr->ClassifierRuleEnable&IPTYPEOFSERVICE) {
-		if (((Stream[1] & csr->IPToSMask) < csr->IPToSLow) ||
-		((Stream[1] & csr->IPToSMask) > csr->IPToSHigh))
+	if (csr->classifier_rule_en&IPTYPEOFSERVICE) {
+		if (((Stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
+		((Stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
 			return 1;
 	}
 
-	if (csr->ClassifierRuleEnable&PROTOCOL) {
-		if (Stream[9] != csr->Protocol)
+	if (csr->classifier_rule_en&PROTOCOL) {
+		if (Stream[9] != csr->protocol)
 			return 1;
 	}
 
-	if (csr->ClassifierRuleEnable&IPMASKEDSRCADDRESS) {
+	if (csr->classifier_rule_en&IPMASKEDSRCADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[12 + i] & csr->IPSrcAddrMask[i]) !=
-			(csr->IPSrcAddr[i] & csr->IPSrcAddrMask[i]))
+			if ((Stream[12 + i] & csr->ipsrc_addrmask[i]) !=
+			(csr->ipsrc_addr[i] & csr->ipsrc_addrmask[i]))
 				return 1;
 		}
 	}
 
-	if (csr->ClassifierRuleEnable&IPMASKEDDSTADDRESS) {
+	if (csr->classifier_rule_en&IPMASKEDDSTADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[16 + i] & csr->IPDstAddrMask[i]) !=
-			(csr->IPDstAddr[i] & csr->IPDstAddrMask[i]))
+			if ((Stream[16 + i] & csr->ipdst_addrmask[i]) !=
+			(csr->ipdst_addr[i] & csr->ipdst_addrmask[i]))
 				return 1;
 		}
 	}
 
-	if (csr->ClassifierRuleEnable&PROTOCOLSRCPORTRANGE) {
+	if (csr->classifier_rule_en&PROTOCOLSRCPORTRANGE) {
 		i = ((port[0]<<8)&0xff00)+port[1];
-		if ((i < csr->SrcPortLow) || (i > csr->SrcPortHigh))
+		if ((i < csr->srcport_lo) || (i > csr->srcport_hi))
 			return 1;
 	}
 
-	if (csr->ClassifierRuleEnable&PROTOCOLDSTPORTRANGE) {
+	if (csr->classifier_rule_en&PROTOCOLDSTPORTRANGE) {
 		i = ((port[2]<<8)&0xff00)+port[3];
-		if ((i < csr->DstPortLow) || (i > csr->DstPortHigh))
+		if ((i < csr->dstport_lo) || (i > csr->dstport_hi))
 			return 1;
 	}
 
@@ -208,8 +208,8 @@ static u32 get_qos_index(struct nic *nic, u8 *iph, u8 *tcpudph)
 
 	if (IP_Ver == 4) {
 		for (i = 0; i < QOS_MAX; i++) {
-			if (qcb->csr[i].Enabled) {
-				if (qcb->csr[i].ClassifierRuleEnable) {
+			if (qcb->csr[i].enabled) {
+				if (qcb->csr[i].classifier_rule_en) {
 					if (chk_ipv4_rule(&qcb->csr[i], iph,
 					tcpudph) == 0)
 						return i;
@@ -230,14 +230,14 @@ static u32 extract_qos_list(struct nic *nic, struct list_head *head)
 	INIT_LIST_HEAD(head);
 
 	for (i = 0; i < QOS_MAX; i++) {
-		if (qcb->csr[i].Enabled) {
-			if (qcb->csr[i].QoSBufCount < qcb->qos_limit_size) {
+		if (qcb->csr[i].enabled) {
+			if (qcb->csr[i].qos_buf_count < qcb->qos_limit_size) {
 				if (!list_empty(&qcb->qos_list[i])) {
 					entry = list_entry(
 					qcb->qos_list[i].prev,
 					struct qos_entry_s, list);
 					list_move_tail(&entry->list, head);
-					qcb->csr[i].QoSBufCount++;
+					qcb->csr[i].qos_buf_count++;
 
 					if (!list_empty(&qcb->qos_list[i]))
 						wprintk("QoS Index(%d) is piled!!\n", i);
@@ -322,8 +322,8 @@ static u32 get_csr(struct qos_cb_s *qcb, u32 SFID, int mode)
 
 	if (mode) {
 		for (i = 0; i < QOS_MAX; i++) {
-			if (qcb->csr[i].Enabled == 0) {
-				qcb->csr[i].Enabled = 1;
+			if (qcb->csr[i].enabled == 0) {
+				qcb->csr[i].enabled = 1;
 				qcb->qos_list_cnt++;
 				return i;
 			}
@@ -365,7 +365,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 				eprintk("QoS ERROR: No SF\n");
 				return;
 			}
-			qcb->csr[index].QoSBufCount = buf[(i*5)+10];
+			qcb->csr[index].qos_buf_count = buf[(i*5)+10];
 		}
 
 		extract_qos_list(nic, &send_list);
@@ -391,38 +391,38 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		qcb->csr[index].SFID = SFID;
-		qcb->csr[index].ClassifierRuleEnable = ((buf[pos++]<<8)&0xff00);
-		qcb->csr[index].ClassifierRuleEnable += buf[pos++];
-		if (qcb->csr[index].ClassifierRuleEnable == 0)
+		qcb->csr[index].classifier_rule_en = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].classifier_rule_en += buf[pos++];
+		if (qcb->csr[index].classifier_rule_en == 0)
 			qcb->qos_null_idx = index;
-		qcb->csr[index].IPToSMask = buf[pos++];
-		qcb->csr[index].IPToSLow = buf[pos++];
-		qcb->csr[index].IPToSHigh = buf[pos++];
-		qcb->csr[index].Protocol = buf[pos++];
-		qcb->csr[index].IPSrcAddrMask[0] = buf[pos++];
-		qcb->csr[index].IPSrcAddrMask[1] = buf[pos++];
-		qcb->csr[index].IPSrcAddrMask[2] = buf[pos++];
-		qcb->csr[index].IPSrcAddrMask[3] = buf[pos++];
-		qcb->csr[index].IPSrcAddr[0] = buf[pos++];
-		qcb->csr[index].IPSrcAddr[1] = buf[pos++];
-		qcb->csr[index].IPSrcAddr[2] = buf[pos++];
-		qcb->csr[index].IPSrcAddr[3] = buf[pos++];
-		qcb->csr[index].IPDstAddrMask[0] = buf[pos++];
-		qcb->csr[index].IPDstAddrMask[1] = buf[pos++];
-		qcb->csr[index].IPDstAddrMask[2] = buf[pos++];
-		qcb->csr[index].IPDstAddrMask[3] = buf[pos++];
-		qcb->csr[index].IPDstAddr[0] = buf[pos++];
-		qcb->csr[index].IPDstAddr[1] = buf[pos++];
-		qcb->csr[index].IPDstAddr[2] = buf[pos++];
-		qcb->csr[index].IPDstAddr[3] = buf[pos++];
-		qcb->csr[index].SrcPortLow = ((buf[pos++]<<8)&0xff00);
-		qcb->csr[index].SrcPortLow += buf[pos++];
-		qcb->csr[index].SrcPortHigh = ((buf[pos++]<<8)&0xff00);
-		qcb->csr[index].SrcPortHigh += buf[pos++];
-		qcb->csr[index].DstPortLow = ((buf[pos++]<<8)&0xff00);
-		qcb->csr[index].DstPortLow += buf[pos++];
-		qcb->csr[index].DstPortHigh = ((buf[pos++]<<8)&0xff00);
-		qcb->csr[index].DstPortHigh += buf[pos++];
+		qcb->csr[index].ip2s_mask = buf[pos++];
+		qcb->csr[index].ip2s_lo = buf[pos++];
+		qcb->csr[index].ip2s_hi = buf[pos++];
+		qcb->csr[index].protocol = buf[pos++];
+		qcb->csr[index].ipsrc_addrmask[0] = buf[pos++];
+		qcb->csr[index].ipsrc_addrmask[1] = buf[pos++];
+		qcb->csr[index].ipsrc_addrmask[2] = buf[pos++];
+		qcb->csr[index].ipsrc_addrmask[3] = buf[pos++];
+		qcb->csr[index].ipsrc_addr[0] = buf[pos++];
+		qcb->csr[index].ipsrc_addr[1] = buf[pos++];
+		qcb->csr[index].ipsrc_addr[2] = buf[pos++];
+		qcb->csr[index].ipsrc_addr[3] = buf[pos++];
+		qcb->csr[index].ipdst_addrmask[0] = buf[pos++];
+		qcb->csr[index].ipdst_addrmask[1] = buf[pos++];
+		qcb->csr[index].ipdst_addrmask[2] = buf[pos++];
+		qcb->csr[index].ipdst_addrmask[3] = buf[pos++];
+		qcb->csr[index].ipdst_addr[0] = buf[pos++];
+		qcb->csr[index].ipdst_addr[1] = buf[pos++];
+		qcb->csr[index].ipdst_addr[2] = buf[pos++];
+		qcb->csr[index].ipdst_addr[3] = buf[pos++];
+		qcb->csr[index].srcport_lo = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].srcport_lo += buf[pos++];
+		qcb->csr[index].srcport_hi = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].srcport_hi += buf[pos++];
+		qcb->csr[index].dstport_lo = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].dstport_lo += buf[pos++];
+		qcb->csr[index].dstport_hi = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].dstport_hi += buf[pos++];
 
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
@@ -444,7 +444,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		INIT_LIST_HEAD(&free_list);
 
 		spin_lock_irqsave(&qcb->qos_lock, flags);
-		qcb->csr[index].Enabled = 0;
+		qcb->csr[index].enabled = 0;
 		qcb->qos_list_cnt--;
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 
diff --git a/drivers/staging/gdm72xx/gdm_qos.h b/drivers/staging/gdm72xx/gdm_qos.h
index 33f2bd4cee32..8f18119d22a9 100644
--- a/drivers/staging/gdm72xx/gdm_qos.h
+++ b/drivers/staging/gdm72xx/gdm_qos.h
@@ -20,18 +20,18 @@
 
 #define BOOLEAN	u8
 
-#define QOS_MAX						16
-#define IPTYPEOFSERVICE				0x8000
-#define	PROTOCOL				0x4000
-#define	IPMASKEDSRCADDRESS			0x2000
-#define	IPMASKEDDSTADDRESS			0x1000
+#define QOS_MAX				16
+#define IPTYPEOFSERVICE			0x8000
+#define	PROTOCOL			0x4000
+#define	IPMASKEDSRCADDRESS		0x2000
+#define	IPMASKEDDSTADDRESS		0x1000
 #define	PROTOCOLSRCPORTRANGE		0x800
 #define	PROTOCOLDSTPORTRANGE		0x400
-#define	DSTMACADDR					0x200
-#define	SRCMACADDR					0x100
-#define	ETHERTYPE					0x80
+#define	DSTMACADDR			0x200
+#define	SRCMACADDR			0x100
+#define	ETHERTYPE			0x80
 #define	IEEE802_1DUSERPRIORITY		0x40
-#define	IEEE802_1QVLANID			0x10
+#define	IEEE802_1QVLANID		0x10
 
 struct gdm_wimax_csr_s {
 	/*	union{
@@ -51,28 +51,28 @@ struct gdm_wimax_csr_s {
 			Reserved:5;
 		} fields;
 	} */
-	BOOLEAN		Enabled;
-	u32			SFID;
-	u8			QoSBufCount;
-	u16		ClassifierRuleEnable;
-	u8			IPToSLow;
-	u8			IPToSHigh;
-	u8			IPToSMask;
-	u8			Protocol;
-	u8			IPSrcAddr[16];
-	u8			IPSrcAddrMask[16];
-	u8			IPDstAddr[16];
-	u8			IPDstAddrMask[16];
-	u16		SrcPortLow;
-	u16		SrcPortHigh;
-	u16		DstPortLow;
-	u16		DstPortHigh;
+	BOOLEAN		enabled;
+	u32		SFID;
+	u8		qos_buf_count;
+	u16		classifier_rule_en;
+	u8		ip2s_lo;
+	u8		ip2s_hi;
+	u8		ip2s_mask;
+	u8		protocol;
+	u8		ipsrc_addr[16];
+	u8		ipsrc_addrmask[16];
+	u8		ipdst_addr[16];
+	u8		ipdst_addrmask[16];
+	u16		srcport_lo;
+	u16		srcport_hi;
+	u16		dstport_lo;
+	u16		dstport_hi;
 };
 
 struct qos_entry_s {
-	struct list_head list;
-	struct sk_buff *skb;
-	struct net_device *dev;
+	struct list_head	list;
+	struct sk_buff		*skb;
+	struct net_device	*dev;
 
 };
 
@@ -81,7 +81,7 @@ struct qos_cb_s {
 	u32			qos_list_cnt;
 	u32			qos_null_idx;
 	struct gdm_wimax_csr_s	csr[QOS_MAX];
-	spinlock_t	qos_lock;
+	spinlock_t		qos_lock;
 	u32			qos_limit_size;
 };
 
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index 3e43c012ef27..ca38d719a1f8 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -60,27 +60,20 @@ static void hexdump(char *title, u8 *data, int len)
 
 static struct sdio_tx *alloc_tx_struct(struct tx_cxt *tx)
 {
-	struct sdio_tx *t = NULL;
-
-	t = kmalloc(sizeof(*t), GFP_ATOMIC);
-	if (t == NULL)
-		goto out;
+	struct sdio_tx *t = kzalloc(sizeof(*t), GFP_ATOMIC);
 
-	memset(t, 0, sizeof(*t));
+	if (!t)
+		return NULL;
 
 	t->buf = kmalloc(TX_BUF_SIZE, GFP_ATOMIC);
-	if (t->buf == NULL)
-		goto out;
+	if (!t->buf) {
+		kfree(t);
+		return NULL;
+	}
 
 	t->tx_cxt = tx;
 
 	return t;
-out:
-	if (t) {
-		kfree(t->buf);
-		kfree(t);
-	}
-	return NULL;
 }
 
 static void free_tx_struct(struct sdio_tx *t)
@@ -93,20 +86,12 @@ static void free_tx_struct(struct sdio_tx *t)
 
 static struct sdio_rx *alloc_rx_struct(struct rx_cxt *rx)
 {
-	struct sdio_rx *r = NULL;
+	struct sdio_rx *r = kzalloc(sizeof(*r), GFP_ATOMIC);
 
-	r = kmalloc(sizeof(*r), GFP_ATOMIC);
-	if (r == NULL)
-		goto out;
-
-	memset(r, 0, sizeof(*r));
-
-	r->rx_cxt = rx;
+	if (r)
+		r->rx_cxt = rx;
 
 	return r;
-out:
-	kfree(r);
-	return NULL;
 }
 
 static void free_rx_struct(struct sdio_rx *r)
@@ -680,7 +665,7 @@ static int sdio_wimax_probe(struct sdio_func *func,
 	phy_dev->rcv_func = gdm_sdio_receive;
 
 	ret = init_sdio(sdev);
-	if (sdev < 0)
+	if (ret < 0)
 		goto out;
 
 	sdev->func = func;
diff --git a/drivers/staging/gdm72xx/gdm_usb.c b/drivers/staging/gdm72xx/gdm_usb.c
index d48d49c145ee..0c9e8958009b 100644
--- a/drivers/staging/gdm72xx/gdm_usb.c
+++ b/drivers/staging/gdm72xx/gdm_usb.c
@@ -26,11 +26,11 @@
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-#define TX_BUF_SIZE	2048
+#define TX_BUF_SIZE		2048
 #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
-#define RX_BUF_SIZE	(128*1024)	/* For packet aggregation */
+#define RX_BUF_SIZE		(128*1024)	/* For packet aggregation */
 #else
-#define RX_BUF_SIZE	2048
+#define RX_BUF_SIZE		2048
 #endif
 
 #define GDM7205_PADDING		256
@@ -39,7 +39,7 @@ MODULE_DEVICE_TABLE(usb, id_table);
 #define B2H(x)		__be16_to_cpu(x)
 #define DB2H(x)		__be32_to_cpu(x)
 
-#define DOWNLOAD_CONF_VALUE		0x21
+#define DOWNLOAD_CONF_VALUE	0x21
 
 #ifdef CONFIG_WIMAX_GDM72XX_K_MODE
 
@@ -48,7 +48,7 @@ static LIST_HEAD(k_list);
 static DEFINE_SPINLOCK(k_lock);
 static int k_mode_stop;
 
-#define K_WAIT_TIME	(2 * HZ / 100)
+#define K_WAIT_TIME		(2 * HZ / 100)
 
 #endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
 
@@ -73,29 +73,23 @@ static void hexdump(char *title, u8 *data, int len)
 
 static struct usb_tx *alloc_tx_struct(struct tx_cxt *tx)
 {
-	struct usb_tx *t = NULL;
+	struct usb_tx *t = kzalloc(sizeof(*t), GFP_ATOMIC);
 
-	t = kmalloc(sizeof(*t), GFP_ATOMIC);
-	if (t == NULL)
-		goto out;
-
-	memset(t, 0, sizeof(*t));
+	if (!t)
+		return NULL;
 
 	t->urb = usb_alloc_urb(0, GFP_ATOMIC);
 	t->buf = kmalloc(TX_BUF_SIZE, GFP_ATOMIC);
-	if (t->urb == NULL || t->buf == NULL)
-		goto out;
-
-	t->tx_cxt = tx;
-
-	return t;
-out:
-	if (t) {
+	if (!t->urb || !t->buf) {
 		usb_free_urb(t->urb);
 		kfree(t->buf);
 		kfree(t);
+		return NULL;
 	}
-	return NULL;
+
+	t->tx_cxt = tx;
+
+	return t;
 }
 
 static void free_tx_struct(struct usb_tx *t)
@@ -109,28 +103,22 @@ static void free_tx_struct(struct usb_tx *t)
 
 static struct usb_rx *alloc_rx_struct(struct rx_cxt *rx)
 {
-	struct usb_rx *r = NULL;
+	struct usb_rx *r = kzalloc(sizeof(*r), GFP_ATOMIC);
 
-	r = kmalloc(sizeof(*r), GFP_ATOMIC);
-	if (r == NULL)
-		goto out;
-
-	memset(r, 0, sizeof(*r));
+	if (!r)
+		return NULL;
 
 	r->urb = usb_alloc_urb(0, GFP_ATOMIC);
 	r->buf = kmalloc(RX_BUF_SIZE, GFP_ATOMIC);
-	if (r->urb == NULL || r->buf == NULL)
-		goto out;
-
-	r->rx_cxt = rx;
-	return r;
-out:
-	if (r) {
+	if (!r->urb || !r->buf) {
 		usb_free_urb(r->urb);
 		kfree(r->buf);
 		kfree(r);
+		return NULL;
 	}
-	return NULL;
+
+	r->rx_cxt = rx;
+	return r;
 }
 
 static void free_rx_struct(struct usb_rx *r)
@@ -180,8 +168,7 @@ static struct usb_rx *get_rx_struct(struct rx_cxt *rx)
 	}
 
 	r = list_entry(rx->free_list.next, struct usb_rx, list);
-	list_del(&r->list);
-	list_add_tail(&r->list, &rx->used_list);
+	list_move_tail(&r->list, &rx->used_list);
 
 	return r;
 }
@@ -189,8 +176,7 @@ static struct usb_rx *get_rx_struct(struct rx_cxt *rx)
 /* Before this function is called, spin lock should be locked. */
 static void put_rx_struct(struct rx_cxt *rx, struct usb_rx *r)
 {
-	list_del(&r->list);
-	list_add(&r->list, &rx->free_list);
+	list_move(&r->list, &rx->free_list);
 }
 
 static int init_usb(struct usbwm_dev *udev)
diff --git a/drivers/staging/gdm72xx/gdm_usb.h b/drivers/staging/gdm72xx/gdm_usb.h
index ecb891f6a599..f2c54511bb96 100644
--- a/drivers/staging/gdm72xx/gdm_usb.h
+++ b/drivers/staging/gdm72xx/gdm_usb.h
@@ -18,8 +18,8 @@
 #include <linux/usb.h>
 #include <linux/list.h>
 
-#define B_DIFF_DL_DRV		(1<<4)
-#define B_DOWNLOAD			(1 << 5)
+#define B_DIFF_DL_DRV		(1 << 4)
+#define B_DOWNLOAD		(1 << 5)
 #define MAX_NR_SDU_BUF		64
 
 struct usb_tx {
@@ -29,7 +29,7 @@ struct usb_tx {
 #endif
 	struct tx_cxt		*tx_cxt;
 
-	struct urb	*urb;
+	struct urb		*urb;
 	u8			*buf;
 
 	void (*callback)(void *cb_data);
@@ -44,14 +44,14 @@ struct tx_cxt {
 	struct list_head	pending_list;
 #endif
 
-	spinlock_t			lock;
+	spinlock_t		lock;
 };
 
 struct usb_rx {
 	struct list_head	list;
 	struct rx_cxt		*rx_cxt;
 
-	struct urb	*urb;
+	struct urb		*urb;
 	u8			*buf;
 
 	void (*callback)(void *cb_data, void *data, int len);
@@ -61,7 +61,7 @@ struct usb_rx {
 struct rx_cxt {
 	struct list_head	free_list;
 	struct list_head	used_list;
-	spinlock_t			lock;
+	spinlock_t		lock;
 };
 
 struct usbwm_dev {
@@ -76,8 +76,8 @@ struct usbwm_dev {
 	struct list_head	list;
 #endif
 
-	struct tx_cxt	tx;
-	struct rx_cxt	rx;
+	struct tx_cxt		tx;
+	struct rx_cxt		rx;
 
 	int padding;
 };
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index 0716efc1817d..6cb810701a3e 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -258,12 +258,16 @@ static int gdm_wimax_event_init(void)
 	if (!wm_event.ref_cnt) {
 		wm_event.sock = netlink_init(NETLINK_WIMAX,
 						gdm_wimax_event_rcv);
-		if (wm_event.sock)
-			wm_event.ref_cnt++;
-		INIT_LIST_HEAD(&wm_event.evtq);
-		INIT_LIST_HEAD(&wm_event.freeq);
-		INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
-		spin_lock_init(&wm_event.evt_lock);
+		if (wm_event.sock) {
+			INIT_LIST_HEAD(&wm_event.evtq);
+			INIT_LIST_HEAD(&wm_event.freeq);
+			INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
+			spin_lock_init(&wm_event.evt_lock);
+		}
+	}
+
+	if (wm_event.sock) {
+		wm_event.ref_cnt++;
 		return 0;
 	}
 
diff --git a/drivers/staging/gdm72xx/usb_boot.c b/drivers/staging/gdm72xx/usb_boot.c
index e3dbd5a552ca..0787188728aa 100644
--- a/drivers/staging/gdm72xx/usb_boot.c
+++ b/drivers/staging/gdm72xx/usb_boot.c
@@ -18,25 +18,22 @@
 #include <linux/usb.h>
 #include <linux/unistd.h>
 #include <linux/slab.h>
+#include <linux/firmware.h>
 
 #include <asm/byteorder.h>
 #include "gdm_usb.h"
 #include "usb_boot.h"
 
-#define DN_KERNEL_MAGIC_NUMBER		0x10760001
-#define DN_ROOTFS_MAGIC_NUMBER		0x10760002
+#define DN_KERNEL_MAGIC_NUMBER	0x10760001
+#define DN_ROOTFS_MAGIC_NUMBER	0x10760002
 
-#define DOWNLOAD_SIZE	1024
-
-#define DH2B(x)		__cpu_to_be32(x)
-#define DL2H(x)		__le32_to_cpu(x)
-
-#define MIN(a, b)	((a) > (b) ? (b) : (a))
+#define DOWNLOAD_SIZE		1024
 
 #define MAX_IMG_CNT		16
-#define UIMG_PATH		"/lib/firmware/gdm72xx/gdmuimg.bin"
-#define KERN_PATH		"/lib/firmware/gdm72xx/zImage"
-#define FS_PATH			"/lib/firmware/gdm72xx/ramdisk.jffs2"
+#define FW_DIR			"gdm72xx/"
+#define FW_UIMG			"gdmuimg.bin"
+#define FW_KERN			"zImage"
+#define FW_FS			"ramdisk.jffs2"
 
 struct dn_header {
 	u32	magic_num;
@@ -44,23 +41,23 @@ struct dn_header {
 };
 
 struct img_header {
-	u32		magic_code;
-	u32		count;
-	u32		len;
-	u32		offset[MAX_IMG_CNT];
+	u32	magic_code;
+	u32	count;
+	u32	len;
+	u32	offset[MAX_IMG_CNT];
 	char	hostname[32];
 	char	date[32];
 };
 
 struct fw_info {
-	u32		id;
-	u32		len;
-	u32		kernel_len;
-	u32		rootfs_len;
-	u32		kernel_offset;
-	u32		rootfs_offset;
-	u32		fw_ver;
-	u32		mac_ver;
+	u32	id;
+	u32	len;
+	u32	kernel_len;
+	u32	rootfs_len;
+	u32	kernel_offset;
+	u32	rootfs_offset;
+	u32	fw_ver;
+	u32	mac_ver;
 	char	hostname[32];
 	char	userid[16];
 	char	date[32];
@@ -71,7 +68,7 @@ static void array_le32_to_cpu(u32 *arr, int num)
 {
 	int i;
 	for (i = 0; i < num; i++, arr++)
-		*arr = DL2H(*arr);
+		*arr = __le32_to_cpu(*arr);
 }
 
 static u8 *tx_buf;
@@ -107,44 +104,37 @@ static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
 	return 0;
 }
 
-static int download_image(struct usb_device *usbdev, struct file *filp,
-				loff_t *pos, u32 img_len, u32 magic_num)
+static int download_image(struct usb_device *usbdev,
+				const struct firmware *firm,
+				loff_t pos, u32 img_len, u32 magic_num)
 {
 	struct dn_header h;
 	int ret = 0;
 	u32 size;
-	int len, readn;
 
-	size = (img_len + DOWNLOAD_SIZE - 1) & ~(DOWNLOAD_SIZE - 1);
-	h.magic_num = DH2B(magic_num);
-	h.file_size = DH2B(size);
+	size = ALIGN(img_len, DOWNLOAD_SIZE);
+	h.magic_num = __cpu_to_be32(magic_num);
+	h.file_size = __cpu_to_be32(size);
 
 	ret = gdm_wibro_send(usbdev, &h, sizeof(h));
 	if (ret < 0)
-		goto out;
+		return ret;
 
-	readn = 0;
-	while ((len = filp->f_op->read(filp, tx_buf, DOWNLOAD_SIZE, pos))) {
+	while (img_len > 0) {
+		if (img_len > DOWNLOAD_SIZE)
+			size = DOWNLOAD_SIZE;
+		else
+			size = img_len;	/* the last chunk of data */
 
-		if (len < 0) {
-			ret = -1;
-			goto out;
-		}
-		readn += len;
+		memcpy(tx_buf, firm->data + pos, size);
+		ret = gdm_wibro_send(usbdev, tx_buf, size);
 
-		ret = gdm_wibro_send(usbdev, tx_buf, DOWNLOAD_SIZE);
 		if (ret < 0)
-			goto out;
-		if (readn >= img_len)
-			break;
-	}
+			return ret;
 
-	if (readn < img_len) {
-		printk(KERN_ERR "gdmwm: Cannot read to the requested size. "
-			"Read = %d Requested = %d\n", readn, img_len);
-		ret = -EIO;
+		img_len -= size;
+		pos += size;
 	}
-out:
 
 	return ret;
 }
@@ -152,14 +142,19 @@ out:
 int usb_boot(struct usb_device *usbdev, u16 pid)
 {
 	int i, ret = 0;
-	struct file *filp = NULL;
-	struct inode *inode = NULL;
-	static mm_segment_t fs;
 	struct img_header hdr;
 	struct fw_info fw_info;
 	loff_t pos = 0;
-	char *img_name = UIMG_PATH;
-	int len;
+	char *img_name = FW_DIR FW_UIMG;
+	const struct firmware *firm;
+
+	ret = request_firmware(&firm, img_name, &usbdev->dev);
+	if (ret < 0) {
+		printk(KERN_ERR
+		       "requesting firmware %s failed with error %d\n",
+			img_name, ret);
+		return ret;
+	}
 
 	tx_buf = kmalloc(DOWNLOAD_SIZE, GFP_KERNEL);
 	if (tx_buf == NULL) {
@@ -167,29 +162,12 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		return -ENOMEM;
 	}
 
-	fs = get_fs();
-	set_fs(get_ds());
-
-	filp = filp_open(img_name, O_RDONLY | O_LARGEFILE, 0);
-	if (IS_ERR(filp)) {
-		printk(KERN_ERR "Can't find %s.\n", img_name);
-		ret = PTR_ERR(filp);
-		goto restore_fs;
-	}
-
-	inode = filp->f_dentry->d_inode;
-	if (!S_ISREG(inode->i_mode)) {
-		printk(KERN_ERR "Invalid file type: %s\n", img_name);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	len = filp->f_op->read(filp, (u8 *)&hdr, sizeof(hdr), &pos);
-	if (len != sizeof(hdr)) {
+	if (firm->size < sizeof(hdr)) {
 		printk(KERN_ERR "gdmwm: Cannot read the image info.\n");
 		ret = -EIO;
 		goto out;
 	}
+	memcpy(&hdr, firm->data, sizeof(hdr));
 
 	array_le32_to_cpu((u32 *)&hdr, 19);
 #if 0
@@ -217,13 +195,12 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		}
 
 		pos = hdr.offset[i];
-		len = filp->f_op->read(filp, (u8 *)&fw_info, sizeof(fw_info),
-					&pos);
-		if (len != sizeof(fw_info)) {
+		if (firm->size < sizeof(fw_info) + pos) {
 			printk(KERN_ERR "gdmwm: Cannot read the FW info.\n");
 			ret = -EIO;
 			goto out;
 		}
+		memcpy(&fw_info, firm->data + pos, sizeof(fw_info));
 
 		array_le32_to_cpu((u32 *)&fw_info, 8);
 #if 0
@@ -239,14 +216,23 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 			continue;
 
 		pos = hdr.offset[i] + fw_info.kernel_offset;
-		ret = download_image(usbdev, filp, &pos, fw_info.kernel_len,
-				DN_KERNEL_MAGIC_NUMBER);
+		if (firm->size < fw_info.kernel_len + pos) {
+			printk(KERN_ERR "gdmwm: Kernel FW is too small.\n");
+			goto out;
+		}
+
+		ret = download_image(usbdev, firm, pos,
+				fw_info.kernel_len, DN_KERNEL_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
 		printk(KERN_INFO "GCT: Kernel download success.\n");
 
 		pos = hdr.offset[i] + fw_info.rootfs_offset;
-		ret = download_image(usbdev, filp, &pos, fw_info.rootfs_len,
+		if (firm->size < fw_info.rootfs_len + pos) {
+			printk(KERN_ERR "gdmwm: Filesystem FW is too small.\n");
+			goto out;
+		}
+		ret = download_image(usbdev, firm, pos, fw_info.rootfs_len,
 				DN_ROOTFS_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
@@ -260,10 +246,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		ret = -EINVAL;
 	}
 out:
-	filp_close(filp, NULL);
-
-restore_fs:
-	set_fs(fs);
+	release_firmware(firm);
 	kfree(tx_buf);
 	return ret;
 }
@@ -293,38 +276,27 @@ out:
 	return ret;
 }
 
-static int em_download_image(struct usb_device *usbdev, char *path,
+static int em_download_image(struct usb_device *usbdev, const char *img_name,
 				char *type_string)
 {
-	struct file *filp;
-	struct inode *inode;
-	static mm_segment_t fs;
 	char *buf = NULL;
 	loff_t pos = 0;
 	int ret = 0;
-	int len, readn = 0;
+	int len;
+	int img_len;
+	const struct firmware *firm;
 	#if defined(GDM7205_PADDING)
 	const int pad_size = GDM7205_PADDING;
 	#else
 	const int pad_size = 0;
 	#endif
 
-	fs = get_fs();
-	set_fs(get_ds());
-
-	filp = filp_open(path, O_RDONLY | O_LARGEFILE, 0);
-	if (IS_ERR(filp)) {
-		printk(KERN_ERR "Can't find %s.\n", path);
-		set_fs(fs);
-		ret = -ENOENT;
-		goto restore_fs;
-	}
-
-	inode = filp->f_dentry->d_inode;
-	if (!S_ISREG(inode->i_mode)) {
-		printk(KERN_ERR "Invalid file type: %s\n", path);
-		ret = -EINVAL;
-		goto out;
+	ret = request_firmware(&firm, img_name, &usbdev->dev);
+	if (ret < 0) {
+		printk(KERN_ERR
+		       "requesting firmware %s failed with error %d\n",
+			img_name, ret);
+		return ret;
 	}
 
 	buf = kmalloc(DOWNLOAD_CHUCK + pad_size, GFP_KERNEL);
@@ -338,18 +310,28 @@ static int em_download_image(struct usb_device *usbdev, char *path,
 	if (ret < 0)
 		goto out;
 
-	while ((len = filp->f_op->read(filp, buf+pad_size, DOWNLOAD_CHUCK,
-					&pos))) {
-		if (len < 0) {
-			ret = -1;
-			goto out;
-		}
-		readn += len;
+	img_len = firm->size;
+
+	if (img_len <= 0) {
+		ret = -1;
+		goto out;
+	}
 
+	while (img_len > 0) {
+		if (img_len > DOWNLOAD_CHUCK)
+			len = DOWNLOAD_CHUCK;
+		else
+			len = img_len; /* the last chunk of data */
+
+		memcpy(buf+pad_size, firm->data + pos, len);
 		ret = gdm_wibro_send(usbdev, buf, len+pad_size);
+
 		if (ret < 0)
 			goto out;
 
+		img_len -= DOWNLOAD_CHUCK;
+		pos += DOWNLOAD_CHUCK;
+
 		ret = em_wait_ack(usbdev, ((len+pad_size) % 512 == 0));
 		if (ret < 0)
 			goto out;
@@ -360,11 +342,7 @@ static int em_download_image(struct usb_device *usbdev, char *path,
 		goto out;
 
 out:
-	filp_close(filp, NULL);
-
-restore_fs:
-	set_fs(fs);
-
+	release_firmware(firm);
 	kfree(buf);
 
 	return ret;
@@ -382,18 +360,20 @@ static int em_fw_reset(struct usb_device *usbdev)
 int usb_emergency(struct usb_device *usbdev)
 {
 	int ret;
+	const char *kern_name = FW_DIR FW_KERN;
+	const char *fs_name = FW_DIR FW_FS;
 
-	ret = em_download_image(usbdev, KERN_PATH, KERNEL_TYPE_STRING);
+	ret = em_download_image(usbdev, kern_name, KERNEL_TYPE_STRING);
 	if (ret < 0)
-		goto out;
+		return ret;
 	printk(KERN_INFO "GCT Emergency: Kernel download success.\n");
 
-	ret = em_download_image(usbdev, FS_PATH, FS_TYPE_STRING);
+	ret = em_download_image(usbdev, fs_name, FS_TYPE_STRING);
 	if (ret < 0)
-		goto out;
+		return ret;
 	printk(KERN_INFO "GCT Emergency: Filesystem download success.\n");
 
 	ret = em_fw_reset(usbdev);
-out:
+
 	return ret;
 }
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
index 827e92de8e30..40d0ecac047f 100644
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ b/drivers/staging/iio/Documentation/generic_buffer.c
@@ -104,6 +104,16 @@ void process_scan(char *data,
 			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 *)
diff --git a/drivers/staging/iio/Documentation/inkernel.txt b/drivers/staging/iio/Documentation/inkernel.txt
index a05823e955d2..ab528409bba6 100644
--- a/drivers/staging/iio/Documentation/inkernel.txt
+++ b/drivers/staging/iio/Documentation/inkernel.txt
@@ -48,11 +48,11 @@ There are then a number of functions that can be used to get information
 about this channel such as it's current reading.
 
 e.g.
-iio_st_read_channel_raw() - get a reading
-iio_st_read_channel_type() - get the type of channel
+iio_read_channel_raw() - get a reading
+iio_get_channel_type() - get the type of channel
 
 There is also provision for retrieving all of the channels associated
 with a given consumer.  This is useful for generic drivers such as
 iio_hwmon where the number and naming of channels is not known by the
-consumer driver.  To do this, use iio_st_channel_get_all.
+consumer driver.  To do this, use iio_channel_get_all.
 
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index 04cd6ec1f70f..ca56c75a35fc 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -1,5 +1,5 @@
 #
-# Industrial I/O subsytem configuration
+# Industrial I/O subsystem configuration
 #
 menu "IIO staging drivers"
 	depends on IIO
diff --git a/drivers/staging/iio/TODO b/drivers/staging/iio/TODO
index cf3f9489b9da..04c23262f8e2 100644
--- a/drivers/staging/iio/TODO
+++ b/drivers/staging/iio/TODO
@@ -69,5 +69,5 @@ Documentation
 1) Lots of cleanup and expansion.
 2) Some device require individual docs.
 
-Contact: Jonathan Cameron <jic23@cam.ac.uk>.
+Contact: Jonathan Cameron <jic23@kernel.org>.
 Mailing list: linux-iio@vger.kernel.org
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 204106b72d24..8e37d6e04277 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -390,7 +390,7 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_chan_spec adis16201_channels[] = {
+static const struct iio_chan_spec adis16201_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -565,7 +565,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16201_remove(struct spi_device *spi)
+static int __devexit adis16201_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
diff --git a/drivers/staging/iio/accel/adis16201_ring.c b/drivers/staging/iio/accel/adis16201_ring.c
index 03fcf6e319db..97c09f0c26ae 100644
--- a/drivers/staging/iio/accel/adis16201_ring.c
+++ b/drivers/staging/iio/accel/adis16201_ring.c
@@ -62,7 +62,6 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16201_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 
 	int i = 0;
 	s16 *data;
@@ -83,7 +82,7 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 22085e9dfd16..002fa9dfc375 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -355,7 +355,7 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 	}
 }
 
-static struct iio_chan_spec adis16203_channels[] = {
+static const struct iio_chan_spec adis16203_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -500,7 +500,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16203_remove(struct spi_device *spi)
+static int __devexit adis16203_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
diff --git a/drivers/staging/iio/accel/adis16203_ring.c b/drivers/staging/iio/accel/adis16203_ring.c
index c16b2b7323ac..7507e1a04591 100644
--- a/drivers/staging/iio/accel/adis16203_ring.c
+++ b/drivers/staging/iio/accel/adis16203_ring.c
@@ -61,7 +61,6 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16203_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 
 	int i = 0;
 	s16 *data;
@@ -82,9 +81,7 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring,
-			      (u8 *)data,
-			      pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 5f2e5f11c543..05bdb7c2c8e3 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -397,7 +397,7 @@ static int adis16204_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_chan_spec adis16204_channels[] = {
+static const struct iio_chan_spec adis16204_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1, /* Note was not previously indexed */
@@ -558,7 +558,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16204_remove(struct spi_device *spi)
+static int __devexit adis16204_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
diff --git a/drivers/staging/iio/accel/adis16204_ring.c b/drivers/staging/iio/accel/adis16204_ring.c
index 1d2b31cc849e..4c976bec986b 100644
--- a/drivers/staging/iio/accel/adis16204_ring.c
+++ b/drivers/staging/iio/accel/adis16204_ring.c
@@ -59,7 +59,6 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16204_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0;
 	s16 *data;
 
@@ -79,7 +78,7 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index 494570508c36..b7333bfe0b2f 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -390,7 +390,7 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_chan_spec adis16209_channels[] = {
+static const struct iio_chan_spec adis16209_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -573,7 +573,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16209_remove(struct spi_device *spi)
+static int __devexit adis16209_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
diff --git a/drivers/staging/iio/accel/adis16209_ring.c b/drivers/staging/iio/accel/adis16209_ring.c
index 1a4a55c27c7c..f939e29d6c82 100644
--- a/drivers/staging/iio/accel/adis16209_ring.c
+++ b/drivers/staging/iio/accel/adis16209_ring.c
@@ -59,7 +59,6 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16209_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0;
 	s16 *data;
 
@@ -79,7 +78,7 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 575f1af25d5d..c755089c7117 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -372,8 +372,7 @@ static ssize_t adis16220_accel_bin_read(struct file *filp, struct kobject *kobj,
 					loff_t off,
 					size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(kobj_to_dev(kobj));
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -394,8 +393,7 @@ static ssize_t adis16220_adc1_bin_read(struct file *filp, struct kobject *kobj,
 				char *buf, loff_t off,
 				size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(kobj_to_dev(kobj));
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -416,8 +414,7 @@ static ssize_t adis16220_adc2_bin_read(struct file *filp, struct kobject *kobj,
 				char *buf, loff_t off,
 				size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(kobj_to_dev(kobj));
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -666,7 +663,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16220_remove(struct spi_device *spi)
+static int __devexit adis16220_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index b30b7874ffb0..0fc26a49d681 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -448,7 +448,7 @@ static int adis16240_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static struct iio_chan_spec adis16240_channels[] = {
+static const struct iio_chan_spec adis16240_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -619,7 +619,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16240_remove(struct spi_device *spi)
+static int __devexit adis16240_remove(struct spi_device *spi)
 {
 
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
diff --git a/drivers/staging/iio/accel/adis16240_ring.c b/drivers/staging/iio/accel/adis16240_ring.c
index 360dfed6d4d1..caff8e25e0a2 100644
--- a/drivers/staging/iio/accel/adis16240_ring.c
+++ b/drivers/staging/iio/accel/adis16240_ring.c
@@ -56,7 +56,6 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16240_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 
 	int i = 0;
 	s16 *data;
@@ -77,7 +76,7 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/accel/kxsd9.c b/drivers/staging/iio/accel/kxsd9.c
index 8cf7cd943c90..fdd5fbded660 100644
--- a/drivers/staging/iio/accel/kxsd9.c
+++ b/drivers/staging/iio/accel/kxsd9.c
@@ -2,7 +2,7 @@
  * kxsd9.c	simple support for the Kionix KXSD9 3D
  *		accelerometer.
  *
- * Copyright (c) 2008-2009 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2008-2009 Jonathan Cameron <jic23@kernel.org>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -186,7 +186,7 @@ error_ret:
 		.address = KXSD9_REG_##axis,				\
 	}
 
-static struct iio_chan_spec kxsd9_channels[] = {
+static const struct iio_chan_spec kxsd9_channels[] = {
 	KXSD9_ACCEL_CHAN(X), KXSD9_ACCEL_CHAN(Y), KXSD9_ACCEL_CHAN(Z),
 	{
 		.type = IIO_VOLTAGE,
@@ -286,6 +286,6 @@ static struct spi_driver kxsd9_driver = {
 };
 module_spi_driver(kxsd9_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Kionix KXSD9 SPI driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/accel/lis3l02dq.h b/drivers/staging/iio/accel/lis3l02dq.h
index ae5f225b4bb2..f9bcd41f7188 100644
--- a/drivers/staging/iio/accel/lis3l02dq.h
+++ b/drivers/staging/iio/accel/lis3l02dq.h
@@ -2,7 +2,7 @@
  * LISL02DQ.h -- support STMicroelectronics LISD02DQ
  *               3d 2g Linear Accelerometers via SPI
  *
- * Copyright (c) 2007 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org>
  *
  * Loosely based upon tle62x0.c
  *
@@ -28,7 +28,7 @@
 /* Control Register (1 of 2) */
 #define LIS3L02DQ_REG_CTRL_1_ADDR		0x20
 /* Power ctrl - either bit set corresponds to on*/
-#define LIS3L02DQ_REG_CTRL_1_PD_ON	0xC0
+#define LIS3L02DQ_REG_CTRL_1_PD_ON		0xC0
 
 /* Decimation Factor  */
 #define LIS3L02DQ_DEC_MASK			0x30
@@ -73,14 +73,14 @@
 /* Interrupt related stuff */
 #define LIS3L02DQ_REG_WAKE_UP_CFG_ADDR			0x23
 
-/* Switch from or combination fo conditions to and */
+/* Switch from or combination of conditions to and */
 #define LIS3L02DQ_REG_WAKE_UP_CFG_BOOLEAN_AND		0x80
 
 /* Latch interrupt request,
  * if on ack must be given by reading the ack register */
 #define LIS3L02DQ_REG_WAKE_UP_CFG_LATCH_SRC		0x40
 
-/* Z Interrupt on High (above threshold)*/
+/* Z Interrupt on High (above threshold) */
 #define LIS3L02DQ_REG_WAKE_UP_CFG_INTERRUPT_Z_HIGH	0x20
 /* Z Interrupt on Low */
 #define LIS3L02DQ_REG_WAKE_UP_CFG_INTERRUPT_Z_LOW	0x10
@@ -117,13 +117,13 @@
 #define LIS3L02DQ_REG_STATUS_Y_OVERRUN			0x20
 #define LIS3L02DQ_REG_STATUS_X_OVERRUN			0x10
 /* XYZ new data available - first is all 3 available? */
-#define LIS3L02DQ_REG_STATUS_XYZ_NEW_DATA 0x08
+#define LIS3L02DQ_REG_STATUS_XYZ_NEW_DATA		0x08
 #define LIS3L02DQ_REG_STATUS_Z_NEW_DATA			0x04
 #define LIS3L02DQ_REG_STATUS_Y_NEW_DATA			0x02
 #define LIS3L02DQ_REG_STATUS_X_NEW_DATA			0x01
 
 /* The accelerometer readings - low and high bytes.
-Form of high byte dependent on justification set in ctrl reg */
+ * Form of high byte dependent on justification set in ctrl reg */
 #define LIS3L02DQ_REG_OUT_X_L_ADDR			0x28
 #define LIS3L02DQ_REG_OUT_X_H_ADDR			0x29
 #define LIS3L02DQ_REG_OUT_Y_L_ADDR			0x2A
@@ -150,9 +150,9 @@ Form of high byte dependent on justification set in ctrl reg */
  * struct lis3l02dq_state - device instance specific data
  * @us:			actual spi_device
  * @trig:		data ready trigger registered with iio
+ * @buf_lock:		mutex to protect tx and rx
  * @tx:			transmit buffer
  * @rx:			receive buffer
- * @buf_lock:		mutex to protect tx and rx
  **/
 struct lis3l02dq_state {
 	struct spi_device		*us;
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index 9d263484fb86..21b0469f8bc2 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -2,7 +2,7 @@
  * lis3l02dq.c	support STMicroelectronics LISD02DQ
  *		3d 2g Linear Accelerometers via SPI
  *
- * Copyright (c) 2007 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -392,7 +392,7 @@ static int lis3l02dq_initial_setup(struct iio_dev *indio_dev)
 		dev_err(&st->us->dev, "problem with setup control register 1");
 		goto err_ret;
 	}
-	/* Repeat as sometimes doesn't work first time?*/
+	/* Repeat as sometimes doesn't work first time? */
 	ret = lis3l02dq_spi_write_reg_8(indio_dev,
 					LIS3L02DQ_REG_CTRL_1_ADDR,
 					val);
@@ -538,7 +538,7 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 		.event_mask = LIS3L02DQ_EVENT_MASK,		\
 	 }
 
-static struct iio_chan_spec lis3l02dq_channels[] = {
+static const struct iio_chan_spec lis3l02dq_channels[] = {
 	LIS3L02DQ_CHAN(0, IIO_MOD_X),
 	LIS3L02DQ_CHAN(1, IIO_MOD_Y),
 	LIS3L02DQ_CHAN(2, IIO_MOD_Z),
@@ -686,7 +686,7 @@ static int __devinit lis3l02dq_probe(struct spi_device *spi)
 		goto error_ret;
 	}
 	st = iio_priv(indio_dev);
-	/* this is only used tor removal purposes */
+	/* this is only used for removal purposes */
 	spi_set_drvdata(spi, indio_dev);
 
 	st->us = spi;
@@ -780,21 +780,15 @@ err_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int lis3l02dq_remove(struct spi_device *spi)
+static int __devexit lis3l02dq_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
 
-	ret = lis3l02dq_disable_all_events(indio_dev);
-	if (ret)
-		goto err_ret;
-
-	ret = lis3l02dq_stop_device(indio_dev);
-	if (ret)
-		goto err_ret;
+	lis3l02dq_disable_all_events(indio_dev);
+	lis3l02dq_stop_device(indio_dev);
 
 	if (spi->irq && gpio_is_valid(irq_to_gpio(spi->irq)) > 0)
 		free_irq(st->us->irq, indio_dev);
@@ -804,8 +798,8 @@ static int lis3l02dq_remove(struct spi_device *spi)
 	lis3l02dq_unconfigure_buffer(indio_dev);
 
 	iio_device_free(indio_dev);
-err_ret:
-	return ret;
+
+	return 0;
 }
 
 static struct spi_driver lis3l02dq_driver = {
@@ -818,7 +812,7 @@ static struct spi_driver lis3l02dq_driver = {
 };
 module_spi_driver(lis3l02dq_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("ST LIS3L02DQ Accelerometer SPI driver");
 MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("spi:lis3l02dq");
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index f3da59063ed2..fa4190d96247 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -14,7 +14,7 @@
 #include "lis3l02dq.h"
 
 /**
- * combine_8_to_16() utility function to munge to u8s into u16
+ * combine_8_to_16() utility function to munge two u8s into u16
  **/
 static inline u16 combine_8_to_16(u8 lower, u8 upper)
 {
@@ -49,7 +49,7 @@ static const u8 read_all_tx_array[] = {
 
 /**
  * lis3l02dq_read_all() Reads all channels currently selected
- * @st:		device specific state
+ * @indio_dev:	IIO device state
  * @rx_array:	(dma capable) receive array, must be at least
  *		4*number of channels
  **/
@@ -137,7 +137,6 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
-	struct iio_buffer *buffer = indio_dev->buffer;
 	int len = 0;
 	char *data;
 
@@ -155,7 +154,7 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*(s64 *)((u8 *)data + ALIGN(len, sizeof(s64)))
 			= pf->timestamp;
-	buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
@@ -172,22 +171,22 @@ __lis3l02dq_write_data_ready_config(struct iio_dev *indio_dev, bool state)
 	bool currentlyset;
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 
-/* Get the current event mask register */
+	/* Get the current event mask register */
 	ret = lis3l02dq_spi_read_reg_8(indio_dev,
 				       LIS3L02DQ_REG_CTRL_2_ADDR,
 				       &valold);
 	if (ret)
 		goto error_ret;
-/* Find out if data ready is already on */
+	/* Find out if data ready is already on */
 	currentlyset
 		= valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
 
-/* Disable requested */
+	/* Disable requested */
 	if (!state && currentlyset) {
-		/* disable the data ready signal */
+		/* Disable the data ready signal */
 		valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
 
-		/* The double write is to overcome a hardware bug?*/
+		/* The double write is to overcome a hardware bug? */
 		ret = lis3l02dq_spi_write_reg_8(indio_dev,
 						LIS3L02DQ_REG_CTRL_2_ADDR,
 						valold);
@@ -199,10 +198,10 @@ __lis3l02dq_write_data_ready_config(struct iio_dev *indio_dev, bool state)
 		if (ret)
 			goto error_ret;
 		st->trigger_on = false;
-/* Enable requested */
+	/* Enable requested */
 	} else if (state && !currentlyset) {
-		/* if not set, enable requested */
-		/* first disable all events */
+		/* If not set, enable requested
+		 * first disable all events */
 		ret = lis3l02dq_disable_all_events(indio_dev);
 		if (ret < 0)
 			goto error_ret;
@@ -241,7 +240,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
 	if (state == false) {
 		/*
 		 * A possible quirk with the handler is currently worked around
-		 *  by ensuring outstanding read events are cleared.
+		 * by ensuring outstanding read events are cleared.
 		 */
 		ret = lis3l02dq_read_all(indio_dev, NULL);
 	}
@@ -252,7 +251,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
 }
 
 /**
- * lis3l02dq_trig_try_reen() try renabling irq for data rdy trigger
+ * lis3l02dq_trig_try_reen() try reenabling irq for data rdy trigger
  * @trig:	the datardy trigger
  */
 static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
@@ -261,8 +260,8 @@ static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 	int i;
 
-	/* If gpio still high (or high again) */
-	/* In theory possible we will need to do this several times */
+	/* If gpio still high (or high again)
+	 * In theory possible we will need to do this several times */
 	for (i = 0; i < 5; i++)
 		if (gpio_get_value(irq_to_gpio(st->us->irq)))
 			lis3l02dq_read_all(indio_dev, NULL);
diff --git a/drivers/staging/iio/accel/sca3000.h b/drivers/staging/iio/accel/sca3000.h
index 131daac90012..c1016c510dae 100644
--- a/drivers/staging/iio/accel/sca3000.h
+++ b/drivers/staging/iio/accel/sca3000.h
@@ -2,7 +2,7 @@
  * sca3000.c -- support VTI sca3000 series accelerometers
  *              via SPI
  *
- * Copyright (c) 2007 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org>
  *
  * Partly based upon tle62x0.c
  *
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index c218d71abf1f..ffd1697a9db0 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -5,7 +5,7 @@
  * under the terms of the GNU General Public License version 2 as published by
  * the Free Software Foundation.
  *
- * Copyright (c) 2009 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2009 Jonathan Cameron <jic23@kernel.org>
  *
  * See industrialio/accels/sca3000.h for comments.
  */
@@ -450,7 +450,7 @@ static IIO_DEVICE_ATTR(revision, S_IRUGO, sca3000_show_rev, NULL, 0);
 		.event_mask = SCA3000_EVENT_MASK,		\
 	 }
 
-static struct iio_chan_spec sca3000_channels[] = {
+static const struct iio_chan_spec sca3000_channels[] = {
 	SCA3000_CHAN(0, IIO_MOD_X),
 	SCA3000_CHAN(1, IIO_MOD_Y),
 	SCA3000_CHAN(2, IIO_MOD_Z),
@@ -1233,15 +1233,13 @@ error_ret:
 	return ret;
 }
 
-static int sca3000_remove(struct spi_device *spi)
+static int __devexit sca3000_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct sca3000_state *st = iio_priv(indio_dev);
-	int ret;
+
 	/* Must ensure no interrupts can be generated after this!*/
-	ret = sca3000_stop_all_interrupts(st);
-	if (ret)
-		return ret;
+	sca3000_stop_all_interrupts(st);
 	if (spi->irq)
 		free_irq(spi->irq, indio_dev);
 	iio_device_unregister(indio_dev);
@@ -1272,6 +1270,6 @@ static struct spi_driver sca3000_driver = {
 };
 module_spi_driver(sca3000_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("VTI SCA3000 Series Accelerometers SPI driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index b7e1a002630a..cbec2f1665e5 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -5,7 +5,7 @@
  * under the terms of the GNU General Public License version 2 as published by
  * the Free Software Foundation.
  *
- * Copyright (c) 2009 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2009 Jonathan Cameron <jic23@kernel.org>
  *
  */
 
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 67711b7d718a..a525143ecbea 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -68,20 +68,6 @@ config AD799X_RING_BUFFER
 	  Say yes here to include ring buffer support in the AD799X
 	  ADC driver.
 
-config AD7476
-	tristate "Analog Devices AD7475/6/7/8 AD7466/7/8 and AD7495 ADC driver"
-	depends on SPI
-	select IIO_BUFFER
-	select IIO_TRIGGERED_BUFFER
-	help
-	  Say yes here to build support for Analog Devices
-	  AD7475, AD7476, AD7477, AD7478, AD7466, AD7467, AD7468, AD7495
-	  SPI analog to digital converters (ADC).
-	  If unsure, say N (but it's safe to say "Y").
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called ad7476.
-
 config AD7887
 	tristate "Analog Devices AD7887 ADC driver"
 	depends on SPI
@@ -96,11 +82,12 @@ config AD7887
 	  module will be called ad7887.
 
 config AD7780
-	tristate "Analog Devices AD7780 AD7781 ADC driver"
+	tristate "Analog Devices AD7780 and similar ADCs driver"
 	depends on SPI
 	depends on GPIOLIB
+	select AD_SIGMA_DELTA
 	help
-	  Say yes here to build support for Analog Devices
+	  Say yes here to build support for Analog Devices AD7170, AD7171,
 	  AD7780 and AD7781 SPI analog to digital converters (ADC).
 	  If unsure, say N (but it's safe to say "Y").
 
@@ -108,13 +95,12 @@ config AD7780
 	  module will be called ad7780.
 
 config AD7793
-	tristate "Analog Devices AD7792 AD7793 ADC driver"
+	tristate "Analog Devices AD7793 and similar ADCs driver"
 	depends on SPI
-	select IIO_BUFFER
-	select IIO_TRIGGERED_BUFFER
+	select AD_SIGMA_DELTA
 	help
-	  Say yes here to build support for Analog Devices
-	  AD7792 and AD7793 SPI analog to digital converters (ADC).
+	  Say yes here to build support for Analog Devices AD7785, AD7792, AD7793,
+	  AD7794 and AD7795 SPI analog to digital converters (ADC).
 	  If unsure, say N (but it's safe to say "Y").
 
 	  To compile this driver as a module, choose M here: the
@@ -131,8 +117,7 @@ config AD7816
 config AD7192
 	tristate "Analog Devices AD7190 AD7192 AD7195 ADC driver"
 	depends on SPI
-	select IIO_BUFFER
-	select IIO_TRIGGERED_BUFFER
+	select AD_SIGMA_DELTA
 	help
 	  Say yes here to build support for Analog Devices AD7190,
 	  AD7192 or AD7195 SPI analog to digital converters (ADC).
@@ -200,6 +185,18 @@ config LPC32XX_ADC
 	  activate only one via device tree selection.  Provides direct access
 	  via sysfs.
 
+config MXS_LRADC
+	tristate "Freescale i.MX28 LRADC"
+	depends on ARCH_MXS
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for i.MX28 LRADC convertor
+	  built into these chips.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mxs-lradc.
+
 config SPEAR_ADC
 	tristate "ST SPEAr ADC"
 	depends on PLAT_SPEAR
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index 14e98b62b70a..62ee02e80cf9 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -17,10 +17,6 @@ ad799x-y := ad799x_core.o
 ad799x-$(CONFIG_AD799X_RING_BUFFER) += ad799x_ring.o
 obj-$(CONFIG_AD799X) += ad799x.o
 
-ad7476-y := ad7476_core.o
-ad7476-$(CONFIG_IIO_BUFFER) += ad7476_ring.o
-obj-$(CONFIG_AD7476) += ad7476.o
-
 ad7887-y := ad7887_core.o
 ad7887-$(CONFIG_IIO_BUFFER) += ad7887_ring.o
 obj-$(CONFIG_AD7887) += ad7887.o
@@ -38,4 +34,5 @@ obj-$(CONFIG_ADT7310) += adt7310.o
 obj-$(CONFIG_ADT7410) += adt7410.o
 obj-$(CONFIG_AD7280) += ad7280a.o
 obj-$(CONFIG_LPC32XX_ADC) += lpc32xx_adc.o
+obj-$(CONFIG_MXS_LRADC) += mxs-lradc.o
 obj-$(CONFIG_SPEAR_ADC) += spear_adc.o
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 19a064d649e3..aeaa61d49f51 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -23,6 +23,7 @@
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
+#include <linux/iio/adc/ad_sigma_delta.h>
 
 #include "ad7192.h"
 
@@ -57,6 +58,7 @@
 
 /* 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_CLKSRC(x)	(((x) & 0x3) << 18) /* Clock Source Select */
 #define AD7192_MODE_SINC3	(1 << 15) /* SINC3 Filter Select */
@@ -91,7 +93,8 @@
 
 #define AD7192_CONF_CHOP	(1 << 23) /* CHOP enable */
 #define AD7192_CONF_REFSEL	(1 << 20) /* REFIN1/REFIN2 Reference Select */
-#define AD7192_CONF_CHAN(x)	(((x) & 0xFF) << 8) /* Channel 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 */
@@ -133,13 +136,7 @@
  */
 
 struct ad7192_state {
-	struct spi_device		*spi;
-	struct iio_trigger		*trig;
 	struct regulator		*reg;
-	struct ad7192_platform_data	*pdata;
-	wait_queue_head_t		wq_data_avail;
-	bool				done;
-	bool				irq_dis;
 	u16				int_vref_mv;
 	u32				mclk;
 	u32				f_order;
@@ -148,178 +145,45 @@ struct ad7192_state {
 	u32				scale_avail[8][2];
 	u8				gpocon;
 	u8				devid;
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	u8				data[4] ____cacheline_aligned;
-};
-
-static int __ad7192_write_reg(struct ad7192_state *st, bool locked,
-			      bool cs_change, unsigned char reg,
-			      unsigned size, unsigned val)
-{
-	u8 *data = st->data;
-	struct spi_transfer t = {
-		.tx_buf		= data,
-		.len		= size + 1,
-		.cs_change	= cs_change,
-	};
-	struct spi_message m;
-
-	data[0] = AD7192_COMM_WRITE | AD7192_COMM_ADDR(reg);
-
-	switch (size) {
-	case 3:
-		data[1] = val >> 16;
-		data[2] = val >> 8;
-		data[3] = val;
-		break;
-	case 2:
-		data[1] = val >> 8;
-		data[2] = val;
-		break;
-	case 1:
-		data[1] = val;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t, &m);
 
-	if (locked)
-		return spi_sync_locked(st->spi, &m);
-	else
-		return spi_sync(st->spi, &m);
-}
+	struct ad_sigma_delta		sd;
+};
 
-static int ad7192_write_reg(struct ad7192_state *st,
-			    unsigned reg, unsigned size, unsigned val)
+static struct ad7192_state *ad_sigma_delta_to_ad7192(struct ad_sigma_delta *sd)
 {
-	return __ad7192_write_reg(st, false, false, reg, size, val);
+	return container_of(sd, struct ad7192_state, sd);
 }
 
-static int __ad7192_read_reg(struct ad7192_state *st, bool locked,
-			     bool cs_change, unsigned char reg,
-			     int *val, unsigned size)
+static int ad7192_set_channel(struct ad_sigma_delta *sd, unsigned int channel)
 {
-	u8 *data = st->data;
-	int ret;
-	struct spi_transfer t[] = {
-		{
-			.tx_buf = data,
-			.len = 1,
-		}, {
-			.rx_buf = data,
-			.len = size,
-			.cs_change = cs_change,
-		},
-	};
-	struct spi_message m;
-
-	data[0] = AD7192_COMM_READ | AD7192_COMM_ADDR(reg);
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t[0], &m);
-	spi_message_add_tail(&t[1], &m);
-
-	if (locked)
-		ret = spi_sync_locked(st->spi, &m);
-	else
-		ret = spi_sync(st->spi, &m);
-
-	if (ret < 0)
-		return ret;
-
-	switch (size) {
-	case 3:
-		*val = data[0] << 16 | data[1] << 8 | data[2];
-		break;
-	case 2:
-		*val = data[0] << 8 | data[1];
-		break;
-	case 1:
-		*val = data[0];
-		break;
-	default:
-		return -EINVAL;
-	}
+	struct ad7192_state *st = ad_sigma_delta_to_ad7192(sd);
 
-	return 0;
-}
+	st->conf &= ~AD7192_CONF_CHAN_MASK;
+	st->conf |= AD7192_CONF_CHAN(channel);
 
-static int ad7192_read_reg(struct ad7192_state *st,
-			   unsigned reg, int *val, unsigned size)
-{
-	return __ad7192_read_reg(st, 0, 0, reg, val, size);
+	return ad_sd_write_reg(&st->sd, AD7192_REG_CONF, 3, st->conf);
 }
 
-static int ad7192_read(struct ad7192_state *st, unsigned ch,
-		       unsigned len, int *val)
+static int ad7192_set_mode(struct ad_sigma_delta *sd,
+			   enum ad_sigma_delta_mode mode)
 {
-	int ret;
-	st->conf = (st->conf & ~AD7192_CONF_CHAN(-1)) |
-		AD7192_CONF_CHAN(1 << ch);
-	st->mode = (st->mode & ~AD7192_MODE_SEL(-1)) |
-		AD7192_MODE_SEL(AD7192_MODE_SINGLE);
+	struct ad7192_state *st = ad_sigma_delta_to_ad7192(sd);
 
-	ad7192_write_reg(st, AD7192_REG_CONF, 3, st->conf);
+	st->mode &= ~AD7192_MODE_SEL_MASK;
+	st->mode |= AD7192_MODE_SEL(mode);
 
-	spi_bus_lock(st->spi->master);
-	st->done = false;
-
-	ret = __ad7192_write_reg(st, 1, 1, AD7192_REG_MODE, 3, st->mode);
-	if (ret < 0)
-		goto out;
-
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-	wait_event_interruptible(st->wq_data_avail, st->done);
-
-	ret = __ad7192_read_reg(st, 1, 0, AD7192_REG_DATA, val, len);
-out:
-	spi_bus_unlock(st->spi->master);
-
-	return ret;
+	return ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode);
 }
 
-static int ad7192_calibrate(struct ad7192_state *st, unsigned mode, unsigned ch)
-{
-	int ret;
-
-	st->conf = (st->conf & ~AD7192_CONF_CHAN(-1)) |
-		AD7192_CONF_CHAN(1 << ch);
-	st->mode = (st->mode & ~AD7192_MODE_SEL(-1)) | AD7192_MODE_SEL(mode);
-
-	ad7192_write_reg(st, AD7192_REG_CONF, 3, st->conf);
-
-	spi_bus_lock(st->spi->master);
-	st->done = false;
-
-	ret = __ad7192_write_reg(st, 1, 1, AD7192_REG_MODE, 3,
-				 (st->devid != ID_AD7195) ?
-				 st->mode | AD7192_MODE_CLKDIV :
-				 st->mode);
-	if (ret < 0)
-		goto out;
-
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-	wait_event_interruptible(st->wq_data_avail, st->done);
-
-	st->mode = (st->mode & ~AD7192_MODE_SEL(-1)) |
-		AD7192_MODE_SEL(AD7192_MODE_IDLE);
-
-	ret = __ad7192_write_reg(st, 1, 0, AD7192_REG_MODE, 3, st->mode);
-out:
-	spi_bus_unlock(st->spi->master);
-
-	return ret;
-}
+static const struct ad_sigma_delta_info ad7192_sigma_delta_info = {
+	.set_channel = ad7192_set_channel,
+	.set_mode = ad7192_set_mode,
+	.has_registers = true,
+	.addr_shift = 3,
+	.read_mask = BIT(6),
+};
 
-static const u8 ad7192_calib_arr[8][2] = {
+static const struct ad_sd_calib_data ad7192_calib_arr[8] = {
 	{AD7192_MODE_CAL_INT_ZERO, AD7192_CH_AIN1},
 	{AD7192_MODE_CAL_INT_FULL, AD7192_CH_AIN1},
 	{AD7192_MODE_CAL_INT_ZERO, AD7192_CH_AIN2},
@@ -332,45 +196,34 @@ static const u8 ad7192_calib_arr[8][2] = {
 
 static int ad7192_calibrate_all(struct ad7192_state *st)
 {
-	int i, ret;
-
-	for (i = 0; i < ARRAY_SIZE(ad7192_calib_arr); i++) {
-		ret = ad7192_calibrate(st, ad7192_calib_arr[i][0],
-				       ad7192_calib_arr[i][1]);
-		if (ret)
-			goto out;
-	}
-
-	return 0;
-out:
-	dev_err(&st->spi->dev, "Calibration failed\n");
-	return ret;
+		return ad_sd_calibrate_all(&st->sd, ad7192_calib_arr,
+				ARRAY_SIZE(ad7192_calib_arr));
 }
 
-static int ad7192_setup(struct ad7192_state *st)
+static int ad7192_setup(struct ad7192_state *st,
+	const struct ad7192_platform_data *pdata)
 {
-	struct iio_dev *indio_dev = spi_get_drvdata(st->spi);
-	struct ad7192_platform_data *pdata = st->pdata;
+	struct iio_dev *indio_dev = spi_get_drvdata(st->sd.spi);
 	unsigned long long scale_uv;
 	int i, ret, id;
 	u8 ones[6];
 
 	/* reset the serial interface */
 	memset(&ones, 0xFF, 6);
-	ret = spi_write(st->spi, &ones, 6);
+	ret = spi_write(st->sd.spi, &ones, 6);
 	if (ret < 0)
 		goto out;
 	msleep(1); /* Wait for at least 500us */
 
 	/* write/read test for device presence */
-	ret = ad7192_read_reg(st, AD7192_REG_ID, &id, 1);
+	ret = ad_sd_read_reg(&st->sd, AD7192_REG_ID, 1, &id);
 	if (ret)
 		goto out;
 
 	id &= AD7192_ID_MASK;
 
 	if (id != st->devid)
-		dev_warn(&st->spi->dev, "device ID query failed (0x%X)\n", id);
+		dev_warn(&st->sd.spi->dev, "device ID query failed (0x%X)\n", id);
 
 	switch (pdata->clock_source_sel) {
 	case AD7192_CLK_EXT_MCLK1_2:
@@ -423,11 +276,11 @@ static int ad7192_setup(struct ad7192_state *st)
 	if (pdata->burnout_curr_en)
 		st->conf |= AD7192_CONF_BURN;
 
-	ret = ad7192_write_reg(st, AD7192_REG_MODE, 3, st->mode);
+	ret = ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode);
 	if (ret)
 		goto out;
 
-	ret = ad7192_write_reg(st, AD7192_REG_CONF, 3, st->conf);
+	ret = ad_sd_write_reg(&st->sd, AD7192_REG_CONF, 3, st->conf);
 	if (ret)
 		goto out;
 
@@ -448,181 +301,10 @@ static int ad7192_setup(struct ad7192_state *st)
 
 	return 0;
 out:
-	dev_err(&st->spi->dev, "setup failed\n");
+	dev_err(&st->sd.spi->dev, "setup failed\n");
 	return ret;
 }
 
-static int ad7192_ring_preenable(struct iio_dev *indio_dev)
-{
-	struct ad7192_state *st = iio_priv(indio_dev);
-	unsigned channel;
-	int ret;
-
-	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
-		return -EINVAL;
-
-	ret = iio_sw_buffer_preenable(indio_dev);
-	if (ret < 0)
-		return ret;
-
-	channel = find_first_bit(indio_dev->active_scan_mask,
-				 indio_dev->masklength);
-
-	st->mode  = (st->mode & ~AD7192_MODE_SEL(-1)) |
-		    AD7192_MODE_SEL(AD7192_MODE_CONT);
-	st->conf  = (st->conf & ~AD7192_CONF_CHAN(-1)) |
-		    AD7192_CONF_CHAN(1 << indio_dev->channels[channel].address);
-
-	ad7192_write_reg(st, AD7192_REG_CONF, 3, st->conf);
-
-	spi_bus_lock(st->spi->master);
-	__ad7192_write_reg(st, 1, 1, AD7192_REG_MODE, 3, st->mode);
-
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-
-	return 0;
-}
-
-static int ad7192_ring_postdisable(struct iio_dev *indio_dev)
-{
-	struct ad7192_state *st = iio_priv(indio_dev);
-
-	st->mode  = (st->mode & ~AD7192_MODE_SEL(-1)) |
-		    AD7192_MODE_SEL(AD7192_MODE_IDLE);
-
-	st->done = false;
-	wait_event_interruptible(st->wq_data_avail, st->done);
-
-	if (!st->irq_dis)
-		disable_irq_nosync(st->spi->irq);
-
-	__ad7192_write_reg(st, 1, 0, AD7192_REG_MODE, 3, st->mode);
-
-	return spi_bus_unlock(st->spi->master);
-}
-
-/**
- * ad7192_trigger_handler() bh of trigger launched polling to ring buffer
- **/
-static irqreturn_t ad7192_trigger_handler(int irq, void *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct iio_buffer *ring = indio_dev->buffer;
-	struct ad7192_state *st = iio_priv(indio_dev);
-	s64 dat64[2];
-	s32 *dat32 = (s32 *)dat64;
-
-	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
-		__ad7192_read_reg(st, 1, 1, AD7192_REG_DATA,
-				  dat32,
-				  indio_dev->channels[0].scan_type.realbits/8);
-
-	/* Guaranteed to be aligned with 8 byte boundary */
-	if (indio_dev->scan_timestamp)
-		dat64[1] = pf->timestamp;
-
-	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
-
-	iio_trigger_notify_done(indio_dev->trig);
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-
-	return IRQ_HANDLED;
-}
-
-static const struct iio_buffer_setup_ops ad7192_ring_setup_ops = {
-	.preenable = &ad7192_ring_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-	.postdisable = &ad7192_ring_postdisable,
-	.validate_scan_mask = &iio_validate_scan_mask_onehot,
-};
-
-static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-			&ad7192_trigger_handler, &ad7192_ring_setup_ops);
-}
-
-static void ad7192_ring_cleanup(struct iio_dev *indio_dev)
-{
-	iio_triggered_buffer_cleanup(indio_dev);
-}
-
-/**
- * ad7192_data_rdy_trig_poll() the event handler for the data rdy trig
- **/
-static irqreturn_t ad7192_data_rdy_trig_poll(int irq, void *private)
-{
-	struct ad7192_state *st = iio_priv(private);
-
-	st->done = true;
-	wake_up_interruptible(&st->wq_data_avail);
-	disable_irq_nosync(irq);
-	st->irq_dis = true;
-	iio_trigger_poll(st->trig, iio_get_time_ns());
-
-	return IRQ_HANDLED;
-}
-
-static struct iio_trigger_ops ad7192_trigger_ops = {
-	.owner = THIS_MODULE,
-};
-
-static int ad7192_probe_trigger(struct iio_dev *indio_dev)
-{
-	struct ad7192_state *st = iio_priv(indio_dev);
-	int ret;
-
-	st->trig = iio_trigger_alloc("%s-dev%d",
-					spi_get_device_id(st->spi)->name,
-					indio_dev->id);
-	if (st->trig == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	st->trig->ops = &ad7192_trigger_ops;
-	ret = request_irq(st->spi->irq,
-			  ad7192_data_rdy_trig_poll,
-			  IRQF_TRIGGER_LOW,
-			  spi_get_device_id(st->spi)->name,
-			  indio_dev);
-	if (ret)
-		goto error_free_trig;
-
-	disable_irq_nosync(st->spi->irq);
-	st->irq_dis = true;
-	st->trig->dev.parent = &st->spi->dev;
-	st->trig->private_data = indio_dev;
-
-	ret = iio_trigger_register(st->trig);
-
-	/* select default trigger */
-	indio_dev->trig = st->trig;
-	if (ret)
-		goto error_free_irq;
-
-	return 0;
-
-error_free_irq:
-	free_irq(st->spi->irq, indio_dev);
-error_free_trig:
-	iio_trigger_free(st->trig);
-error_ret:
-	return ret;
-}
-
-static void ad7192_remove_trigger(struct iio_dev *indio_dev)
-{
-	struct ad7192_state *st = iio_priv(indio_dev);
-
-	iio_trigger_unregister(st->trig);
-	free_irq(st->spi->irq, indio_dev);
-	iio_trigger_free(st->trig);
-}
-
 static ssize_t ad7192_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
@@ -664,7 +346,7 @@ static ssize_t ad7192_write_frequency(struct device *dev,
 
 	st->mode &= ~AD7192_MODE_RATE(-1);
 	st->mode |= AD7192_MODE_RATE(div);
-	ad7192_write_reg(st, AD7192_REG_MODE, 3, st->mode);
+	ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode);
 
 out:
 	mutex_unlock(&indio_dev->mlock);
@@ -676,7 +358,6 @@ static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
 		ad7192_read_frequency,
 		ad7192_write_frequency);
 
-
 static ssize_t ad7192_show_scale_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
@@ -748,7 +429,7 @@ static ssize_t ad7192_set(struct device *dev,
 		else
 			st->gpocon &= ~AD7192_GPOCON_BPDSW;
 
-		ad7192_write_reg(st, AD7192_REG_GPOCON, 1, st->gpocon);
+		ad_sd_write_reg(&st->sd, AD7192_REG_GPOCON, 1, st->gpocon);
 		break;
 	case AD7192_REG_MODE:
 		if (val)
@@ -756,7 +437,7 @@ static ssize_t ad7192_set(struct device *dev,
 		else
 			st->mode &= ~AD7192_MODE_ACX;
 
-		ad7192_write_reg(st, AD7192_REG_MODE, 3, st->mode);
+		ad_sd_write_reg(&st->sd, AD7192_REG_MODE, 3, st->mode);
 		break;
 	default:
 		ret = -EINVAL;
@@ -812,27 +493,11 @@ static int ad7192_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad7192_state *st = iio_priv(indio_dev);
-	int ret, smpl = 0;
 	bool unipolar = !!(st->conf & AD7192_CONF_UNIPOLAR);
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
-		if (iio_buffer_enabled(indio_dev))
-			ret = -EBUSY;
-		else
-			ret = ad7192_read(st, chan->address,
-					chan->scan_type.realbits / 8, &smpl);
-		mutex_unlock(&indio_dev->mlock);
-
-		if (ret < 0)
-			return ret;
-
-		*val = (smpl >> chan->scan_type.shift) &
-			((1 << (chan->scan_type.realbits)) - 1);
-
-		return IIO_VAL_INT;
-
+		return ad_sigma_delta_single_conversion(indio_dev, chan, val);
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
@@ -883,16 +548,16 @@ static int ad7192_write_raw(struct iio_dev *indio_dev,
 		ret = -EINVAL;
 		for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++)
 			if (val2 == st->scale_avail[i][1]) {
+				ret = 0;
 				tmp = st->conf;
 				st->conf &= ~AD7192_CONF_GAIN(-1);
 				st->conf |= AD7192_CONF_GAIN(i);
-
-				if (tmp != st->conf) {
-					ad7192_write_reg(st, AD7192_REG_CONF,
-							 3, st->conf);
-					ad7192_calibrate_all(st);
-				}
-				ret = 0;
+				if (tmp == st->conf)
+					break;
+				ad_sd_write_reg(&st->sd, AD7192_REG_CONF,
+						 3, st->conf);
+				ad7192_calibrate_all(st);
+				break;
 			}
 		break;
 	default:
@@ -904,15 +569,6 @@ static int ad7192_write_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
-static int ad7192_validate_trigger(struct iio_dev *indio_dev,
-				   struct iio_trigger *trig)
-{
-	if (indio_dev->trig != trig)
-		return -EINVAL;
-
-	return 0;
-}
-
 static int ad7192_write_raw_get_fmt(struct iio_dev *indio_dev,
 			       struct iio_chan_spec const *chan,
 			       long mask)
@@ -925,7 +581,7 @@ static const struct iio_info ad7192_info = {
 	.write_raw = &ad7192_write_raw,
 	.write_raw_get_fmt = &ad7192_write_raw_get_fmt,
 	.attrs = &ad7192_attribute_group,
-	.validate_trigger = ad7192_validate_trigger,
+	.validate_trigger = ad_sd_validate_trigger,
 	.driver_module = THIS_MODULE,
 };
 
@@ -934,60 +590,25 @@ static const struct iio_info ad7195_info = {
 	.write_raw = &ad7192_write_raw,
 	.write_raw_get_fmt = &ad7192_write_raw_get_fmt,
 	.attrs = &ad7195_attribute_group,
-	.validate_trigger = ad7192_validate_trigger,
+	.validate_trigger = ad_sd_validate_trigger,
 	.driver_module = THIS_MODULE,
 };
 
-#define AD7192_CHAN_DIFF(_chan, _chan2, _name, _address, _si)		\
-	{ .type = IIO_VOLTAGE,						\
-	  .differential = 1,						\
-	  .indexed = 1,							\
-	  .extend_name = _name,						\
-	  .channel = _chan,						\
-	  .channel2 = _chan2,						\
-	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
-	  IIO_CHAN_INFO_SCALE_SHARED_BIT |				\
-	  IIO_CHAN_INFO_OFFSET_SHARED_BIT,				\
-	  .address = _address,						\
-	  .scan_index = _si,						\
-	  .scan_type =  IIO_ST('u', 24, 32, 0)}
-
-#define AD7192_CHAN(_chan, _address, _si)				\
-	{ .type = IIO_VOLTAGE,						\
-	  .indexed = 1,							\
-	  .channel = _chan,						\
-	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
-	  IIO_CHAN_INFO_SCALE_SHARED_BIT |				\
-	  IIO_CHAN_INFO_OFFSET_SHARED_BIT,				\
-	  .address = _address,						\
-	  .scan_index = _si,						\
-	  .scan_type =  IIO_ST('u', 24, 32, 0)}
-
-#define AD7192_CHAN_TEMP(_chan, _address, _si)				\
-	{ .type = IIO_TEMP,						\
-	  .indexed = 1,							\
-	  .channel = _chan,						\
-	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
-	  IIO_CHAN_INFO_SCALE_SEPARATE_BIT,				\
-	  .address = _address,						\
-	  .scan_index = _si,						\
-	  .scan_type =  IIO_ST('u', 24, 32, 0)}
-
-static struct iio_chan_spec ad7192_channels[] = {
-	AD7192_CHAN_DIFF(1, 2, NULL, AD7192_CH_AIN1P_AIN2M, 0),
-	AD7192_CHAN_DIFF(3, 4, NULL, AD7192_CH_AIN3P_AIN4M, 1),
-	AD7192_CHAN_TEMP(0, AD7192_CH_TEMP, 2),
-	AD7192_CHAN_DIFF(2, 2, "shorted", AD7192_CH_AIN2P_AIN2M, 3),
-	AD7192_CHAN(1, AD7192_CH_AIN1, 4),
-	AD7192_CHAN(2, AD7192_CH_AIN2, 5),
-	AD7192_CHAN(3, AD7192_CH_AIN3, 6),
-	AD7192_CHAN(4, AD7192_CH_AIN4, 7),
+static const struct iio_chan_spec ad7192_channels[] = {
+	AD_SD_DIFF_CHANNEL(0, 1, 2, AD7192_CH_AIN1P_AIN2M, 24, 32, 0),
+	AD_SD_DIFF_CHANNEL(1, 3, 4, AD7192_CH_AIN3P_AIN4M, 24, 32, 0),
+	AD_SD_TEMP_CHANNEL(2, AD7192_CH_TEMP, 24, 32, 0),
+	AD_SD_SHORTED_CHANNEL(3, 2, AD7192_CH_AIN2P_AIN2M, 24, 32, 0),
+	AD_SD_CHANNEL(4, 1, AD7192_CH_AIN1, 24, 32, 0),
+	AD_SD_CHANNEL(5, 2, AD7192_CH_AIN2, 24, 32, 0),
+	AD_SD_CHANNEL(6, 3, AD7192_CH_AIN3, 24, 32, 0),
+	AD_SD_CHANNEL(7, 4, AD7192_CH_AIN4, 24, 32, 0),
 	IIO_CHAN_SOFT_TIMESTAMP(8),
 };
 
 static int __devinit ad7192_probe(struct spi_device *spi)
 {
-	struct ad7192_platform_data *pdata = spi->dev.platform_data;
+	const struct ad7192_platform_data *pdata = spi->dev.platform_data;
 	struct ad7192_state *st;
 	struct iio_dev *indio_dev;
 	int ret , voltage_uv = 0;
@@ -1017,8 +638,6 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 		voltage_uv = regulator_get_voltage(st->reg);
 	}
 
-	st->pdata = pdata;
-
 	if (pdata && pdata->vref_mv)
 		st->int_vref_mv = pdata->vref_mv;
 	else if (voltage_uv)
@@ -1027,7 +646,6 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 		dev_warn(&spi->dev, "reference voltage undefined\n");
 
 	spi_set_drvdata(spi, indio_dev);
-	st->spi = spi;
 	st->devid = spi_get_device_id(spi)->driver_data;
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
@@ -1039,17 +657,13 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 	else
 		indio_dev->info = &ad7192_info;
 
-	init_waitqueue_head(&st->wq_data_avail);
+	ad_sd_init(&st->sd, indio_dev, spi, &ad7192_sigma_delta_info);
 
-	ret = ad7192_register_ring_funcs_and_init(indio_dev);
+	ret = ad_sd_setup_buffer_and_trigger(indio_dev);
 	if (ret)
 		goto error_disable_reg;
 
-	ret = ad7192_probe_trigger(indio_dev);
-	if (ret)
-		goto error_ring_cleanup;
-
-	ret = ad7192_setup(st);
+	ret = ad7192_setup(st, pdata);
 	if (ret)
 		goto error_remove_trigger;
 
@@ -1059,9 +673,7 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 	return 0;
 
 error_remove_trigger:
-	ad7192_remove_trigger(indio_dev);
-error_ring_cleanup:
-	ad7192_ring_cleanup(indio_dev);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
@@ -1074,14 +686,13 @@ error_put_reg:
 	return ret;
 }
 
-static int ad7192_remove(struct spi_device *spi)
+static int __devexit ad7192_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ad7192_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	ad7192_remove_trigger(indio_dev);
-	ad7192_ring_cleanup(indio_dev);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
 
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
diff --git a/drivers/staging/iio/adc/ad7298_core.c b/drivers/staging/iio/adc/ad7298_core.c
index 6141f4a70cfa..4c75114e7d7c 100644
--- a/drivers/staging/iio/adc/ad7298_core.c
+++ b/drivers/staging/iio/adc/ad7298_core.c
@@ -38,7 +38,7 @@
 		},							\
 	}
 
-static struct iio_chan_spec ad7298_channels[] = {
+static const struct iio_chan_spec ad7298_channels[] = {
 	{
 		.type = IIO_TEMP,
 		.indexed = 1,
diff --git a/drivers/staging/iio/adc/ad7298_ring.c b/drivers/staging/iio/adc/ad7298_ring.c
index 506016f01593..c2906a85fedb 100644
--- a/drivers/staging/iio/adc/ad7298_ring.c
+++ b/drivers/staging/iio/adc/ad7298_ring.c
@@ -75,7 +75,6 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct ad7298_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	s64 time_ns = 0;
 	__u16 buf[16];
 	int b_sent, i;
@@ -94,7 +93,7 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 						 indio_dev->masklength); i++)
 		buf[i] = be16_to_cpu(st->rx_buf[i]);
 
-	indio_dev->buffer->access->store_to(ring, (u8 *)buf, time_ns);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)buf);
 
 done:
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/staging/iio/adc/ad7476.h b/drivers/staging/iio/adc/ad7476.h
deleted file mode 100644
index b1dd9317fe1f..000000000000
--- a/drivers/staging/iio/adc/ad7476.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
- * AD7476/5/7/8 (A) SPI ADC driver
- *
- * Copyright 2010 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-#ifndef IIO_ADC_AD7476_H_
-#define IIO_ADC_AD7476_H_
-
-#define RES_MASK(bits)	((1 << (bits)) - 1)
-
-/*
- * TODO: struct ad7476_platform_data needs to go into include/linux/iio
- */
-
-struct ad7476_platform_data {
-	u16				vref_mv;
-};
-
-struct ad7476_chip_info {
-	u16				int_vref_mv;
-	struct iio_chan_spec		channel[2];
-};
-
-struct ad7476_state {
-	struct spi_device		*spi;
-	const struct ad7476_chip_info	*chip_info;
-	struct regulator		*reg;
-	u16				int_vref_mv;
-	struct spi_transfer		xfer;
-	struct spi_message		msg;
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	unsigned char			data[2] ____cacheline_aligned;
-};
-
-enum ad7476_supported_device_ids {
-	ID_AD7466,
-	ID_AD7467,
-	ID_AD7468,
-	ID_AD7475,
-	ID_AD7476,
-	ID_AD7477,
-	ID_AD7478,
-	ID_AD7495
-};
-
-#ifdef CONFIG_IIO_BUFFER
-int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev);
-void ad7476_ring_cleanup(struct iio_dev *indio_dev);
-#else /* CONFIG_IIO_BUFFER */
-
-static inline int
-ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return 0;
-}
-
-static inline void ad7476_ring_cleanup(struct iio_dev *indio_dev)
-{
-}
-#endif /* CONFIG_IIO_BUFFER */
-#endif /* IIO_ADC_AD7476_H_ */
diff --git a/drivers/staging/iio/adc/ad7476_ring.c b/drivers/staging/iio/adc/ad7476_ring.c
deleted file mode 100644
index d087b21c51f6..000000000000
--- a/drivers/staging/iio/adc/ad7476_ring.c
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Copyright 2010-2012 Analog Devices Inc.
- * Copyright (C) 2008 Jonathan Cameron
- *
- * Licensed under the GPL-2 or later.
- *
- * ad7476_ring.c
- */
-
-#include <linux/interrupt.h>
-#include <linux/device.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/spi/spi.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
-
-#include "ad7476.h"
-
-static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct ad7476_state *st = iio_priv(indio_dev);
-	s64 time_ns;
-	__u8 *rxbuf;
-	int b_sent;
-
-	rxbuf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (rxbuf == NULL)
-		goto done;
-
-	b_sent = spi_read(st->spi, rxbuf,
-			  st->chip_info->channel[0].scan_type.storagebits / 8);
-	if (b_sent < 0)
-		goto done;
-
-	time_ns = iio_get_time_ns();
-
-	if (indio_dev->scan_timestamp)
-		memcpy(rxbuf + indio_dev->scan_bytes - sizeof(s64),
-			&time_ns, sizeof(time_ns));
-
-	indio_dev->buffer->access->store_to(indio_dev->buffer, rxbuf, time_ns);
-done:
-	iio_trigger_notify_done(indio_dev->trig);
-	kfree(rxbuf);
-
-	return IRQ_HANDLED;
-}
-
-int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return iio_triggered_buffer_setup(indio_dev, NULL,
-			&ad7476_trigger_handler, NULL);
-}
-
-void ad7476_ring_cleanup(struct iio_dev *indio_dev)
-{
-	iio_triggered_buffer_cleanup(indio_dev);
-}
diff --git a/drivers/staging/iio/adc/ad7606.h b/drivers/staging/iio/adc/ad7606.h
index 10f59896597f..9221a74efd18 100644
--- a/drivers/staging/iio/adc/ad7606.h
+++ b/drivers/staging/iio/adc/ad7606.h
@@ -51,7 +51,7 @@ struct ad7606_platform_data {
 struct ad7606_chip_info {
 	const char			*name;
 	u16				int_vref_mv;
-	struct iio_chan_spec		*channels;
+	const struct iio_chan_spec	*channels;
 	unsigned			num_channels;
 };
 
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index ccb97fecdea7..bae61cbe9212 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -241,7 +241,7 @@ static const struct attribute_group ad7606_attribute_group_range = {
 		.scan_type = IIO_ST('s', 16, 16, 0),		\
 	}
 
-static struct iio_chan_spec ad7606_8_channels[] = {
+static const struct iio_chan_spec ad7606_8_channels[] = {
 	AD7606_CHANNEL(0),
 	AD7606_CHANNEL(1),
 	AD7606_CHANNEL(2),
@@ -253,7 +253,7 @@ static struct iio_chan_spec ad7606_8_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(8),
 };
 
-static struct iio_chan_spec ad7606_6_channels[] = {
+static const struct iio_chan_spec ad7606_6_channels[] = {
 	AD7606_CHANNEL(0),
 	AD7606_CHANNEL(1),
 	AD7606_CHANNEL(2),
@@ -263,7 +263,7 @@ static struct iio_chan_spec ad7606_6_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(6),
 };
 
-static struct iio_chan_spec ad7606_4_channels[] = {
+static const struct iio_chan_spec ad7606_4_channels[] = {
 	AD7606_CHANNEL(0),
 	AD7606_CHANNEL(1),
 	AD7606_CHANNEL(2),
diff --git a/drivers/staging/iio/adc/ad7606_ring.c b/drivers/staging/iio/adc/ad7606_ring.c
index f15afe47c20d..ba04d0ffd4f4 100644
--- a/drivers/staging/iio/adc/ad7606_ring.c
+++ b/drivers/staging/iio/adc/ad7606_ring.c
@@ -46,7 +46,6 @@ static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
 	struct ad7606_state *st = container_of(work_s, struct ad7606_state,
 						poll_work);
 	struct iio_dev *indio_dev = iio_priv_to_dev(st);
-	struct iio_buffer *ring = indio_dev->buffer;
 	s64 time_ns;
 	__u8 *buf;
 	int ret;
@@ -84,7 +83,7 @@ static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(buf + indio_dev->scan_bytes - sizeof(s64))) = time_ns;
 
-	ring->access->store_to(indio_dev->buffer, buf, time_ns);
+	iio_push_to_buffer(indio_dev->buffer, buf);
 done:
 	gpio_set_value(st->pdata->gpio_convst, 0);
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index 19ee49c95de4..0a1328b8657f 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -1,5 +1,5 @@
 /*
- * AD7780/AD7781 SPI ADC driver
+ * AD7170/AD7171 and AD7780/AD7781 SPI ADC driver
  *
  * Copyright 2011 Analog Devices Inc.
  *
@@ -20,6 +20,7 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/adc/ad_sigma_delta.h>
 
 #include "ad7780.h"
 
@@ -33,53 +34,53 @@
 #define AD7780_PAT0	(1 << 0)
 
 struct ad7780_chip_info {
-	struct iio_chan_spec		channel;
+	struct iio_chan_spec	channel;
+	unsigned int		pattern_mask;
+	unsigned int		pattern;
 };
 
 struct ad7780_state {
-	struct spi_device		*spi;
 	const struct ad7780_chip_info	*chip_info;
 	struct regulator		*reg;
-	struct ad7780_platform_data	*pdata;
-	wait_queue_head_t		wq_data_avail;
-	bool				done;
+	int				powerdown_gpio;
+	unsigned int	gain;
 	u16				int_vref_mv;
-	struct spi_transfer		xfer;
-	struct spi_message		msg;
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	unsigned int			data ____cacheline_aligned;
+
+	struct ad_sigma_delta sd;
 };
 
 enum ad7780_supported_device_ids {
+	ID_AD7170,
+	ID_AD7171,
 	ID_AD7780,
 	ID_AD7781,
 };
 
-static int ad7780_read(struct ad7780_state *st, int *val)
+static struct ad7780_state *ad_sigma_delta_to_ad7780(struct ad_sigma_delta *sd)
 {
-	int ret;
-
-	spi_bus_lock(st->spi->master);
-
-	enable_irq(st->spi->irq);
-	st->done = false;
-	gpio_set_value(st->pdata->gpio_pdrst, 1);
+	return container_of(sd, struct ad7780_state, sd);
+}
 
-	ret = wait_event_interruptible(st->wq_data_avail, st->done);
-	disable_irq_nosync(st->spi->irq);
-	if (ret)
-		goto out;
+static int ad7780_set_mode(struct ad_sigma_delta *sigma_delta,
+	enum ad_sigma_delta_mode mode)
+{
+	struct ad7780_state *st = ad_sigma_delta_to_ad7780(sigma_delta);
+	unsigned val;
+
+	switch (mode) {
+	case AD_SD_MODE_SINGLE:
+	case AD_SD_MODE_CONTINUOUS:
+		val = 1;
+		break;
+	default:
+		val = 0;
+		break;
+	}
 
-	ret = spi_sync_locked(st->spi, &st->msg);
-	*val = be32_to_cpu(st->data);
-out:
-	gpio_set_value(st->pdata->gpio_pdrst, 0);
-	spi_bus_unlock(st->spi->master);
+	if (gpio_is_valid(st->powerdown_gpio))
+		gpio_set_value(st->powerdown_gpio, val);
 
-	return ret;
+	return 0;
 }
 
 static int ad7780_read_raw(struct iio_dev *indio_dev,
@@ -89,89 +90,75 @@ static int ad7780_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad7780_state *st = iio_priv(indio_dev);
-	struct iio_chan_spec channel = st->chip_info->channel;
-	int ret, smpl = 0;
 	unsigned long scale_uv;
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
-		ret = ad7780_read(st, &smpl);
-		mutex_unlock(&indio_dev->mlock);
-
-		if (ret < 0)
-			return ret;
-
-		if ((smpl & AD7780_ERR) ||
-			!((smpl & AD7780_PAT0) && !(smpl & AD7780_PAT1)))
-			return -EIO;
-
-		*val = (smpl >> channel.scan_type.shift) &
-			((1 << (channel.scan_type.realbits)) - 1);
-		*val -= (1 << (channel.scan_type.realbits - 1));
-
-		if (!(smpl & AD7780_GAIN))
-			*val *= 128;
-
-		return IIO_VAL_INT;
+		return ad_sigma_delta_single_conversion(indio_dev, chan, val);
 	case IIO_CHAN_INFO_SCALE:
-		scale_uv = (st->int_vref_mv * 100000)
-			>> (channel.scan_type.realbits - 1);
+		scale_uv = (st->int_vref_mv * 100000 * st->gain)
+			>> (chan->scan_type.realbits - 1);
 		*val =  scale_uv / 100000;
 		*val2 = (scale_uv % 100000) * 10;
 		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_OFFSET:
+		*val -= (1 << (chan->scan_type.realbits - 1));
+		return IIO_VAL_INT;
 	}
+
 	return -EINVAL;
 }
 
+static int ad7780_postprocess_sample(struct ad_sigma_delta *sigma_delta,
+	unsigned int raw_sample)
+{
+	struct ad7780_state *st = ad_sigma_delta_to_ad7780(sigma_delta);
+	const struct ad7780_chip_info *chip_info = st->chip_info;
+
+	if ((raw_sample & AD7780_ERR) ||
+		((raw_sample & chip_info->pattern_mask) != chip_info->pattern))
+		return -EIO;
+
+	if (raw_sample & AD7780_GAIN)
+		st->gain = 1;
+	else
+		st->gain = 128;
+
+	return 0;
+}
+
+static const struct ad_sigma_delta_info ad7780_sigma_delta_info = {
+	.set_mode = ad7780_set_mode,
+	.postprocess_sample = ad7780_postprocess_sample,
+	.has_registers = false,
+};
+
+#define AD7780_CHANNEL(bits, wordsize) \
+	AD_SD_CHANNEL(1, 0, 0, bits, 32, wordsize - bits)
+
 static const struct ad7780_chip_info ad7780_chip_info_tbl[] = {
+	[ID_AD7170] = {
+		.channel = AD7780_CHANNEL(12, 24),
+		.pattern = 0x5,
+		.pattern_mask = 0x7,
+	},
+	[ID_AD7171] = {
+		.channel = AD7780_CHANNEL(16, 24),
+		.pattern = 0x5,
+		.pattern_mask = 0x7,
+	},
 	[ID_AD7780] = {
-		.channel = {
-			.type = IIO_VOLTAGE,
-			.indexed = 1,
-			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_type = {
-				.sign = 'u',
-				.realbits = 24,
-				.storagebits = 32,
-				.shift = 8,
-			},
-		},
+		.channel = AD7780_CHANNEL(24, 32),
+		.pattern = 0x1,
+		.pattern_mask = 0x3,
 	},
 	[ID_AD7781] = {
-		.channel = {
-			.type = IIO_VOLTAGE,
-			.indexed = 1,
-			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_type = {
-				.sign = 'u',
-				.realbits = 20,
-				.storagebits = 32,
-				.shift = 12,
-			},
-		},
+		.channel = AD7780_CHANNEL(20, 32),
+		.pattern = 0x1,
+		.pattern_mask = 0x3,
 	},
 };
 
-/**
- *  Interrupt handler
- */
-static irqreturn_t ad7780_interrupt(int irq, void *dev_id)
-{
-	struct ad7780_state *st = dev_id;
-
-	st->done = true;
-	wake_up_interruptible(&st->wq_data_avail);
-
-	return IRQ_HANDLED;
-};
-
 static const struct iio_info ad7780_info = {
 	.read_raw = &ad7780_read_raw,
 	.driver_module = THIS_MODULE,
@@ -184,16 +171,14 @@ static int __devinit ad7780_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
 
-	if (!pdata) {
-		dev_dbg(&spi->dev, "no platform data?\n");
-		return -ENODEV;
-	}
-
 	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
 	st = iio_priv(indio_dev);
+	st->gain = 1;
+
+	ad_sd_init(&st->sd, indio_dev, spi, &ad7780_sigma_delta_info);
 
 	st->reg = regulator_get(&spi->dev, "vcc");
 	if (!IS_ERR(st->reg)) {
@@ -207,8 +192,6 @@ static int __devinit ad7780_probe(struct spi_device *spi)
 	st->chip_info =
 		&ad7780_chip_info_tbl[spi_get_device_id(spi)->driver_data];
 
-	st->pdata = pdata;
-
 	if (pdata && pdata->vref_mv)
 		st->int_vref_mv = pdata->vref_mv;
 	else if (voltage_uv)
@@ -217,7 +200,6 @@ static int __devinit ad7780_probe(struct spi_device *spi)
 		dev_warn(&spi->dev, "reference voltage unspecified\n");
 
 	spi_set_drvdata(spi, indio_dev);
-	st->spi = spi;
 
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
@@ -226,40 +208,34 @@ static int __devinit ad7780_probe(struct spi_device *spi)
 	indio_dev->num_channels = 1;
 	indio_dev->info = &ad7780_info;
 
-	init_waitqueue_head(&st->wq_data_avail);
-
-	/* Setup default message */
+	if (pdata && gpio_is_valid(pdata->gpio_pdrst)) {
 
-	st->xfer.rx_buf = &st->data;
-	st->xfer.len = st->chip_info->channel.scan_type.storagebits / 8;
-
-	spi_message_init(&st->msg);
-	spi_message_add_tail(&st->xfer, &st->msg);
-
-	ret = gpio_request_one(st->pdata->gpio_pdrst, GPIOF_OUT_INIT_LOW,
+		ret = gpio_request_one(pdata->gpio_pdrst, GPIOF_OUT_INIT_LOW,
 			       "AD7780 /PDRST");
-	if (ret) {
-		dev_err(&spi->dev, "failed to request GPIO PDRST\n");
-		goto error_disable_reg;
+		if (ret) {
+			dev_err(&spi->dev, "failed to request GPIO PDRST\n");
+			goto error_disable_reg;
+		}
+		st->powerdown_gpio = pdata->gpio_pdrst;
+	} else {
+		st->powerdown_gpio = -1;
 	}
 
-	ret = request_irq(spi->irq, ad7780_interrupt,
-		IRQF_TRIGGER_FALLING, spi_get_device_id(spi)->name, st);
+	ret = ad_sd_setup_buffer_and_trigger(indio_dev);
 	if (ret)
 		goto error_free_gpio;
 
-	disable_irq(spi->irq);
-
 	ret = iio_device_register(indio_dev);
 	if (ret)
-		goto error_free_irq;
+		goto error_cleanup_buffer_and_trigger;
 
 	return 0;
 
-error_free_irq:
-	free_irq(spi->irq, st);
+error_cleanup_buffer_and_trigger:
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
 error_free_gpio:
-	gpio_free(st->pdata->gpio_pdrst);
+	if (pdata && gpio_is_valid(pdata->gpio_pdrst))
+		gpio_free(pdata->gpio_pdrst);
 error_disable_reg:
 	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
@@ -272,14 +248,17 @@ error_put_reg:
 	return ret;
 }
 
-static int ad7780_remove(struct spi_device *spi)
+static int __devexit ad7780_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ad7780_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	free_irq(spi->irq, st);
-	gpio_free(st->pdata->gpio_pdrst);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
+
+	if (gpio_is_valid(st->powerdown_gpio))
+		gpio_free(st->powerdown_gpio);
+
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
@@ -290,6 +269,8 @@ static int ad7780_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id ad7780_id[] = {
+	{"ad7170", ID_AD7170},
+	{"ad7171", ID_AD7171},
 	{"ad7780", ID_AD7780},
 	{"ad7781", ID_AD7781},
 	{}
@@ -308,5 +289,5 @@ static struct spi_driver ad7780_driver = {
 module_spi_driver(ad7780_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
-MODULE_DESCRIPTION("Analog Devices AD7780/1 ADC");
+MODULE_DESCRIPTION("Analog Devices AD7780 and similar ADCs");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/ad7793.c b/drivers/staging/iio/adc/ad7793.c
index 112e2b7b5bc4..691a7be6f5cb 100644
--- a/drivers/staging/iio/adc/ad7793.c
+++ b/drivers/staging/iio/adc/ad7793.c
@@ -1,5 +1,5 @@
 /*
- * AD7792/AD7793 SPI ADC driver
+ * AD7785/AD7792/AD7793/AD7794/AD7795 SPI ADC driver
  *
  * Copyright 2011-2012 Analog Devices Inc.
  *
@@ -24,6 +24,7 @@
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
+#include <linux/iio/adc/ad_sigma_delta.h>
 
 #include "ad7793.h"
 
@@ -36,198 +37,65 @@
  */
 
 struct ad7793_chip_info {
-	struct iio_chan_spec		channel[7];
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
 };
 
 struct ad7793_state {
-	struct spi_device		*spi;
-	struct iio_trigger		*trig;
 	const struct ad7793_chip_info	*chip_info;
 	struct regulator		*reg;
-	struct ad7793_platform_data	*pdata;
-	wait_queue_head_t		wq_data_avail;
-	bool				done;
-	bool				irq_dis;
 	u16				int_vref_mv;
 	u16				mode;
 	u16				conf;
 	u32				scale_avail[8][2];
 
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	u8				data[4] ____cacheline_aligned;
+	struct ad_sigma_delta		sd;
+
 };
 
 enum ad7793_supported_device_ids {
+	ID_AD7785,
 	ID_AD7792,
 	ID_AD7793,
+	ID_AD7794,
+	ID_AD7795,
 };
 
-static int __ad7793_write_reg(struct ad7793_state *st, bool locked,
-			      bool cs_change, unsigned char reg,
-			      unsigned size, unsigned val)
-{
-	u8 *data = st->data;
-	struct spi_transfer t = {
-		.tx_buf		= data,
-		.len		= size + 1,
-		.cs_change	= cs_change,
-	};
-	struct spi_message m;
-
-	data[0] = AD7793_COMM_WRITE | AD7793_COMM_ADDR(reg);
-
-	switch (size) {
-	case 3:
-		data[1] = val >> 16;
-		data[2] = val >> 8;
-		data[3] = val;
-		break;
-	case 2:
-		data[1] = val >> 8;
-		data[2] = val;
-		break;
-	case 1:
-		data[1] = val;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t, &m);
-
-	if (locked)
-		return spi_sync_locked(st->spi, &m);
-	else
-		return spi_sync(st->spi, &m);
-}
-
-static int ad7793_write_reg(struct ad7793_state *st,
-			    unsigned reg, unsigned size, unsigned val)
+static struct ad7793_state *ad_sigma_delta_to_ad7793(struct ad_sigma_delta *sd)
 {
-	return __ad7793_write_reg(st, false, false, reg, size, val);
+	return container_of(sd, struct ad7793_state, sd);
 }
 
-static int __ad7793_read_reg(struct ad7793_state *st, bool locked,
-			     bool cs_change, unsigned char reg,
-			     int *val, unsigned size)
+static int ad7793_set_channel(struct ad_sigma_delta *sd, unsigned int channel)
 {
-	u8 *data = st->data;
-	int ret;
-	struct spi_transfer t[] = {
-		{
-			.tx_buf = data,
-			.len = 1,
-		}, {
-			.rx_buf = data,
-			.len = size,
-			.cs_change = cs_change,
-		},
-	};
-	struct spi_message m;
-
-	data[0] = AD7793_COMM_READ | AD7793_COMM_ADDR(reg);
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t[0], &m);
-	spi_message_add_tail(&t[1], &m);
-
-	if (locked)
-		ret = spi_sync_locked(st->spi, &m);
-	else
-		ret = spi_sync(st->spi, &m);
-
-	if (ret < 0)
-		return ret;
-
-	switch (size) {
-	case 3:
-		*val = data[0] << 16 | data[1] << 8 | data[2];
-		break;
-	case 2:
-		*val = data[0] << 8 | data[1];
-		break;
-	case 1:
-		*val = data[0];
-		break;
-	default:
-		return -EINVAL;
-	}
+	struct ad7793_state *st = ad_sigma_delta_to_ad7793(sd);
 
-	return 0;
-}
+	st->conf &= ~AD7793_CONF_CHAN_MASK;
+	st->conf |= AD7793_CONF_CHAN(channel);
 
-static int ad7793_read_reg(struct ad7793_state *st,
-			   unsigned reg, int *val, unsigned size)
-{
-	return __ad7793_read_reg(st, 0, 0, reg, val, size);
+	return ad_sd_write_reg(&st->sd, AD7793_REG_CONF, 2, st->conf);
 }
 
-static int ad7793_read(struct ad7793_state *st, unsigned ch,
-		       unsigned len, int *val)
+static int ad7793_set_mode(struct ad_sigma_delta *sd,
+			   enum ad_sigma_delta_mode mode)
 {
-	int ret;
-	st->conf = (st->conf & ~AD7793_CONF_CHAN(-1)) | AD7793_CONF_CHAN(ch);
-	st->mode = (st->mode & ~AD7793_MODE_SEL(-1)) |
-		AD7793_MODE_SEL(AD7793_MODE_SINGLE);
-
-	ad7793_write_reg(st, AD7793_REG_CONF, sizeof(st->conf), st->conf);
-
-	spi_bus_lock(st->spi->master);
-	st->done = false;
-
-	ret = __ad7793_write_reg(st, 1, 1, AD7793_REG_MODE,
-				 sizeof(st->mode), st->mode);
-	if (ret < 0)
-		goto out;
+	struct ad7793_state *st = ad_sigma_delta_to_ad7793(sd);
 
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-	wait_event_interruptible(st->wq_data_avail, st->done);
+	st->mode &= ~AD7793_MODE_SEL_MASK;
+	st->mode |= AD7793_MODE_SEL(mode);
 
-	ret = __ad7793_read_reg(st, 1, 0, AD7793_REG_DATA, val, len);
-out:
-	spi_bus_unlock(st->spi->master);
-
-	return ret;
+	return ad_sd_write_reg(&st->sd, AD7793_REG_MODE, 2, st->mode);
 }
 
-static int ad7793_calibrate(struct ad7793_state *st, unsigned mode, unsigned ch)
-{
-	int ret;
-
-	st->conf = (st->conf & ~AD7793_CONF_CHAN(-1)) | AD7793_CONF_CHAN(ch);
-	st->mode = (st->mode & ~AD7793_MODE_SEL(-1)) | AD7793_MODE_SEL(mode);
-
-	ad7793_write_reg(st, AD7793_REG_CONF, sizeof(st->conf), st->conf);
-
-	spi_bus_lock(st->spi->master);
-	st->done = false;
-
-	ret = __ad7793_write_reg(st, 1, 1, AD7793_REG_MODE,
-				 sizeof(st->mode), st->mode);
-	if (ret < 0)
-		goto out;
-
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-	wait_event_interruptible(st->wq_data_avail, st->done);
-
-	st->mode = (st->mode & ~AD7793_MODE_SEL(-1)) |
-		AD7793_MODE_SEL(AD7793_MODE_IDLE);
-
-	ret = __ad7793_write_reg(st, 1, 0, AD7793_REG_MODE,
-				 sizeof(st->mode), st->mode);
-out:
-	spi_bus_unlock(st->spi->master);
-
-	return ret;
-}
+static const struct ad_sigma_delta_info ad7793_sigma_delta_info = {
+	.set_channel = ad7793_set_channel,
+	.set_mode = ad7793_set_mode,
+	.has_registers = true,
+	.addr_shift = 3,
+	.read_mask = BIT(6),
+};
 
-static const u8 ad7793_calib_arr[6][2] = {
+static const struct ad_sd_calib_data ad7793_calib_arr[6] = {
 	{AD7793_MODE_CAL_INT_ZERO, AD7793_CH_AIN1P_AIN1M},
 	{AD7793_MODE_CAL_INT_FULL, AD7793_CH_AIN1P_AIN1M},
 	{AD7793_MODE_CAL_INT_ZERO, AD7793_CH_AIN2P_AIN2M},
@@ -238,59 +106,49 @@ static const u8 ad7793_calib_arr[6][2] = {
 
 static int ad7793_calibrate_all(struct ad7793_state *st)
 {
-	int i, ret;
-
-	for (i = 0; i < ARRAY_SIZE(ad7793_calib_arr); i++) {
-		ret = ad7793_calibrate(st, ad7793_calib_arr[i][0],
-				       ad7793_calib_arr[i][1]);
-		if (ret)
-			goto out;
-	}
-
-	return 0;
-out:
-	dev_err(&st->spi->dev, "Calibration failed\n");
-	return ret;
+	return ad_sd_calibrate_all(&st->sd, ad7793_calib_arr,
+				   ARRAY_SIZE(ad7793_calib_arr));
 }
 
-static int ad7793_setup(struct ad7793_state *st)
+static int ad7793_setup(struct iio_dev *indio_dev,
+	const struct ad7793_platform_data *pdata)
 {
+	struct ad7793_state *st = iio_priv(indio_dev);
 	int i, ret = -1;
 	unsigned long long scale_uv;
 	u32 id;
 
 	/* reset the serial interface */
-	ret = spi_write(st->spi, (u8 *)&ret, sizeof(ret));
+	ret = spi_write(st->sd.spi, (u8 *)&ret, sizeof(ret));
 	if (ret < 0)
 		goto out;
 	msleep(1); /* Wait for at least 500us */
 
 	/* write/read test for device presence */
-	ret = ad7793_read_reg(st, AD7793_REG_ID, &id, 1);
+	ret = ad_sd_read_reg(&st->sd, AD7793_REG_ID, 1, &id);
 	if (ret)
 		goto out;
 
 	id &= AD7793_ID_MASK;
 
-	if (!((id == AD7792_ID) || (id == AD7793_ID))) {
-		dev_err(&st->spi->dev, "device ID query failed\n");
+	if (!((id == AD7792_ID) || (id == AD7793_ID) || (id == AD7795_ID))) {
+		dev_err(&st->sd.spi->dev, "device ID query failed\n");
 		goto out;
 	}
 
-	st->mode  = (st->pdata->mode & ~AD7793_MODE_SEL(-1)) |
-			AD7793_MODE_SEL(AD7793_MODE_IDLE);
-	st->conf  = st->pdata->conf & ~AD7793_CONF_CHAN(-1);
+	st->mode = pdata->mode;
+	st->conf = pdata->conf;
 
-	ret = ad7793_write_reg(st, AD7793_REG_MODE, sizeof(st->mode), st->mode);
+	ret = ad7793_set_mode(&st->sd, AD_SD_MODE_IDLE);
 	if (ret)
 		goto out;
 
-	ret = ad7793_write_reg(st, AD7793_REG_CONF, sizeof(st->conf), st->conf);
+	ret = ad7793_set_channel(&st->sd, 0);
 	if (ret)
 		goto out;
 
-	ret = ad7793_write_reg(st, AD7793_REG_IO,
-			       sizeof(st->pdata->io), st->pdata->io);
+	ret = ad_sd_write_reg(&st->sd, AD7793_REG_IO,
+			       sizeof(pdata->io), pdata->io);
 	if (ret)
 		goto out;
 
@@ -301,7 +159,7 @@ static int ad7793_setup(struct ad7793_state *st)
 	/* Populate available ADC input ranges */
 	for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++) {
 		scale_uv = ((u64)st->int_vref_mv * 100000000)
-			>> (st->chip_info->channel[0].scan_type.realbits -
+			>> (st->chip_info->channels[0].scan_type.realbits -
 			(!!(st->conf & AD7793_CONF_UNIPOLAR) ? 0 : 1));
 		scale_uv >>= i;
 
@@ -311,184 +169,10 @@ static int ad7793_setup(struct ad7793_state *st)
 
 	return 0;
 out:
-	dev_err(&st->spi->dev, "setup failed\n");
+	dev_err(&st->sd.spi->dev, "setup failed\n");
 	return ret;
 }
 
-static int ad7793_ring_preenable(struct iio_dev *indio_dev)
-{
-	struct ad7793_state *st = iio_priv(indio_dev);
-	unsigned channel;
-	int ret;
-
-	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
-		return -EINVAL;
-	ret = iio_sw_buffer_preenable(indio_dev);
-	if (ret < 0)
-		return ret;
-
-	channel = find_first_bit(indio_dev->active_scan_mask,
-				 indio_dev->masklength);
-
-	st->mode  = (st->mode & ~AD7793_MODE_SEL(-1)) |
-		    AD7793_MODE_SEL(AD7793_MODE_CONT);
-	st->conf  = (st->conf & ~AD7793_CONF_CHAN(-1)) |
-		    AD7793_CONF_CHAN(indio_dev->channels[channel].address);
-
-	ad7793_write_reg(st, AD7793_REG_CONF, sizeof(st->conf), st->conf);
-
-	spi_bus_lock(st->spi->master);
-	__ad7793_write_reg(st, 1, 1, AD7793_REG_MODE,
-			   sizeof(st->mode), st->mode);
-
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-
-	return 0;
-}
-
-static int ad7793_ring_postdisable(struct iio_dev *indio_dev)
-{
-	struct ad7793_state *st = iio_priv(indio_dev);
-
-	st->mode  = (st->mode & ~AD7793_MODE_SEL(-1)) |
-		    AD7793_MODE_SEL(AD7793_MODE_IDLE);
-
-	st->done = false;
-	wait_event_interruptible(st->wq_data_avail, st->done);
-
-	if (!st->irq_dis)
-		disable_irq_nosync(st->spi->irq);
-
-	__ad7793_write_reg(st, 1, 0, AD7793_REG_MODE,
-			   sizeof(st->mode), st->mode);
-
-	return spi_bus_unlock(st->spi->master);
-}
-
-/**
- * ad7793_trigger_handler() bh of trigger launched polling to ring buffer
- **/
-
-static irqreturn_t ad7793_trigger_handler(int irq, void *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct iio_buffer *ring = indio_dev->buffer;
-	struct ad7793_state *st = iio_priv(indio_dev);
-	s64 dat64[2];
-	s32 *dat32 = (s32 *)dat64;
-
-	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
-		__ad7793_read_reg(st, 1, 1, AD7793_REG_DATA,
-				  dat32,
-				  indio_dev->channels[0].scan_type.realbits/8);
-
-	/* Guaranteed to be aligned with 8 byte boundary */
-	if (indio_dev->scan_timestamp)
-		dat64[1] = pf->timestamp;
-
-	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
-
-	iio_trigger_notify_done(indio_dev->trig);
-	st->irq_dis = false;
-	enable_irq(st->spi->irq);
-
-	return IRQ_HANDLED;
-}
-
-static const struct iio_buffer_setup_ops ad7793_ring_setup_ops = {
-	.preenable = &ad7793_ring_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-	.postdisable = &ad7793_ring_postdisable,
-	.validate_scan_mask = &iio_validate_scan_mask_onehot,
-};
-
-static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-			&ad7793_trigger_handler, &ad7793_ring_setup_ops);
-}
-
-static void ad7793_ring_cleanup(struct iio_dev *indio_dev)
-{
-	iio_triggered_buffer_cleanup(indio_dev);
-}
-
-/**
- * ad7793_data_rdy_trig_poll() the event handler for the data rdy trig
- **/
-static irqreturn_t ad7793_data_rdy_trig_poll(int irq, void *private)
-{
-	struct ad7793_state *st = iio_priv(private);
-
-	st->done = true;
-	wake_up_interruptible(&st->wq_data_avail);
-	disable_irq_nosync(irq);
-	st->irq_dis = true;
-	iio_trigger_poll(st->trig, iio_get_time_ns());
-
-	return IRQ_HANDLED;
-}
-
-static struct iio_trigger_ops ad7793_trigger_ops = {
-	.owner = THIS_MODULE,
-};
-
-static int ad7793_probe_trigger(struct iio_dev *indio_dev)
-{
-	struct ad7793_state *st = iio_priv(indio_dev);
-	int ret;
-
-	st->trig = iio_trigger_alloc("%s-dev%d",
-					spi_get_device_id(st->spi)->name,
-					indio_dev->id);
-	if (st->trig == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	st->trig->ops = &ad7793_trigger_ops;
-
-	ret = request_irq(st->spi->irq,
-			  ad7793_data_rdy_trig_poll,
-			  IRQF_TRIGGER_LOW,
-			  spi_get_device_id(st->spi)->name,
-			  indio_dev);
-	if (ret)
-		goto error_free_trig;
-
-	disable_irq_nosync(st->spi->irq);
-	st->irq_dis = true;
-	st->trig->dev.parent = &st->spi->dev;
-	st->trig->private_data = indio_dev;
-
-	ret = iio_trigger_register(st->trig);
-
-	/* select default trigger */
-	indio_dev->trig = st->trig;
-	if (ret)
-		goto error_free_irq;
-
-	return 0;
-
-error_free_irq:
-	free_irq(st->spi->irq, indio_dev);
-error_free_trig:
-	iio_trigger_free(st->trig);
-error_ret:
-	return ret;
-}
-
-static void ad7793_remove_trigger(struct iio_dev *indio_dev)
-{
-	struct ad7793_state *st = iio_priv(indio_dev);
-
-	iio_trigger_unregister(st->trig);
-	free_irq(st->spi->irq, indio_dev);
-	iio_trigger_free(st->trig);
-}
-
 static const u16 sample_freq_avail[16] = {0, 470, 242, 123, 62, 50, 39, 33, 19,
 					  17, 16, 12, 10, 8, 6, 4};
 
@@ -531,7 +215,7 @@ static ssize_t ad7793_write_frequency(struct device *dev,
 			mutex_lock(&indio_dev->mlock);
 			st->mode &= ~AD7793_MODE_RATE(-1);
 			st->mode |= AD7793_MODE_RATE(i);
-			ad7793_write_reg(st, AD7793_REG_MODE,
+			ad_sd_write_reg(&st->sd, AD7793_REG_MODE,
 					 sizeof(st->mode), st->mode);
 			mutex_unlock(&indio_dev->mlock);
 			ret = 0;
@@ -585,26 +269,16 @@ static int ad7793_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad7793_state *st = iio_priv(indio_dev);
-	int ret, smpl = 0;
+	int ret;
 	unsigned long long scale_uv;
 	bool unipolar = !!(st->conf & AD7793_CONF_UNIPOLAR);
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
-		if (iio_buffer_enabled(indio_dev))
-			ret = -EBUSY;
-		else
-			ret = ad7793_read(st, chan->address,
-					chan->scan_type.realbits / 8, &smpl);
-		mutex_unlock(&indio_dev->mlock);
-
+		ret = ad_sigma_delta_single_conversion(indio_dev, chan, val);
 		if (ret < 0)
 			return ret;
 
-		*val = (smpl >> chan->scan_type.shift) &
-			((1 << (chan->scan_type.realbits)) - 1);
-
 		return IIO_VAL_INT;
 
 	case IIO_CHAN_INFO_SCALE:
@@ -675,17 +349,18 @@ static int ad7793_write_raw(struct iio_dev *indio_dev,
 		ret = -EINVAL;
 		for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++)
 			if (val2 == st->scale_avail[i][1]) {
+				ret = 0;
 				tmp = st->conf;
 				st->conf &= ~AD7793_CONF_GAIN(-1);
 				st->conf |= AD7793_CONF_GAIN(i);
 
-				if (tmp != st->conf) {
-					ad7793_write_reg(st, AD7793_REG_CONF,
-							 sizeof(st->conf),
-							 st->conf);
-					ad7793_calibrate_all(st);
-				}
-				ret = 0;
+				if (tmp == st->conf)
+					break;
+
+				ad_sd_write_reg(&st->sd, AD7793_REG_CONF,
+						sizeof(st->conf), st->conf);
+				ad7793_calibrate_all(st);
+				break;
 			}
 		break;
 	default:
@@ -696,15 +371,6 @@ static int ad7793_write_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
-static int ad7793_validate_trigger(struct iio_dev *indio_dev,
-				   struct iio_trigger *trig)
-{
-	if (indio_dev->trig != trig)
-		return -EINVAL;
-
-	return 0;
-}
-
 static int ad7793_write_raw_get_fmt(struct iio_dev *indio_dev,
 			       struct iio_chan_spec const *chan,
 			       long mask)
@@ -717,172 +383,67 @@ static const struct iio_info ad7793_info = {
 	.write_raw = &ad7793_write_raw,
 	.write_raw_get_fmt = &ad7793_write_raw_get_fmt,
 	.attrs = &ad7793_attribute_group,
-	.validate_trigger = ad7793_validate_trigger,
+	.validate_trigger = ad_sd_validate_trigger,
 	.driver_module = THIS_MODULE,
 };
 
+#define DECLARE_AD7793_CHANNELS(_name, _b, _sb, _s) \
+const struct iio_chan_spec _name##_channels[] = { \
+	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), (_s)), \
+	AD_SD_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), (_s)), \
+	AD_SD_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), (_s)), \
+	AD_SD_SHORTED_CHANNEL(3, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), (_s)), \
+	AD_SD_TEMP_CHANNEL(4, AD7793_CH_TEMP, (_b), (_sb), (_s)), \
+	AD_SD_SUPPLY_CHANNEL(5, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), (_s)), \
+	IIO_CHAN_SOFT_TIMESTAMP(6), \
+}
+
+#define DECLARE_AD7795_CHANNELS(_name, _b, _sb) \
+const struct iio_chan_spec _name##_channels[] = { \
+	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
+	AD_SD_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), 0), \
+	AD_SD_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), 0), \
+	AD_SD_DIFF_CHANNEL(3, 3, 3, AD7795_CH_AIN4P_AIN4M, (_b), (_sb), 0), \
+	AD_SD_DIFF_CHANNEL(4, 4, 4, AD7795_CH_AIN5P_AIN5M, (_b), (_sb), 0), \
+	AD_SD_DIFF_CHANNEL(5, 5, 5, AD7795_CH_AIN6P_AIN6M, (_b), (_sb), 0), \
+	AD_SD_SHORTED_CHANNEL(6, 0, AD7795_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
+	AD_SD_TEMP_CHANNEL(7, AD7793_CH_TEMP, (_b), (_sb), 0), \
+	AD_SD_SUPPLY_CHANNEL(8, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
+	IIO_CHAN_SOFT_TIMESTAMP(9), \
+}
+
+static DECLARE_AD7793_CHANNELS(ad7785, 20, 32, 4);
+static DECLARE_AD7793_CHANNELS(ad7792, 16, 32, 0);
+static DECLARE_AD7793_CHANNELS(ad7793, 24, 32, 0);
+static DECLARE_AD7795_CHANNELS(ad7794, 16, 32);
+static DECLARE_AD7795_CHANNELS(ad7795, 24, 32);
+
 static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
-	[ID_AD7793] = {
-		.channel[0] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 0,
-			.channel2 = 0,
-			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 0,
-			.scan_type = IIO_ST('u', 24, 32, 0)
-		},
-		.channel[1] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 1,
-			.channel2 = 1,
-			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 1,
-			.scan_type = IIO_ST('u', 24, 32, 0)
-		},
-		.channel[2] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 2,
-			.channel2 = 2,
-			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 2,
-			.scan_type = IIO_ST('u', 24, 32, 0)
-		},
-		.channel[3] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.extend_name = "shorted",
-			.indexed = 1,
-			.channel = 2,
-			.channel2 = 2,
-			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 3,
-			.scan_type = IIO_ST('u', 24, 32, 0)
-		},
-		.channel[4] = {
-			.type = IIO_TEMP,
-			.indexed = 1,
-			.channel = 0,
-			.address = AD7793_CH_TEMP,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-			.scan_index = 4,
-			.scan_type = IIO_ST('u', 24, 32, 0),
-		},
-		.channel[5] = {
-			.type = IIO_VOLTAGE,
-			.extend_name = "supply",
-			.indexed = 1,
-			.channel = 4,
-			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 5,
-			.scan_type = IIO_ST('u', 24, 32, 0),
-		},
-		.channel[6] = IIO_CHAN_SOFT_TIMESTAMP(6),
+	[ID_AD7785] = {
+		.channels = ad7785_channels,
+		.num_channels = ARRAY_SIZE(ad7785_channels),
 	},
 	[ID_AD7792] = {
-		.channel[0] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 0,
-			.channel2 = 0,
-			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 0,
-			.scan_type = IIO_ST('u', 16, 32, 0)
-		},
-		.channel[1] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 1,
-			.channel2 = 1,
-			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 1,
-			.scan_type = IIO_ST('u', 16, 32, 0)
-		},
-		.channel[2] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.indexed = 1,
-			.channel = 2,
-			.channel2 = 2,
-			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 2,
-			.scan_type = IIO_ST('u', 16, 32, 0)
-		},
-		.channel[3] = {
-			.type = IIO_VOLTAGE,
-			.differential = 1,
-			.extend_name = "shorted",
-			.indexed = 1,
-			.channel = 2,
-			.channel2 = 2,
-			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SHARED_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 3,
-			.scan_type = IIO_ST('u', 16, 32, 0)
-		},
-		.channel[4] = {
-			.type = IIO_TEMP,
-			.indexed = 1,
-			.channel = 0,
-			.address = AD7793_CH_TEMP,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-			.scan_index = 4,
-			.scan_type = IIO_ST('u', 16, 32, 0),
-		},
-		.channel[5] = {
-			.type = IIO_VOLTAGE,
-			.extend_name = "supply",
-			.indexed = 1,
-			.channel = 4,
-			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
-			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-			IIO_CHAN_INFO_OFFSET_SHARED_BIT,
-			.scan_index = 5,
-			.scan_type = IIO_ST('u', 16, 32, 0),
-		},
-		.channel[6] = IIO_CHAN_SOFT_TIMESTAMP(6),
+		.channels = ad7792_channels,
+		.num_channels = ARRAY_SIZE(ad7792_channels),
+	},
+	[ID_AD7793] = {
+		.channels = ad7793_channels,
+		.num_channels = ARRAY_SIZE(ad7793_channels),
+	},
+	[ID_AD7794] = {
+		.channels = ad7794_channels,
+		.num_channels = ARRAY_SIZE(ad7794_channels),
+	},
+	[ID_AD7795] = {
+		.channels = ad7795_channels,
+		.num_channels = ARRAY_SIZE(ad7795_channels),
 	},
 };
 
 static int __devinit ad7793_probe(struct spi_device *spi)
 {
-	struct ad7793_platform_data *pdata = spi->dev.platform_data;
+	const struct ad7793_platform_data *pdata = spi->dev.platform_data;
 	struct ad7793_state *st;
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
@@ -903,6 +464,8 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 
 	st = iio_priv(indio_dev);
 
+	ad_sd_init(&st->sd, indio_dev, spi, &ad7793_sigma_delta_info);
+
 	st->reg = regulator_get(&spi->dev, "vcc");
 	if (!IS_ERR(st->reg)) {
 		ret = regulator_enable(st->reg);
@@ -915,8 +478,6 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 	st->chip_info =
 		&ad7793_chip_info_tbl[spi_get_device_id(spi)->driver_data];
 
-	st->pdata = pdata;
-
 	if (pdata && pdata->vref_mv)
 		st->int_vref_mv = pdata->vref_mv;
 	else if (voltage_uv)
@@ -925,26 +486,19 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 		st->int_vref_mv = 1170; /* Build-in ref */
 
 	spi_set_drvdata(spi, indio_dev);
-	st->spi = spi;
 
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-	indio_dev->channels = st->chip_info->channel;
-	indio_dev->num_channels = 7;
+	indio_dev->channels = st->chip_info->channels;
+	indio_dev->num_channels = st->chip_info->num_channels;
 	indio_dev->info = &ad7793_info;
 
-	init_waitqueue_head(&st->wq_data_avail);
-
-	ret = ad7793_register_ring_funcs_and_init(indio_dev);
+	ret = ad_sd_setup_buffer_and_trigger(indio_dev);
 	if (ret)
 		goto error_disable_reg;
 
-	ret = ad7793_probe_trigger(indio_dev);
-	if (ret)
-		goto error_unreg_ring;
-
-	ret = ad7793_setup(st);
+	ret = ad7793_setup(indio_dev, pdata);
 	if (ret)
 		goto error_remove_trigger;
 
@@ -955,9 +509,7 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 	return 0;
 
 error_remove_trigger:
-	ad7793_remove_trigger(indio_dev);
-error_unreg_ring:
-	ad7793_ring_cleanup(indio_dev);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(st->reg))
 		regulator_disable(st->reg);
@@ -970,14 +522,13 @@ error_put_reg:
 	return ret;
 }
 
-static int ad7793_remove(struct spi_device *spi)
+static int __devexit ad7793_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ad7793_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	ad7793_remove_trigger(indio_dev);
-	ad7793_ring_cleanup(indio_dev);
+	ad_sd_cleanup_buffer_and_trigger(indio_dev);
 
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
@@ -990,8 +541,11 @@ static int ad7793_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id ad7793_id[] = {
+	{"ad7785", ID_AD7785},
 	{"ad7792", ID_AD7792},
 	{"ad7793", ID_AD7793},
+	{"ad7794", ID_AD7794},
+	{"ad7795", ID_AD7795},
 	{}
 };
 MODULE_DEVICE_TABLE(spi, ad7793_id);
@@ -1008,5 +562,5 @@ static struct spi_driver ad7793_driver = {
 module_spi_driver(ad7793_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
-MODULE_DESCRIPTION("Analog Devices AD7792/3 ADC");
+MODULE_DESCRIPTION("Analog Devices AD7793 and simialr ADCs");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/ad7793.h b/drivers/staging/iio/adc/ad7793.h
index 64f7d41dc453..8fdd450a2cd9 100644
--- a/drivers/staging/iio/adc/ad7793.h
+++ b/drivers/staging/iio/adc/ad7793.h
@@ -41,6 +41,7 @@
 
 /* Mode Register Bit Designations (AD7793_REG_MODE) */
 #define AD7793_MODE_SEL(x)	(((x) & 0x7) << 13) /* Operation Mode Select */
+#define AD7793_MODE_SEL_MASK	(0x7 << 13) /* Operation Mode Select mask */
 #define AD7793_MODE_CLKSRC(x)	(((x) & 0x3) << 6) /* ADC Clock Source Select */
 #define AD7793_MODE_RATE(x)	((x) & 0xF) /* Filter Update Rate Select */
 
@@ -69,7 +70,8 @@
 #define AD7793_CONF_GAIN(x)	(((x) & 0x7) << 8) /* Gain Select */
 #define AD7793_CONF_REFSEL	(1 << 7) /* INT/EXT Reference Select */
 #define AD7793_CONF_BUF		(1 << 4) /* Buffered Mode Enable */
-#define AD7793_CONF_CHAN(x)	((x) & 0x7) /* Channel select */
+#define AD7793_CONF_CHAN(x)	((x) & 0xf) /* Channel select */
+#define AD7793_CONF_CHAN_MASK	0xf /* Channel select mask */
 
 #define AD7793_CH_AIN1P_AIN1M	0 /* AIN1(+) - AIN1(-) */
 #define AD7793_CH_AIN2P_AIN2M	1 /* AIN2(+) - AIN2(-) */
@@ -78,9 +80,15 @@
 #define AD7793_CH_TEMP		6 /* Temp Sensor */
 #define AD7793_CH_AVDD_MONITOR	7 /* AVDD Monitor */
 
+#define AD7795_CH_AIN4P_AIN4M	4 /* AIN4(+) - AIN4(-) */
+#define AD7795_CH_AIN5P_AIN5M	5 /* AIN5(+) - AIN5(-) */
+#define AD7795_CH_AIN6P_AIN6M	6 /* AIN6(+) - AIN6(-) */
+#define AD7795_CH_AIN1M_AIN1M	8 /* AIN1(-) - AIN1(-) */
+
 /* ID Register Bit Designations (AD7793_REG_ID) */
 #define AD7792_ID		0xA
 #define AD7793_ID		0xB
+#define AD7795_ID		0xF
 #define AD7793_ID_MASK		0xF
 
 /* IO (Excitation Current Sources) Register Bit Designations (AD7793_REG_IO) */
diff --git a/drivers/staging/iio/adc/ad7887_core.c b/drivers/staging/iio/adc/ad7887_core.c
index 397b84947155..551790584a12 100644
--- a/drivers/staging/iio/adc/ad7887_core.c
+++ b/drivers/staging/iio/adc/ad7887_core.c
@@ -219,7 +219,7 @@ error_put_reg:
 	return ret;
 }
 
-static int ad7887_remove(struct spi_device *spi)
+static int __devexit ad7887_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ad7887_state *st = iio_priv(indio_dev);
diff --git a/drivers/staging/iio/adc/ad7887_ring.c b/drivers/staging/iio/adc/ad7887_ring.c
index c76fdb5081c2..b39923bbeedc 100644
--- a/drivers/staging/iio/adc/ad7887_ring.c
+++ b/drivers/staging/iio/adc/ad7887_ring.c
@@ -95,7 +95,7 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 		memcpy(buf + indio_dev->scan_bytes - sizeof(s64),
 		       &time_ns, sizeof(time_ns));
 
-	indio_dev->buffer->access->store_to(indio_dev->buffer, buf, time_ns);
+	iio_push_to_buffer(indio_dev->buffer, buf);
 done:
 	kfree(buf);
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
index 858a685e3889..86026d9b20bc 100644
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ b/drivers/staging/iio/adc/ad799x_ring.c
@@ -35,7 +35,6 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct ad799x_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	s64 time_ns;
 	__u8 *rxbuf;
 	int b_sent;
@@ -78,7 +77,7 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 		memcpy(rxbuf + indio_dev->scan_bytes - sizeof(s64),
 			&time_ns, sizeof(time_ns));
 
-	ring->access->store_to(indio_dev->buffer, rxbuf, time_ns);
+	iio_push_to_buffer(indio_dev->buffer, rxbuf);
 done:
 	kfree(rxbuf);
 out:
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/staging/iio/adc/lpc32xx_adc.c
index 348d051fc2f8..7e9bd0001cc7 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/staging/iio/adc/lpc32xx_adc.c
@@ -108,7 +108,7 @@ static const struct iio_info lpc32xx_adc_iio_info = {
 	.scan_index = _index,				\
 }
 
-static struct iio_chan_spec lpc32xx_adc_iio_channels[] = {
+static const struct iio_chan_spec lpc32xx_adc_iio_channels[] = {
 	LPC32XX_ADC_CHANNEL(0),
 	LPC32XX_ADC_CHANNEL(1),
 	LPC32XX_ADC_CHANNEL(2),
diff --git a/drivers/staging/iio/adc/max1363.h b/drivers/staging/iio/adc/max1363.h
index 2cd0112067b2..c746918683f1 100644
--- a/drivers/staging/iio/adc/max1363.h
+++ b/drivers/staging/iio/adc/max1363.h
@@ -100,7 +100,7 @@ enum max1363_modes {
  */
 struct max1363_chip_info {
 	const struct iio_info		*info;
-	struct iio_chan_spec *channels;
+	const struct iio_chan_spec *channels;
 	int num_channels;
 	const enum max1363_modes	*mode_list;
 	enum max1363_modes		default_mode;
diff --git a/drivers/staging/iio/adc/max1363_core.c b/drivers/staging/iio/adc/max1363_core.c
index 6799ce23a395..d7b4ffcfa052 100644
--- a/drivers/staging/iio/adc/max1363_core.c
+++ b/drivers/staging/iio/adc/max1363_core.c
@@ -335,12 +335,12 @@ static const enum max1363_modes max1363_mode_list[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(8)			\
 	}
 
-static struct iio_chan_spec max1036_channels[] = MAX1363_4X_CHANS(8, 0);
-static struct iio_chan_spec max1136_channels[] = MAX1363_4X_CHANS(10, 0);
-static struct iio_chan_spec max1236_channels[] = MAX1363_4X_CHANS(12, 0);
-static struct iio_chan_spec max1361_channels[] =
+static const struct iio_chan_spec max1036_channels[] = MAX1363_4X_CHANS(8, 0);
+static const struct iio_chan_spec max1136_channels[] = MAX1363_4X_CHANS(10, 0);
+static const struct iio_chan_spec max1236_channels[] = MAX1363_4X_CHANS(12, 0);
+static const struct iio_chan_spec max1361_channels[] =
 	MAX1363_4X_CHANS(10, MAX1363_EV_M);
-static struct iio_chan_spec max1363_channels[] =
+static const struct iio_chan_spec max1363_channels[] =
 	MAX1363_4X_CHANS(12, MAX1363_EV_M);
 
 /* Applies to max1236, max1237 */
@@ -392,9 +392,9 @@ static const enum max1363_modes max1238_mode_list[] = {
 	MAX1363_CHAN_B(11, 10, d11m10, 23, bits, 0),	\
 	IIO_CHAN_SOFT_TIMESTAMP(24)			\
 	}
-static struct iio_chan_spec max1038_channels[] = MAX1363_12X_CHANS(8);
-static struct iio_chan_spec max1138_channels[] = MAX1363_12X_CHANS(10);
-static struct iio_chan_spec max1238_channels[] = MAX1363_12X_CHANS(12);
+static const struct iio_chan_spec max1038_channels[] = MAX1363_12X_CHANS(8);
+static const struct iio_chan_spec max1138_channels[] = MAX1363_12X_CHANS(10);
+static const struct iio_chan_spec max1238_channels[] = MAX1363_12X_CHANS(12);
 
 static const enum max1363_modes max11607_mode_list[] = {
 	_s0, _s1, _s2, _s3,
@@ -433,9 +433,9 @@ static const enum max1363_modes max11608_mode_list[] = {
 	MAX1363_CHAN_B(7, 6, d7m6, 15, bits, 0),	\
 	IIO_CHAN_SOFT_TIMESTAMP(16)			\
 }
-static struct iio_chan_spec max11602_channels[] = MAX1363_8X_CHANS(8);
-static struct iio_chan_spec max11608_channels[] = MAX1363_8X_CHANS(10);
-static struct iio_chan_spec max11614_channels[] = MAX1363_8X_CHANS(12);
+static const struct iio_chan_spec max11602_channels[] = MAX1363_8X_CHANS(8);
+static const struct iio_chan_spec max11608_channels[] = MAX1363_8X_CHANS(10);
+static const struct iio_chan_spec max11614_channels[] = MAX1363_8X_CHANS(12);
 
 static const enum max1363_modes max11644_mode_list[] = {
 	_s0, _s1, s0to1, d0m1, d1m0,
@@ -449,8 +449,8 @@ static const enum max1363_modes max11644_mode_list[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(4)			\
 	}
 
-static struct iio_chan_spec max11646_channels[] = MAX1363_2X_CHANS(10);
-static struct iio_chan_spec max11644_channels[] = MAX1363_2X_CHANS(12);
+static const struct iio_chan_spec max11646_channels[] = MAX1363_2X_CHANS(10);
+static const struct iio_chan_spec max11644_channels[] = MAX1363_2X_CHANS(12);
 
 enum { max1361,
        max1362,
@@ -1367,7 +1367,7 @@ error_out:
 	return ret;
 }
 
-static int max1363_remove(struct i2c_client *client)
+static int __devexit max1363_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct max1363_state *st = iio_priv(indio_dev);
@@ -1434,11 +1434,11 @@ static struct i2c_driver max1363_driver = {
 		.name = "max1363",
 	},
 	.probe = max1363_probe,
-	.remove = max1363_remove,
+	.remove = __devexit_p(max1363_remove),
 	.id_table = max1363_id,
 };
 module_i2c_driver(max1363_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Maxim 1363 ADC");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/max1363_ring.c b/drivers/staging/iio/adc/max1363_ring.c
index 774ae1b63550..5f74f3b7671a 100644
--- a/drivers/staging/iio/adc/max1363_ring.c
+++ b/drivers/staging/iio/adc/max1363_ring.c
@@ -80,7 +80,7 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 
 	if (indio_dev->scan_timestamp)
 		memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
-	iio_push_to_buffer(indio_dev->buffer, rxbuf, time_ns);
+	iio_push_to_buffer(indio_dev->buffer, rxbuf);
 
 done_free:
 	kfree(rxbuf);
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
new file mode 100644
index 000000000000..ca7c1fa88e71
--- /dev/null
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -0,0 +1,590 @@
+/*
+ * Freescale i.MX28 LRADC driver
+ *
+ * Copyright (c) 2012 DENX Software Engineering, GmbH.
+ * Marek Vasut <marex@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; 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/interrupt.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/sysfs.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/stmp_device.h>
+#include <linux/bitops.h>
+#include <linux/completion.h>
+
+#include <mach/mxs.h>
+#include <mach/common.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define DRIVER_NAME		"mxs-lradc"
+
+#define LRADC_MAX_DELAY_CHANS	4
+#define LRADC_MAX_MAPPED_CHANS	8
+#define LRADC_MAX_TOTAL_CHANS	16
+
+#define LRADC_DELAY_TIMER_HZ	2000
+
+/*
+ * Make this runtime configurable if necessary. Currently, if the buffered mode
+ * is enabled, the LRADC takes LRADC_DELAY_TIMER_LOOP samples of data before
+ * triggering IRQ. The sampling happens every (LRADC_DELAY_TIMER_PER / 2000)
+ * seconds. The result is that the samples arrive every 500mS.
+ */
+#define LRADC_DELAY_TIMER_PER	200
+#define LRADC_DELAY_TIMER_LOOP	5
+
+static const char * const mxs_lradc_irq_name[] = {
+	"mxs-lradc-touchscreen",
+	"mxs-lradc-thresh0",
+	"mxs-lradc-thresh1",
+	"mxs-lradc-channel0",
+	"mxs-lradc-channel1",
+	"mxs-lradc-channel2",
+	"mxs-lradc-channel3",
+	"mxs-lradc-channel4",
+	"mxs-lradc-channel5",
+	"mxs-lradc-channel6",
+	"mxs-lradc-channel7",
+	"mxs-lradc-button0",
+	"mxs-lradc-button1",
+};
+
+struct mxs_lradc_chan {
+	uint8_t				slot;
+	uint8_t				flags;
+};
+
+struct mxs_lradc {
+	struct device		*dev;
+	void __iomem		*base;
+	int			irq[13];
+
+	uint32_t		*buffer;
+	struct iio_trigger	*trig;
+
+	struct mutex		lock;
+
+	uint8_t			enable;
+
+	struct completion	completion;
+};
+
+#define	LRADC_CTRL0				0x00
+#define LRADC_CTRL0_TOUCH_DETECT_ENABLE		(1 << 23)
+#define LRADC_CTRL0_TOUCH_SCREEN_TYPE		(1 << 22)
+
+#define	LRADC_CTRL1				0x10
+#define	LRADC_CTRL1_LRADC_IRQ(n)		(1 << (n))
+#define	LRADC_CTRL1_LRADC_IRQ_MASK		0x1fff
+#define	LRADC_CTRL1_LRADC_IRQ_EN(n)		(1 << ((n) + 16))
+#define	LRADC_CTRL1_LRADC_IRQ_EN_MASK		(0x1fff << 16)
+
+#define	LRADC_CTRL2				0x20
+#define	LRADC_CTRL2_TEMPSENSE_PWD		(1 << 15)
+
+#define	LRADC_CH(n)				(0x50 + (0x10 * (n)))
+#define	LRADC_CH_ACCUMULATE			(1 << 29)
+#define	LRADC_CH_NUM_SAMPLES_MASK		(0x1f << 24)
+#define	LRADC_CH_NUM_SAMPLES_OFFSET		24
+#define	LRADC_CH_VALUE_MASK			0x3ffff
+#define	LRADC_CH_VALUE_OFFSET			0
+
+#define	LRADC_DELAY(n)				(0xd0 + (0x10 * (n)))
+#define	LRADC_DELAY_TRIGGER_LRADCS_MASK		(0xff << 24)
+#define	LRADC_DELAY_TRIGGER_LRADCS_OFFSET	24
+#define	LRADC_DELAY_KICK			(1 << 20)
+#define	LRADC_DELAY_TRIGGER_DELAYS_MASK		(0xf << 16)
+#define	LRADC_DELAY_TRIGGER_DELAYS_OFFSET	16
+#define	LRADC_DELAY_LOOP_COUNT_MASK		(0x1f << 11)
+#define	LRADC_DELAY_LOOP_COUNT_OFFSET		11
+#define	LRADC_DELAY_DELAY_MASK			0x7ff
+#define	LRADC_DELAY_DELAY_OFFSET		0
+
+#define	LRADC_CTRL4				0x140
+#define	LRADC_CTRL4_LRADCSELECT_MASK(n)		(0xf << ((n) * 4))
+#define	LRADC_CTRL4_LRADCSELECT_OFFSET(n)	((n) * 4)
+
+/*
+ * Raw I/O operations
+ */
+static int mxs_lradc_read_raw(struct iio_dev *iio_dev,
+			const struct iio_chan_spec *chan,
+			int *val, int *val2, long m)
+{
+	struct mxs_lradc *lradc = iio_priv(iio_dev);
+	int ret;
+
+	if (m != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	/* Check for invalid channel */
+	if (chan->channel > LRADC_MAX_TOTAL_CHANS)
+		return -EINVAL;
+
+	/*
+	 * See if there is no buffered operation in progess. If there is, simply
+	 * bail out. This can be improved to support both buffered and raw IO at
+	 * the same time, yet the code becomes horribly complicated. Therefore I
+	 * applied KISS principle here.
+	 */
+	ret = mutex_trylock(&lradc->lock);
+	if (!ret)
+		return -EBUSY;
+
+	INIT_COMPLETION(lradc->completion);
+
+	/*
+	 * No buffered operation in progress, map the channel and trigger it.
+	 * Virtual channel 0 is always used here as the others are always not
+	 * used if doing raw sampling.
+	 */
+	writel(LRADC_CTRL1_LRADC_IRQ_EN_MASK,
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+	writel(0xff, lradc->base + LRADC_CTRL0 + STMP_OFFSET_REG_CLR);
+
+	writel(chan->channel, lradc->base + LRADC_CTRL4);
+	writel(0, lradc->base + LRADC_CH(0));
+
+	/* Enable the IRQ and start sampling the channel. */
+	writel(LRADC_CTRL1_LRADC_IRQ_EN(0),
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_SET);
+	writel(1 << 0, lradc->base + LRADC_CTRL0 + STMP_OFFSET_REG_SET);
+
+	/* Wait for completion on the channel, 1 second max. */
+	ret = wait_for_completion_killable_timeout(&lradc->completion, HZ);
+	if (!ret)
+		ret = -ETIMEDOUT;
+	if (ret < 0)
+		goto err;
+
+	/* Read the data. */
+	*val = readl(lradc->base + LRADC_CH(0)) & LRADC_CH_VALUE_MASK;
+	ret = IIO_VAL_INT;
+
+err:
+	writel(LRADC_CTRL1_LRADC_IRQ_EN(0),
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+
+	mutex_unlock(&lradc->lock);
+
+	return ret;
+}
+
+static const struct iio_info mxs_lradc_iio_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= mxs_lradc_read_raw,
+};
+
+/*
+ * IRQ Handling
+ */
+static irqreturn_t mxs_lradc_handle_irq(int irq, void *data)
+{
+	struct iio_dev *iio = data;
+	struct mxs_lradc *lradc = iio_priv(iio);
+	unsigned long reg = readl(lradc->base + LRADC_CTRL1);
+
+	if (!(reg & LRADC_CTRL1_LRADC_IRQ_MASK))
+		return IRQ_NONE;
+
+	/*
+	 * Touchscreen IRQ handling code shall probably have priority
+	 * and therefore shall be placed here.
+	 */
+
+	if (iio_buffer_enabled(iio))
+		iio_trigger_poll(iio->trig, iio_get_time_ns());
+	else if (reg & LRADC_CTRL1_LRADC_IRQ(0))
+		complete(&lradc->completion);
+
+	writel(reg & LRADC_CTRL1_LRADC_IRQ_MASK,
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Trigger handling
+ */
+static irqreturn_t mxs_lradc_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *iio = pf->indio_dev;
+	struct mxs_lradc *lradc = iio_priv(iio);
+	struct iio_buffer *buffer = iio->buffer;
+	const uint32_t chan_value = LRADC_CH_ACCUMULATE |
+		((LRADC_DELAY_TIMER_LOOP - 1) << LRADC_CH_NUM_SAMPLES_OFFSET);
+	int i, j = 0;
+
+	for_each_set_bit(i, iio->active_scan_mask, iio->masklength) {
+		lradc->buffer[j] = readl(lradc->base + LRADC_CH(j));
+		writel(chan_value, lradc->base + LRADC_CH(j));
+		lradc->buffer[j] &= LRADC_CH_VALUE_MASK;
+		lradc->buffer[j] /= LRADC_DELAY_TIMER_LOOP;
+		j++;
+	}
+
+	if (iio->scan_timestamp) {
+		s64 *timestamp = (s64 *)((u8 *)lradc->buffer +
+					ALIGN(j, sizeof(s64)));
+		*timestamp = pf->timestamp;
+	}
+
+	iio_push_to_buffer(buffer, (u8 *)lradc->buffer);
+
+	iio_trigger_notify_done(iio->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int mxs_lradc_configure_trigger(struct iio_trigger *trig, bool state)
+{
+	struct iio_dev *iio = trig->private_data;
+	struct mxs_lradc *lradc = iio_priv(iio);
+	const uint32_t st = state ? STMP_OFFSET_REG_SET : STMP_OFFSET_REG_CLR;
+
+	writel(LRADC_DELAY_KICK, lradc->base + LRADC_DELAY(0) + st);
+
+	return 0;
+}
+
+static const struct iio_trigger_ops mxs_lradc_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = &mxs_lradc_configure_trigger,
+};
+
+static int mxs_lradc_trigger_init(struct iio_dev *iio)
+{
+	int ret;
+	struct iio_trigger *trig;
+
+	trig = iio_trigger_alloc("%s-dev%i", iio->name, iio->id);
+	if (trig == NULL)
+		return -ENOMEM;
+
+	trig->dev.parent = iio->dev.parent;
+	trig->private_data = iio;
+	trig->ops = &mxs_lradc_trigger_ops;
+
+	ret = iio_trigger_register(trig);
+	if (ret) {
+		iio_trigger_free(trig);
+		return ret;
+	}
+
+	iio->trig = trig;
+
+	return 0;
+}
+
+static void mxs_lradc_trigger_remove(struct iio_dev *iio)
+{
+	iio_trigger_unregister(iio->trig);
+	iio_trigger_free(iio->trig);
+}
+
+static int mxs_lradc_buffer_preenable(struct iio_dev *iio)
+{
+	struct mxs_lradc *lradc = iio_priv(iio);
+	struct iio_buffer *buffer = iio->buffer;
+	int ret = 0, chan, ofs = 0, enable = 0;
+	uint32_t ctrl4 = 0;
+	uint32_t ctrl1_irq = 0;
+	const uint32_t chan_value = LRADC_CH_ACCUMULATE |
+		((LRADC_DELAY_TIMER_LOOP - 1) << LRADC_CH_NUM_SAMPLES_OFFSET);
+	const int len = bitmap_weight(buffer->scan_mask, LRADC_MAX_TOTAL_CHANS);
+
+	if (!len)
+		return -EINVAL;
+
+	/*
+	 * Lock the driver so raw access can not be done during buffered
+	 * operation. This simplifies the code a lot.
+	 */
+	ret = mutex_trylock(&lradc->lock);
+	if (!ret)
+		return -EBUSY;
+
+	lradc->buffer = kmalloc(len * sizeof(*lradc->buffer), GFP_KERNEL);
+	if (!lradc->buffer) {
+		ret = -ENOMEM;
+		goto err_mem;
+	}
+
+	ret = iio_sw_buffer_preenable(iio);
+	if (ret < 0)
+		goto err_buf;
+
+	writel(LRADC_CTRL1_LRADC_IRQ_EN_MASK,
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+	writel(0xff, lradc->base + LRADC_CTRL0 + STMP_OFFSET_REG_CLR);
+
+	for_each_set_bit(chan, buffer->scan_mask, LRADC_MAX_TOTAL_CHANS) {
+		ctrl4 |= chan << LRADC_CTRL4_LRADCSELECT_OFFSET(ofs);
+		ctrl1_irq |= LRADC_CTRL1_LRADC_IRQ_EN(ofs);
+		writel(chan_value, lradc->base + LRADC_CH(ofs));
+		enable |= 1 << ofs;
+		ofs++;
+	};
+
+	writel(LRADC_DELAY_TRIGGER_LRADCS_MASK | LRADC_DELAY_KICK,
+		lradc->base + LRADC_DELAY(0) + STMP_OFFSET_REG_CLR);
+
+	writel(ctrl4, lradc->base + LRADC_CTRL4);
+	writel(ctrl1_irq, lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_SET);
+
+	writel(enable << LRADC_DELAY_TRIGGER_LRADCS_OFFSET,
+		lradc->base + LRADC_DELAY(0) + STMP_OFFSET_REG_SET);
+
+	return 0;
+
+err_buf:
+	kfree(lradc->buffer);
+err_mem:
+	mutex_unlock(&lradc->lock);
+	return ret;
+}
+
+static int mxs_lradc_buffer_postdisable(struct iio_dev *iio)
+{
+	struct mxs_lradc *lradc = iio_priv(iio);
+
+	writel(LRADC_DELAY_TRIGGER_LRADCS_MASK | LRADC_DELAY_KICK,
+		lradc->base + LRADC_DELAY(0) + STMP_OFFSET_REG_CLR);
+
+	writel(0xff, lradc->base + LRADC_CTRL0 + STMP_OFFSET_REG_CLR);
+	writel(LRADC_CTRL1_LRADC_IRQ_EN_MASK,
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+
+	kfree(lradc->buffer);
+	mutex_unlock(&lradc->lock);
+
+	return 0;
+}
+
+static bool mxs_lradc_validate_scan_mask(struct iio_dev *iio,
+					const unsigned long *mask)
+{
+	const int mw = bitmap_weight(mask, iio->masklength);
+
+	return mw <= LRADC_MAX_MAPPED_CHANS;
+}
+
+static const struct iio_buffer_setup_ops mxs_lradc_buffer_ops = {
+	.preenable = &mxs_lradc_buffer_preenable,
+	.postenable = &iio_triggered_buffer_postenable,
+	.predisable = &iio_triggered_buffer_predisable,
+	.postdisable = &mxs_lradc_buffer_postdisable,
+	.validate_scan_mask = &mxs_lradc_validate_scan_mask,
+};
+
+/*
+ * Driver initialization
+ */
+
+#define MXS_ADC_CHAN(idx, chan_type) {				\
+	.type = (chan_type),					\
+	.indexed = 1,						\
+	.scan_index = (idx),					\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,		\
+	.channel = (idx),					\
+	.scan_type = {						\
+		.sign = 'u',					\
+		.realbits = 18,					\
+		.storagebits = 32,				\
+	},							\
+}
+
+static const struct iio_chan_spec mxs_lradc_chan_spec[] = {
+	MXS_ADC_CHAN(0, IIO_VOLTAGE),
+	MXS_ADC_CHAN(1, IIO_VOLTAGE),
+	MXS_ADC_CHAN(2, IIO_VOLTAGE),
+	MXS_ADC_CHAN(3, IIO_VOLTAGE),
+	MXS_ADC_CHAN(4, IIO_VOLTAGE),
+	MXS_ADC_CHAN(5, IIO_VOLTAGE),
+	MXS_ADC_CHAN(6, IIO_VOLTAGE),
+	MXS_ADC_CHAN(7, IIO_VOLTAGE),	/* VBATT */
+	MXS_ADC_CHAN(8, IIO_TEMP),	/* Temp sense 0 */
+	MXS_ADC_CHAN(9, IIO_TEMP),	/* Temp sense 1 */
+	MXS_ADC_CHAN(10, IIO_VOLTAGE),	/* VDDIO */
+	MXS_ADC_CHAN(11, IIO_VOLTAGE),	/* VTH */
+	MXS_ADC_CHAN(12, IIO_VOLTAGE),	/* VDDA */
+	MXS_ADC_CHAN(13, IIO_VOLTAGE),	/* VDDD */
+	MXS_ADC_CHAN(14, IIO_VOLTAGE),	/* VBG */
+	MXS_ADC_CHAN(15, IIO_VOLTAGE),	/* VDD5V */
+};
+
+static void mxs_lradc_hw_init(struct mxs_lradc *lradc)
+{
+	int i;
+	const uint32_t cfg =
+		(LRADC_DELAY_TIMER_PER << LRADC_DELAY_DELAY_OFFSET);
+
+	stmp_reset_block(lradc->base);
+
+	for (i = 0; i < LRADC_MAX_DELAY_CHANS; i++)
+		writel(cfg | (1 << (LRADC_DELAY_TRIGGER_DELAYS_OFFSET + i)),
+			lradc->base + LRADC_DELAY(i));
+
+	/* Start internal temperature sensing. */
+	writel(0, lradc->base + LRADC_CTRL2);
+}
+
+static void mxs_lradc_hw_stop(struct mxs_lradc *lradc)
+{
+	int i;
+
+	writel(LRADC_CTRL1_LRADC_IRQ_EN_MASK,
+		lradc->base + LRADC_CTRL1 + STMP_OFFSET_REG_CLR);
+
+	for (i = 0; i < LRADC_MAX_DELAY_CHANS; i++)
+		writel(0, lradc->base + LRADC_DELAY(i));
+}
+
+static int __devinit mxs_lradc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mxs_lradc *lradc;
+	struct iio_dev *iio;
+	struct resource *iores;
+	int ret = 0;
+	int i;
+
+	/* Allocate the IIO device. */
+	iio = iio_device_alloc(sizeof(*lradc));
+	if (!iio) {
+		dev_err(dev, "Failed to allocate IIO device\n");
+		return -ENOMEM;
+	}
+
+	lradc = iio_priv(iio);
+
+	/* Grab the memory area */
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	lradc->dev = &pdev->dev;
+	lradc->base = devm_request_and_ioremap(dev, iores);
+	if (!lradc->base) {
+		ret = -EADDRNOTAVAIL;
+		goto err_addr;
+	}
+
+	/* Grab all IRQ sources */
+	for (i = 0; i < 13; i++) {
+		lradc->irq[i] = platform_get_irq(pdev, i);
+		if (lradc->irq[i] < 0) {
+			ret = -EINVAL;
+			goto err_addr;
+		}
+
+		ret = devm_request_irq(dev, lradc->irq[i],
+					mxs_lradc_handle_irq, 0,
+					mxs_lradc_irq_name[i], iio);
+		if (ret)
+			goto err_addr;
+	}
+
+	platform_set_drvdata(pdev, iio);
+
+	init_completion(&lradc->completion);
+	mutex_init(&lradc->lock);
+
+	iio->name = pdev->name;
+	iio->dev.parent = &pdev->dev;
+	iio->info = &mxs_lradc_iio_info;
+	iio->modes = INDIO_DIRECT_MODE;
+	iio->channels = mxs_lradc_chan_spec;
+	iio->num_channels = ARRAY_SIZE(mxs_lradc_chan_spec);
+
+	ret = iio_triggered_buffer_setup(iio, &iio_pollfunc_store_time,
+				&mxs_lradc_trigger_handler,
+				&mxs_lradc_buffer_ops);
+	if (ret)
+		goto err_addr;
+
+	ret = mxs_lradc_trigger_init(iio);
+	if (ret)
+		goto err_trig;
+
+	/* Register IIO device. */
+	ret = iio_device_register(iio);
+	if (ret) {
+		dev_err(dev, "Failed to register IIO device\n");
+		goto err_dev;
+	}
+
+	/* Configure the hardware. */
+	mxs_lradc_hw_init(lradc);
+
+	return 0;
+
+err_dev:
+	mxs_lradc_trigger_remove(iio);
+err_trig:
+	iio_triggered_buffer_cleanup(iio);
+err_addr:
+	iio_device_free(iio);
+	return ret;
+}
+
+static int __devexit mxs_lradc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *iio = platform_get_drvdata(pdev);
+	struct mxs_lradc *lradc = iio_priv(iio);
+
+	mxs_lradc_hw_stop(lradc);
+
+	iio_device_unregister(iio);
+	iio_triggered_buffer_cleanup(iio);
+	mxs_lradc_trigger_remove(iio);
+	iio_device_free(iio);
+
+	return 0;
+}
+
+static const struct of_device_id mxs_lradc_dt_ids[] = {
+	{ .compatible = "fsl,imx28-lradc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_lradc_dt_ids);
+
+static struct platform_driver mxs_lradc_driver = {
+	.driver	= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = mxs_lradc_dt_ids,
+	},
+	.probe	= mxs_lradc_probe,
+	.remove	= __devexit_p(mxs_lradc_remove),
+};
+
+module_platform_driver(mxs_lradc_driver);
+
+MODULE_AUTHOR("Marek Vasut <marex@denx.de>");
+MODULE_DESCRIPTION("Freescale i.MX28 LRADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index 64d630e6fe29..0b83e2e1f410 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -189,7 +189,7 @@ static int spear_read_raw(struct iio_dev *indio_dev,
 	},						\
 }
 
-static struct iio_chan_spec spear_adc_iio_channels[] = {
+static const struct iio_chan_spec spear_adc_iio_channels[] = {
 	SPEAR_ADC_CHAN(0),
 	SPEAR_ADC_CHAN(1),
 	SPEAR_ADC_CHAN(2),
@@ -330,36 +330,30 @@ static int __devinit spear_adc_probe(struct platform_device *pdev)
 		goto errout3;
 	}
 
-	ret = clk_prepare(info->clk);
-	if (ret) {
-		dev_err(dev, "failed preparing clock\n");
-		goto errout4;
-	}
-
-	ret = clk_enable(info->clk);
+	ret = clk_prepare_enable(info->clk);
 	if (ret) {
 		dev_err(dev, "failed enabling clock\n");
-		goto errout5;
+		goto errout4;
 	}
 
 	irq = platform_get_irq(pdev, 0);
 	if ((irq < 0) || (irq >= NR_IRQS)) {
 		dev_err(dev, "failed getting interrupt resource\n");
 		ret = -EINVAL;
-		goto errout6;
+		goto errout5;
 	}
 
 	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, MOD_NAME, info);
 	if (ret < 0) {
 		dev_err(dev, "failed requesting interrupt\n");
-		goto errout6;
+		goto errout5;
 	}
 
 	if (of_property_read_u32(np, "sampling-frequency",
 				 &info->sampling_freq)) {
 		dev_err(dev, "sampling-frequency missing in DT\n");
 		ret = -EINVAL;
-		goto errout6;
+		goto errout5;
 	}
 
 	/*
@@ -389,16 +383,14 @@ static int __devinit spear_adc_probe(struct platform_device *pdev)
 
 	ret = iio_device_register(iodev);
 	if (ret)
-		goto errout6;
+		goto errout5;
 
 	dev_info(dev, "SPEAR ADC driver loaded, IRQ %d\n", irq);
 
 	return 0;
 
-errout6:
-	clk_disable(info->clk);
 errout5:
-	clk_unprepare(info->clk);
+	clk_disable_unprepare(info->clk);
 errout4:
 	clk_put(info->clk);
 errout3:
@@ -416,8 +408,7 @@ static int __devexit spear_adc_remove(struct platform_device *pdev)
 
 	iio_device_unregister(iodev);
 	platform_set_drvdata(pdev, NULL);
-	clk_disable(info->clk);
-	clk_unprepare(info->clk);
+	clk_disable_unprepare(info->clk);
 	clk_put(info->clk);
 	iounmap(info->adc_base_spear6xx);
 	iio_device_free(iodev);
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index 9931e2060e1f..87151a7cff04 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -184,7 +184,7 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int adis16060_r_remove(struct spi_device *spi)
+static int __devexit adis16060_r_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
 	iio_device_free(spi_get_drvdata(spi));
@@ -210,7 +210,7 @@ error_ret:
 	return ret;
 }
 
-static int adis16060_w_remove(struct spi_device *spi)
+static int __devexit adis16060_w_remove(struct spi_device *spi)
 {
 	return 0;
 }
diff --git a/drivers/staging/iio/gyro/adis16080_core.c b/drivers/staging/iio/gyro/adis16080_core.c
index 345e4fa778ba..a73902573f79 100644
--- a/drivers/staging/iio/gyro/adis16080_core.c
+++ b/drivers/staging/iio/gyro/adis16080_core.c
@@ -177,7 +177,7 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int adis16080_remove(struct spi_device *spi)
+static int __devexit adis16080_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
 	iio_device_free(spi_get_drvdata(spi));
diff --git a/drivers/staging/iio/gyro/adis16130_core.c b/drivers/staging/iio/gyro/adis16130_core.c
index bf61cd0b5bbc..fbf96b0b6ee8 100644
--- a/drivers/staging/iio/gyro/adis16130_core.c
+++ b/drivers/staging/iio/gyro/adis16130_core.c
@@ -154,7 +154,7 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int adis16130_remove(struct spi_device *spi)
+static int __devexit adis16130_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
 	iio_device_free(spi_get_drvdata(spi));
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index eb8e9d69efd3..9571c03aa4cc 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -700,24 +700,18 @@ error_ret:
 	return ret;
 }
 
-static int adis16260_remove(struct spi_device *spi)
+static int __devexit adis16260_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
 	iio_device_unregister(indio_dev);
-
-	ret = adis16260_stop_device(indio_dev);
-	if (ret)
-		goto err_ret;
-
+	adis16260_stop_device(indio_dev);
 	adis16260_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16260_unconfigure_ring(indio_dev);
 	iio_device_free(indio_dev);
 
-err_ret:
-	return ret;
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/iio/gyro/adis16260_ring.c b/drivers/staging/iio/gyro/adis16260_ring.c
index eeee8e760e6c..e294cb49736d 100644
--- a/drivers/staging/iio/gyro/adis16260_ring.c
+++ b/drivers/staging/iio/gyro/adis16260_ring.c
@@ -62,7 +62,6 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16260_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0;
 	s16 *data;
 
@@ -82,7 +81,7 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)data);
 
 	kfree(data);
 done:
diff --git a/drivers/staging/iio/gyro/adxrs450_core.c b/drivers/staging/iio/gyro/adxrs450_core.c
index 6513119b1e90..d93527d15917 100644
--- a/drivers/staging/iio/gyro/adxrs450_core.c
+++ b/drivers/staging/iio/gyro/adxrs450_core.c
@@ -409,7 +409,7 @@ error_ret:
 	return ret;
 }
 
-static int adxrs450_remove(struct spi_device *spi)
+static int __devexit adxrs450_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
 	iio_device_free(spi_get_drvdata(spi));
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index 0cd4fe916bf9..74e24e8aa876 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -216,6 +216,6 @@ static __exit void iio_dummy_evgen_exit(void)
 }
 module_exit(iio_dummy_evgen_exit);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("IIO dummy driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/iio_hwmon.c b/drivers/staging/iio/iio_hwmon.c
index 27d27ec9521f..5d491227e01b 100644
--- a/drivers/staging/iio/iio_hwmon.c
+++ b/drivers/staging/iio/iio_hwmon.c
@@ -42,40 +42,17 @@ static ssize_t iio_hwmon_read_val(struct device *dev,
 				  struct device_attribute *attr,
 				  char *buf)
 {
-	long result;
-	int val, ret, scaleint, scalepart;
+	int result;
+	int ret;
 	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
 	struct iio_hwmon_state *state = dev_get_drvdata(dev);
 
-	/*
-	 * No locking between this pair, so theoretically possible
-	 * the scale has changed.
-	 */
-	ret = iio_read_channel_raw(&state->channels[sattr->index],
-				      &val);
+	ret = iio_read_channel_processed(&state->channels[sattr->index],
+					&result);
 	if (ret < 0)
 		return ret;
 
-	ret = iio_read_channel_scale(&state->channels[sattr->index],
-					&scaleint, &scalepart);
-	if (ret < 0)
-		return ret;
-	switch (ret) {
-	case IIO_VAL_INT:
-		result = val * scaleint;
-		break;
-	case IIO_VAL_INT_PLUS_MICRO:
-		result = (s64)val * (s64)scaleint +
-			div_s64((s64)val * (s64)scalepart, 1000000LL);
-		break;
-	case IIO_VAL_INT_PLUS_NANO:
-		result = (s64)val * (s64)scaleint +
-			div_s64((s64)val * (s64)scalepart, 1000000000LL);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return sprintf(buf, "%ld\n", result);
+	return sprintf(buf, "%d\n", result);
 }
 
 static void iio_hwmon_free_attrs(struct iio_hwmon_state *st)
@@ -215,18 +192,8 @@ static struct platform_driver __refdata iio_hwmon_driver = {
 	.remove = __devexit_p(iio_hwmon_remove),
 };
 
-static int iio_inkern_init(void)
-{
-	return platform_driver_register(&iio_hwmon_driver);
-}
-module_init(iio_inkern_init);
-
-static void iio_inkern_exit(void)
-{
-	platform_driver_unregister(&iio_hwmon_driver);
-}
-module_exit(iio_inkern_exit);
+module_platform_driver(iio_hwmon_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("IIO to hwmon driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index 155a49a9da7e..dc6c728ea47a 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -63,7 +63,7 @@ static const struct iio_dummy_accel_calibscale dummy_scales[] = {
  * This array of structures tells the IIO core about what the device
  * actually provides for a given channel.
  */
-static struct iio_chan_spec iio_dummy_channels[] = {
+static const struct iio_chan_spec iio_dummy_channels[] = {
 	/* indexed ADC channel in_voltage0_raw etc */
 	{
 		.type = IIO_VOLTAGE,
@@ -445,26 +445,20 @@ static int __devinit iio_dummy_probe(int index)
 	if (ret < 0)
 		goto error_free_device;
 
-	/* Configure buffered capture support. */
-	ret = iio_simple_dummy_configure_buffer(indio_dev);
-	if (ret < 0)
-		goto error_unregister_events;
-
 	/*
-	 * Register the channels with the buffer, but avoid the output
-	 * channel being registered by reducing the number of channels by 1.
+	 * Configure buffered capture support and register the channels with the
+	 * buffer, but avoid the output channel being registered by reducing the
+	 * number of channels by 1.
 	 */
-	ret = iio_buffer_register(indio_dev, iio_dummy_channels, 5);
+	ret = iio_simple_dummy_configure_buffer(indio_dev, iio_dummy_channels, 5);
 	if (ret < 0)
-		goto error_unconfigure_buffer;
+		goto error_unregister_events;
 
 	ret = iio_device_register(indio_dev);
 	if (ret < 0)
-		goto error_unregister_buffer;
+		goto error_unconfigure_buffer;
 
 	return 0;
-error_unregister_buffer:
-	iio_buffer_unregister(indio_dev);
 error_unconfigure_buffer:
 	iio_simple_dummy_unconfigure_buffer(indio_dev);
 error_unregister_events:
@@ -499,7 +493,6 @@ static int iio_dummy_remove(int index)
 	/* Device specific code to power down etc */
 
 	/* Buffered capture related cleanup */
-	iio_buffer_unregister(indio_dev);
 	iio_simple_dummy_unconfigure_buffer(indio_dev);
 
 	ret = iio_simple_dummy_events_unregister(indio_dev);
@@ -530,6 +523,7 @@ static __init int iio_dummy_init(void)
 		instances = 1;
 		return -EINVAL;
 	}
+
 	/* Fake a bus */
 	iio_dummy_devs = kcalloc(instances, sizeof(*iio_dummy_devs),
 				 GFP_KERNEL);
@@ -558,6 +552,6 @@ static __exit void iio_dummy_exit(void)
 }
 module_exit(iio_dummy_exit);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("IIO dummy driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/iio_simple_dummy.h b/drivers/staging/iio/iio_simple_dummy.h
index 53975d916fc9..c9e8702caca4 100644
--- a/drivers/staging/iio/iio_simple_dummy.h
+++ b/drivers/staging/iio/iio_simple_dummy.h
@@ -95,10 +95,12 @@ enum iio_simple_dummy_scan_elements {
 };
 
 #ifdef CONFIG_IIO_SIMPLE_DUMMY_BUFFER
-int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev);
+int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *channels, unsigned int num_channels);
 void iio_simple_dummy_unconfigure_buffer(struct iio_dev *indio_dev);
 #else
-static inline int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
+static inline int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *channels, unsigned int num_channels)
 {
 	return 0;
 };
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index bd628de472a9..697d9700db2f 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -87,7 +87,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		*(s64 *)((u8 *)data + ALIGN(len, sizeof(s64)))
 			= iio_get_time_ns();
-	buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
+	iio_push_to_buffer(buffer, (u8 *)data);
 
 	kfree(data);
 
@@ -126,7 +126,8 @@ static const struct iio_buffer_setup_ops iio_simple_dummy_buffer_setup_ops = {
 	.predisable = &iio_triggered_buffer_predisable,
 };
 
-int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
+int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *channels, unsigned int num_channels)
 {
 	int ret;
 	struct iio_buffer *buffer;
@@ -182,8 +183,15 @@ int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
 	 * driven by a trigger.
 	 */
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
+
+	ret = iio_buffer_register(indio_dev, channels, num_channels);
+	if (ret)
+		goto error_dealloc_pollfunc;
+
 	return 0;
 
+error_dealloc_pollfunc:
+	iio_dealloc_pollfunc(indio_dev->pollfunc);
 error_free_buffer:
 	iio_kfifo_free(indio_dev->buffer);
 error_ret:
@@ -197,6 +205,7 @@ error_ret:
  */
 void iio_simple_dummy_unconfigure_buffer(struct iio_dev *indio_dev)
 {
+	iio_buffer_unregister(indio_dev);
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
 	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index a8e51bc04439..de21d47f33e9 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -108,7 +108,7 @@ static struct ad5933_platform_data ad5933_default_pdata  = {
 	.vref_mv = 3300,
 };
 
-static struct iio_chan_spec ad5933_channels[] = {
+static const struct iio_chan_spec ad5933_channels[] = {
 	{
 		.type = IIO_TEMP,
 		.indexed = 1,
@@ -678,7 +678,7 @@ static void ad5933_work(struct work_struct *work)
 			buf[0] = be16_to_cpu(buf[0]);
 		}
 		/* save datum to the ring */
-		ring->access->store_to(ring, (u8 *)buf, iio_get_time_ns());
+		iio_push_to_buffer(ring, (u8 *)buf);
 	} else {
 		/* no data available - try again later */
 		schedule_delayed_work(&st->work, st->poll_time_jiffies);
diff --git a/drivers/staging/iio/imu/adis16400.h b/drivers/staging/iio/imu/adis16400.h
index 9dd9f1459a4d..d59d7ac856a9 100644
--- a/drivers/staging/iio/imu/adis16400.h
+++ b/drivers/staging/iio/imu/adis16400.h
@@ -5,7 +5,7 @@
  *		3d 2.5gauss magnetometers via SPI
  *
  * Copyright (c) 2009 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
- * Copyright (c) 2007 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org>
  *
  * Loosely based upon lis3l02dq.h
  *
diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c
index a618327e06ed..b302c9ba2712 100644
--- a/drivers/staging/iio/imu/adis16400_core.c
+++ b/drivers/staging/iio/imu/adis16400_core.c
@@ -5,7 +5,7 @@
  *		3d Magnetometers via SPI
  *
  * Copyright (c) 2009 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
- * Copyright (c) 2007 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org>
  * Copyright (c) 2011 Analog Devices Inc.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -612,7 +612,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	}
 }
 
-static struct iio_chan_spec adis16400_channels[] = {
+static const struct iio_chan_spec adis16400_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -742,7 +742,7 @@ static struct iio_chan_spec adis16400_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(12)
 };
 
-static struct iio_chan_spec adis16350_channels[] = {
+static const struct iio_chan_spec adis16350_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -867,7 +867,7 @@ static struct iio_chan_spec adis16350_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(11)
 };
 
-static struct iio_chan_spec adis16300_channels[] = {
+static const struct iio_chan_spec adis16300_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -1206,15 +1206,12 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int adis16400_remove(struct spi_device *spi)
+static int __devexit adis16400_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev =  spi_get_drvdata(spi);
 
 	iio_device_unregister(indio_dev);
-	ret = adis16400_stop_device(indio_dev);
-	if (ret)
-		goto err_ret;
+	adis16400_stop_device(indio_dev);
 
 	adis16400_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
@@ -1222,9 +1219,6 @@ static int adis16400_remove(struct spi_device *spi)
 	iio_device_free(indio_dev);
 
 	return 0;
-
-err_ret:
-	return ret;
 }
 
 static const struct spi_device_id adis16400_id[] = {
diff --git a/drivers/staging/iio/imu/adis16400_ring.c b/drivers/staging/iio/imu/adis16400_ring.c
index beec650ddbdb..260bdd1a4681 100644
--- a/drivers/staging/iio/imu/adis16400_ring.c
+++ b/drivers/staging/iio/imu/adis16400_ring.c
@@ -150,7 +150,7 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 	/* Guaranteed to be aligned with 8 byte boundary */
 	if (ring->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
-	ring->access->store_to(indio_dev->buffer, (u8 *) data, pf->timestamp);
+	iio_push_to_buffer(ring, (u8 *) data);
 
 done:
 	kfree(data);
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index 31d22f5591ca..6ee5567d9813 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -63,6 +63,7 @@ struct isl29018_chip {
 	struct regmap		*regmap;
 	struct mutex		lock;
 	unsigned int		lux_scale;
+	unsigned int		lux_uscale;
 	unsigned int		range;
 	unsigned int		adc_bit;
 	int			prox_scheme;
@@ -145,13 +146,22 @@ static int isl29018_read_sensor_input(struct isl29018_chip *chip, int mode)
 static int isl29018_read_lux(struct isl29018_chip *chip, int *lux)
 {
 	int lux_data;
+	unsigned int data_x_range, lux_unshifted;
 
 	lux_data = isl29018_read_sensor_input(chip, COMMMAND1_OPMODE_ALS_ONCE);
 
 	if (lux_data < 0)
 		return lux_data;
 
-	*lux = (lux_data * chip->range * chip->lux_scale) >> chip->adc_bit;
+	/* To support fractional scaling, separate the unshifted lux
+	 * into two calculations: int scaling and micro-scaling.
+	 * lux_uscale ranges from 0-999999, so about 20 bits.  Split
+	 * the /1,000,000 in two to reduce the risk of over/underflow.
+	 */
+	data_x_range = lux_data * chip->range;
+	lux_unshifted = data_x_range * chip->lux_scale;
+	lux_unshifted += data_x_range / 1000 * chip->lux_uscale / 1000;
+	*lux = lux_unshifted >> chip->adc_bit;
 
 	return 0;
 }
@@ -339,6 +349,8 @@ static int isl29018_write_raw(struct iio_dev *indio_dev,
 	mutex_lock(&chip->lock);
 	if (mask == IIO_CHAN_INFO_CALIBSCALE && chan->type == IIO_LIGHT) {
 		chip->lux_scale = val;
+		/* With no write_raw_get_fmt(), val2 is a MICRO fraction. */
+		chip->lux_uscale = val2;
 		ret = 0;
 	}
 	mutex_unlock(&chip->lock);
@@ -379,7 +391,8 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_CALIBSCALE:
 		if (chan->type == IIO_LIGHT) {
 			*val = chip->lux_scale;
-			ret = IIO_VAL_INT;
+			*val2 = chip->lux_uscale;
+			ret = IIO_VAL_INT_PLUS_MICRO;
 		}
 		break;
 	default:
diff --git a/drivers/staging/iio/light/tsl2563.c b/drivers/staging/iio/light/tsl2563.c
index 9d740be43a82..954ca2c172c6 100644
--- a/drivers/staging/iio/light/tsl2563.c
+++ b/drivers/staging/iio/light/tsl2563.c
@@ -805,7 +805,7 @@ fail1:
 	return err;
 }
 
-static int tsl2563_remove(struct i2c_client *client)
+static int __devexit tsl2563_remove(struct i2c_client *client)
 {
 	struct tsl2563_chip *chip = i2c_get_clientdata(client);
 	struct iio_dev *indio_dev = iio_priv_to_dev(chip);
diff --git a/drivers/staging/iio/magnetometer/hmc5843.c b/drivers/staging/iio/magnetometer/hmc5843.c
index 6c3e50f7c0d8..10e095486e54 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.c
+++ b/drivers/staging/iio/magnetometer/hmc5843.c
@@ -1,6 +1,6 @@
 /*  Copyright (C) 2010 Texas Instruments
     Author: Shubhrajyoti Datta <shubhrajyoti@ti.com>
-    Acknowledgement: Jonathan Cameron <jic23@cam.ac.uk> for valuable inputs.
+    Acknowledgement: Jonathan Cameron <jic23@kernel.org> for valuable inputs.
 
     Support for HMC5883 and HMC5883L by Peter Meerwald <pmeerw@pmeerw.net>.
 
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 3ccff189f258..8b9eceb66b37 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -555,20 +555,15 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int ade7753_remove(struct spi_device *spi)
+static int __devexit ade7753_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
 	iio_device_unregister(indio_dev);
-
-	ret = ade7753_stop_device(&(indio_dev->dev));
-	if (ret)
-		goto err_ret;
-
+	ade7753_stop_device(&indio_dev->dev);
 	iio_device_free(indio_dev);
-err_ret:
-	return ret;
+
+	return 0;
 }
 
 static struct spi_driver ade7753_driver = {
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index abb1e9c8d094..76e0adee96ea 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -577,21 +577,15 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int ade7754_remove(struct spi_device *spi)
+static int __devexit ade7754_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
 	iio_device_unregister(indio_dev);
-	ret = ade7754_stop_device(&(indio_dev->dev));
-	if (ret)
-		goto err_ret;
-
+	ade7754_stop_device(&indio_dev->dev);
 	iio_device_free(indio_dev);
 
-err_ret:
-	return ret;
-
+	return 0;
 }
 
 static struct spi_driver ade7754_driver = {
diff --git a/drivers/staging/iio/meter/ade7758.h b/drivers/staging/iio/meter/ade7758.h
index ec202b4ecfb2..1e11ad5ae5a4 100644
--- a/drivers/staging/iio/meter/ade7758.h
+++ b/drivers/staging/iio/meter/ade7758.h
@@ -122,7 +122,7 @@ struct ade7758_state {
 	u8			*tx;
 	u8			*rx;
 	struct mutex		buf_lock;
-	struct iio_chan_spec	*ade7758_ring_channels;
+	const struct iio_chan_spec *ade7758_ring_channels;
 	struct spi_transfer	ring_xfer[4];
 	struct spi_message	ring_msg;
 	/*
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 7014a0078446..a0fef77d8e5e 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -661,7 +661,7 @@ static const struct attribute_group ade7758_attribute_group = {
 	.attrs = ade7758_attributes,
 };
 
-static struct iio_chan_spec ade7758_channels[] = {
+static const struct iio_chan_spec ade7758_channels[] = {
 	{
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
@@ -962,17 +962,13 @@ error_ret:
 	return ret;
 }
 
-static int ade7758_remove(struct spi_device *spi)
+static int __devexit ade7758_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 	struct ade7758_state *st = iio_priv(indio_dev);
-	int ret;
 
 	iio_device_unregister(indio_dev);
-	ret = ade7758_stop_device(&indio_dev->dev);
-	if (ret)
-		goto err_ret;
-
+	ade7758_stop_device(&indio_dev->dev);
 	ade7758_remove_trigger(indio_dev);
 	ade7758_uninitialize_ring(indio_dev);
 	ade7758_unconfigure_ring(indio_dev);
@@ -981,8 +977,7 @@ static int ade7758_remove(struct spi_device *spi)
 
 	iio_device_free(indio_dev);
 
-err_ret:
-	return ret;
+	return 0;
 }
 
 static const struct spi_device_id ade7758_id[] = {
diff --git a/drivers/staging/iio/meter/ade7758_ring.c b/drivers/staging/iio/meter/ade7758_ring.c
index 1ce10b21f4d6..9e49baccf660 100644
--- a/drivers/staging/iio/meter/ade7758_ring.c
+++ b/drivers/staging/iio/meter/ade7758_ring.c
@@ -61,7 +61,6 @@ static irqreturn_t ade7758_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
-	struct iio_buffer *ring = indio_dev->buffer;
 	struct ade7758_state *st = iio_priv(indio_dev);
 	s64 dat64[2];
 	u32 *dat32 = (u32 *)dat64;
@@ -74,7 +73,7 @@ static irqreturn_t ade7758_trigger_handler(int irq, void *p)
 	if (indio_dev->scan_timestamp)
 		dat64[1] = pf->timestamp;
 
-	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
+	iio_push_to_buffer(indio_dev->buffer, (u8 *)dat64);
 
 	iio_trigger_notify_done(indio_dev->trig);
 
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index eb0a2a98f388..cb0707cbc347 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -499,20 +499,15 @@ error_ret:
 }
 
 /* fixme, confirm ordering in this function */
-static int ade7759_remove(struct spi_device *spi)
+static int __devexit ade7759_remove(struct spi_device *spi)
 {
-	int ret;
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
 	iio_device_unregister(indio_dev);
-	ret = ade7759_stop_device(&(indio_dev->dev));
-	if (ret)
-		goto err_ret;
-
+	ade7759_stop_device(&indio_dev->dev);
 	iio_device_free(indio_dev);
 
-err_ret:
-	return ret;
+	return 0;
 }
 
 static struct spi_driver ade7759_driver = {
diff --git a/drivers/staging/iio/meter/ade7854-spi.c b/drivers/staging/iio/meter/ade7854-spi.c
index 9fb2f8bfca81..7dae03573428 100644
--- a/drivers/staging/iio/meter/ade7854-spi.c
+++ b/drivers/staging/iio/meter/ade7854-spi.c
@@ -330,7 +330,7 @@ static int __devinit ade7854_spi_probe(struct spi_device *spi)
 	return 0;
 }
 
-static int ade7854_spi_remove(struct spi_device *spi)
+static int __devexit ade7854_spi_remove(struct spi_device *spi)
 {
 	ade7854_remove(spi_get_drvdata(spi));
 
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index f313859476c1..4ba4d05ed423 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -575,7 +575,7 @@ static IIO_DEVICE_ATTR(lot_low_thrd, S_IRUGO | S_IWUSR,
 		       AD2S1210_REG_LOT_LOW_THRD);
 
 
-static struct iio_chan_spec ad2s1210_channels[] = {
+static const struct iio_chan_spec ad2s1210_channels[] = {
 	{
 		.type = IIO_ANGL,
 		.indexed = 1,
diff --git a/drivers/staging/iio/ring_hw.h b/drivers/staging/iio/ring_hw.h
index cad8a2ed9b68..39c14a715868 100644
--- a/drivers/staging/iio/ring_hw.h
+++ b/drivers/staging/iio/ring_hw.h
@@ -5,7 +5,7 @@
  * under the terms of the GNU General Public License version 2 as published by
  * the Free Software Foundation.
  *
- * Copyright (c) 2009 Jonathan Cameron <jic23@cam.ac.uk>
+ * Copyright (c) 2009 Jonathan Cameron <jic23@kernel.org>
  *
  */
 
diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
index f61c8fdaab06..3a45f9a52de8 100644
--- a/drivers/staging/iio/ring_sw.c
+++ b/drivers/staging/iio/ring_sw.c
@@ -65,7 +65,7 @@ static inline void __iio_free_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
 /* Lock always held if their is a chance this may be called */
 /* Only one of these per ring may run concurrently - enforced by drivers */
 static int iio_store_to_sw_ring(struct iio_sw_ring_buffer *ring,
-				unsigned char *data, s64 timestamp)
+				unsigned char *data)
 {
 	int ret = 0;
 	unsigned char *temp_ptr, *change_test_ptr;
@@ -256,11 +256,10 @@ error_ret:
 }
 
 static int iio_store_to_sw_rb(struct iio_buffer *r,
-			      u8 *data,
-			      s64 timestamp)
+			      u8 *data)
 {
 	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
-	return iio_store_to_sw_ring(ring, data, timestamp);
+	return iio_store_to_sw_ring(ring, data);
 }
 
 static int iio_request_update_sw_rb(struct iio_buffer *r)
diff --git a/drivers/staging/iio/trigger/Kconfig b/drivers/staging/iio/trigger/Kconfig
index b8abf5473ddc..7d3207559265 100644
--- a/drivers/staging/iio/trigger/Kconfig
+++ b/drivers/staging/iio/trigger/Kconfig
@@ -21,6 +21,8 @@ config IIO_GPIO_TRIGGER
 config IIO_SYSFS_TRIGGER
 	tristate "SYSFS trigger"
 	depends on SYSFS
+	depends on HAVE_IRQ_WORK
+	select IRQ_WORK
 	help
 	  Provides support for using SYSFS entry as IIO triggers.
 	  If unsure, say N (but it's safe to say "Y").
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index ce6a7b1b8860..52062d786f84 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -14,14 +14,18 @@
 #include <linux/delay.h>
 
 #include <asm/gptimers.h>
+#include <asm/portmux.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
 
+#include "iio-trig-bfin-timer.h"
+
 struct bfin_timer {
 	unsigned short id, bit;
 	unsigned long irqbit;
 	int irq;
+	int pin;
 };
 
 /*
@@ -30,22 +34,22 @@ struct bfin_timer {
  */
 
 static struct bfin_timer iio_bfin_timer_code[MAX_BLACKFIN_GPTIMERS] = {
-	{TIMER0_id,  TIMER0bit,  TIMER_STATUS_TIMIL0,  IRQ_TIMER0},
-	{TIMER1_id,  TIMER1bit,  TIMER_STATUS_TIMIL1,  IRQ_TIMER1},
-	{TIMER2_id,  TIMER2bit,  TIMER_STATUS_TIMIL2,  IRQ_TIMER2},
+	{TIMER0_id,  TIMER0bit,  TIMER_STATUS_TIMIL0,  IRQ_TIMER0, P_TMR0},
+	{TIMER1_id,  TIMER1bit,  TIMER_STATUS_TIMIL1,  IRQ_TIMER1, P_TMR1},
+	{TIMER2_id,  TIMER2bit,  TIMER_STATUS_TIMIL2,  IRQ_TIMER2, P_TMR2},
 #if (MAX_BLACKFIN_GPTIMERS > 3)
-	{TIMER3_id,  TIMER3bit,  TIMER_STATUS_TIMIL3,  IRQ_TIMER3},
-	{TIMER4_id,  TIMER4bit,  TIMER_STATUS_TIMIL4,  IRQ_TIMER4},
-	{TIMER5_id,  TIMER5bit,  TIMER_STATUS_TIMIL5,  IRQ_TIMER5},
-	{TIMER6_id,  TIMER6bit,  TIMER_STATUS_TIMIL6,  IRQ_TIMER6},
-	{TIMER7_id,  TIMER7bit,  TIMER_STATUS_TIMIL7,  IRQ_TIMER7},
+	{TIMER3_id,  TIMER3bit,  TIMER_STATUS_TIMIL3,  IRQ_TIMER3, P_TMR3},
+	{TIMER4_id,  TIMER4bit,  TIMER_STATUS_TIMIL4,  IRQ_TIMER4, P_TMR4},
+	{TIMER5_id,  TIMER5bit,  TIMER_STATUS_TIMIL5,  IRQ_TIMER5, P_TMR5},
+	{TIMER6_id,  TIMER6bit,  TIMER_STATUS_TIMIL6,  IRQ_TIMER6, P_TMR6},
+	{TIMER7_id,  TIMER7bit,  TIMER_STATUS_TIMIL7,  IRQ_TIMER7, P_TMR7},
 #endif
 #if (MAX_BLACKFIN_GPTIMERS > 8)
-	{TIMER8_id,  TIMER8bit,  TIMER_STATUS_TIMIL8,  IRQ_TIMER8},
-	{TIMER9_id,  TIMER9bit,  TIMER_STATUS_TIMIL9,  IRQ_TIMER9},
-	{TIMER10_id, TIMER10bit, TIMER_STATUS_TIMIL10, IRQ_TIMER10},
+	{TIMER8_id,  TIMER8bit,  TIMER_STATUS_TIMIL8,  IRQ_TIMER8, P_TMR8},
+	{TIMER9_id,  TIMER9bit,  TIMER_STATUS_TIMIL9,  IRQ_TIMER9, P_TMR9},
+	{TIMER10_id, TIMER10bit, TIMER_STATUS_TIMIL10, IRQ_TIMER10, P_TMR10},
 #if (MAX_BLACKFIN_GPTIMERS > 11)
-	{TIMER11_id, TIMER11bit, TIMER_STATUS_TIMIL11, IRQ_TIMER11},
+	{TIMER11_id, TIMER11bit, TIMER_STATUS_TIMIL11, IRQ_TIMER11, P_TMR11},
 #endif
 #endif
 };
@@ -54,15 +58,33 @@ struct bfin_tmr_state {
 	struct iio_trigger *trig;
 	struct bfin_timer *t;
 	unsigned timer_num;
+	bool output_enable;
+	unsigned int duty;
 	int irq;
 };
 
+static int iio_bfin_tmr_set_state(struct iio_trigger *trig, bool state)
+{
+	struct bfin_tmr_state *st = trig->private_data;
+
+	if (get_gptimer_period(st->t->id) == 0)
+		return -EINVAL;
+
+	if (state)
+		enable_gptimers(st->t->bit);
+	else
+		disable_gptimers(st->t->bit);
+
+	return 0;
+}
+
 static ssize_t iio_bfin_tmr_frequency_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct bfin_tmr_state *st = trig->private_data;
-	long val;
+	unsigned long val;
+	bool enabled;
 	int ret;
 
 	ret = strict_strtoul(buf, 10, &val);
@@ -74,20 +96,25 @@ static ssize_t iio_bfin_tmr_frequency_store(struct device *dev,
 		goto error_ret;
 	}
 
-	disable_gptimers(st->t->bit);
+	enabled = get_enabled_gptimers() & st->t->bit;
+
+	if (enabled)
+		disable_gptimers(st->t->bit);
 
 	if (!val)
 		goto error_ret;
 
 	val = get_sclk() / val;
-	if (val <= 4) {
+	if (val <= 4 || val <= st->duty) {
 		ret = -EINVAL;
 		goto error_ret;
 	}
 
 	set_gptimer_period(st->t->id, val);
-	set_gptimer_pwidth(st->t->id, 1);
-	enable_gptimers(st->t->bit);
+	set_gptimer_pwidth(st->t->id, val - st->duty);
+
+	if (enabled)
+		enable_gptimers(st->t->bit);
 
 error_ret:
 	return ret ? ret : count;
@@ -99,9 +126,15 @@ static ssize_t iio_bfin_tmr_frequency_show(struct device *dev,
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct bfin_tmr_state *st = trig->private_data;
+	unsigned int period = get_gptimer_period(st->t->id);
+	unsigned long val;
+
+	if (period == 0)
+		val = 0;
+	else
+		val = get_sclk() / get_gptimer_period(st->t->id);
 
-	return sprintf(buf, "%lu\n",
-			get_sclk() / get_gptimer_period(st->t->id));
+	return sprintf(buf, "%lu\n", val);
 }
 
 static DEVICE_ATTR(frequency, S_IRUGO | S_IWUSR, iio_bfin_tmr_frequency_show,
@@ -121,7 +154,6 @@ static const struct attribute_group *iio_bfin_tmr_trigger_attr_groups[] = {
 	NULL
 };
 
-
 static irqreturn_t iio_bfin_tmr_trigger_isr(int irq, void *devid)
 {
 	struct bfin_tmr_state *st = devid;
@@ -145,11 +177,14 @@ static int iio_bfin_tmr_get_number(int irq)
 
 static const struct iio_trigger_ops iio_bfin_tmr_trigger_ops = {
 	.owner = THIS_MODULE,
+	.set_trigger_state = iio_bfin_tmr_set_state,
 };
 
 static int __devinit iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 {
+	struct iio_bfin_timer_trigger_pdata *pdata = pdev->dev.platform_data;
 	struct bfin_tmr_state *st;
+	unsigned int config;
 	int ret;
 
 	st = kzalloc(sizeof(*st), GFP_KERNEL);
@@ -193,13 +228,43 @@ static int __devinit iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 		goto out4;
 	}
 
-	set_gptimer_config(st->t->id, OUT_DIS | PWM_OUT | PERIOD_CNT | IRQ_ENA);
+	config = PWM_OUT | PERIOD_CNT | IRQ_ENA;
+
+	if (pdata && pdata->output_enable) {
+		unsigned long long val;
+
+		st->output_enable = true;
+
+		ret = peripheral_request(st->t->pin, st->trig->name);
+		if (ret)
+			goto out_free_irq;
+
+		val = (unsigned long long)get_sclk() * pdata->duty_ns;
+		do_div(val, NSEC_PER_SEC);
+		st->duty = val;
+
+		/**
+		 * The interrupt will be generated at the end of the period,
+		 * since we want the interrupt to be generated at end of the
+		 * pulse we invert both polarity and duty cycle, so that the
+		 * pulse will be generated directly before the interrupt.
+		 */
+		if (pdata->active_low)
+			config |= PULSE_HI;
+	} else {
+		st->duty = 1;
+		config |= OUT_DIS;
+	}
+
+	set_gptimer_config(st->t->id, config);
 
 	dev_info(&pdev->dev, "iio trigger Blackfin TMR%d, IRQ-%d",
 		 st->timer_num, st->irq);
 	platform_set_drvdata(pdev, st);
 
 	return 0;
+out_free_irq:
+	free_irq(st->irq, st);
 out4:
 	iio_trigger_unregister(st->trig);
 out2:
@@ -215,6 +280,8 @@ static int __devexit iio_bfin_tmr_trigger_remove(struct platform_device *pdev)
 	struct bfin_tmr_state *st = platform_get_drvdata(pdev);
 
 	disable_gptimers(st->t->bit);
+	if (st->output_enable)
+		peripheral_free(st->t->pin);
 	free_irq(st->irq, st);
 	iio_trigger_unregister(st->trig);
 	iio_trigger_put(st->trig);
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.h b/drivers/staging/iio/trigger/iio-trig-bfin-timer.h
new file mode 100644
index 000000000000..c07321f8d94c
--- /dev/null
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.h
@@ -0,0 +1,24 @@
+#ifndef __IIO_BFIN_TIMER_TRIGGER_H__
+#define __IIO_BFIN_TIMER_TRIGGER_H__
+
+/**
+ * struct iio_bfin_timer_trigger_pdata - timer trigger platform data
+ * @output_enable: Enable external trigger pulse generation.
+ * @active_low: Whether the trigger pulse is active low.
+ * @duty_ns: Length of the trigger pulse in nanoseconds.
+ *
+ * This struct is used to configure the output pulse generation of the blackfin
+ * timer trigger. If output_enable is set to true an external trigger signal
+ * will generated on the pin corresponding to the timer. This is useful for
+ * converters which needs an external signal to start conversion. active_low and
+ * duty_ns are used to configure the type of the trigger pulse. If output_enable
+ * is set to false no external trigger pulse will be generated and active_low
+ * and duty_ns are ignored.
+ **/
+struct iio_bfin_timer_trigger_pdata {
+	bool output_enable;
+	bool active_low;
+	unsigned int duty_ns;
+};
+
+#endif
diff --git a/drivers/staging/iio/trigger/iio-trig-gpio.c b/drivers/staging/iio/trigger/iio-trig-gpio.c
index 90b26846fc6b..5ff4d7fa20fa 100644
--- a/drivers/staging/iio/trigger/iio-trig-gpio.c
+++ b/drivers/staging/iio/trigger/iio-trig-gpio.c
@@ -51,7 +51,7 @@ static const struct iio_trigger_ops iio_gpio_trigger_ops = {
 	.owner = THIS_MODULE,
 };
 
-static int iio_gpio_trigger_probe(struct platform_device *pdev)
+static int __devinit iio_gpio_trigger_probe(struct platform_device *pdev)
 {
 	struct iio_gpio_trigger_info *trig_info;
 	struct iio_trigger *trig, *trig2;
@@ -130,7 +130,7 @@ error_free_completed_registrations:
 	return ret;
 }
 
-static int iio_gpio_trigger_remove(struct platform_device *pdev)
+static int __devexit iio_gpio_trigger_remove(struct platform_device *pdev)
 {
 	struct iio_trigger *trig, *trig2;
 	struct iio_gpio_trigger_info *trig_info;
@@ -153,7 +153,7 @@ static int iio_gpio_trigger_remove(struct platform_device *pdev)
 
 static struct platform_driver iio_gpio_trigger_driver = {
 	.probe = iio_gpio_trigger_probe,
-	.remove = iio_gpio_trigger_remove,
+	.remove = __devexit_p(iio_gpio_trigger_remove),
 	.driver = {
 		.name = "iio_gpio_trigger",
 		.owner = THIS_MODULE,
@@ -162,6 +162,6 @@ static struct platform_driver iio_gpio_trigger_driver = {
 
 module_platform_driver(iio_gpio_trigger_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Example gpio trigger for the iio subsystem");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 4ceaa18ef9f4..a3de76d70cdc 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -101,7 +101,7 @@ static const struct iio_trigger_ops iio_prtc_trigger_ops = {
 	.set_trigger_state = &iio_trig_periodic_rtc_set_state,
 };
 
-static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
+static int __devinit iio_trig_periodic_rtc_probe(struct platform_device *dev)
 {
 	char **pdata = dev->dev.platform_data;
 	struct iio_prtc_trigger_info *trig_info;
@@ -167,7 +167,7 @@ error_free_completed_registrations:
 	return ret;
 }
 
-static int iio_trig_periodic_rtc_remove(struct platform_device *dev)
+static int __devexit iio_trig_periodic_rtc_remove(struct platform_device *dev)
 {
 	struct iio_trigger *trig, *trig2;
 	struct iio_prtc_trigger_info *trig_info;
@@ -188,7 +188,7 @@ static int iio_trig_periodic_rtc_remove(struct platform_device *dev)
 
 static struct platform_driver iio_trig_periodic_rtc_driver = {
 	.probe = iio_trig_periodic_rtc_probe,
-	.remove = iio_trig_periodic_rtc_remove,
+	.remove = __devexit_p(iio_trig_periodic_rtc_remove),
 	.driver = {
 		.name = "iio_prtc_trigger",
 		.owner = THIS_MODULE,
@@ -197,6 +197,6 @@ static struct platform_driver iio_trig_periodic_rtc_driver = {
 
 module_platform_driver(iio_trig_periodic_rtc_driver);
 
-MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
+MODULE_AUTHOR("Jonathan Cameron <jic23@kernel.org>");
 MODULE_DESCRIPTION("Periodic realtime clock  trigger for the iio subsystem");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/trigger/iio-trig-sysfs.c b/drivers/staging/iio/trigger/iio-trig-sysfs.c
index fee474648108..3bac97224bf4 100644
--- a/drivers/staging/iio/trigger/iio-trig-sysfs.c
+++ b/drivers/staging/iio/trigger/iio-trig-sysfs.c
@@ -10,12 +10,14 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/list.h>
+#include <linux/irq_work.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
 
 struct iio_sysfs_trig {
 	struct iio_trigger *trig;
+	struct irq_work work;
 	int id;
 	struct list_head l;
 };
@@ -89,11 +91,21 @@ static struct device iio_sysfs_trig_dev = {
 	.release = &iio_trigger_sysfs_release,
 };
 
+static void iio_sysfs_trigger_work(struct irq_work *work)
+{
+	struct iio_sysfs_trig *trig = container_of(work, struct iio_sysfs_trig,
+							work);
+
+	iio_trigger_poll(trig->trig, 0);
+}
+
 static ssize_t iio_sysfs_trigger_poll(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
-	iio_trigger_poll_chained(trig, 0);
+	struct iio_sysfs_trig *sysfs_trig = trig->private_data;
+
+	irq_work_queue(&sysfs_trig->work);
 
 	return count;
 }
@@ -148,6 +160,9 @@ static int iio_sysfs_trigger_probe(int id)
 	t->trig->dev.groups = iio_sysfs_trigger_attr_groups;
 	t->trig->ops = &iio_sysfs_trigger_ops;
 	t->trig->dev.parent = &iio_sysfs_trig_dev;
+	t->trig->private_data = t;
+
+	init_irq_work(&t->work, iio_sysfs_trigger_work);
 
 	ret = iio_trigger_register(t->trig);
 	if (ret)
diff --git a/drivers/staging/imx-drm/Kconfig b/drivers/staging/imx-drm/Kconfig
new file mode 100644
index 000000000000..14b4449df234
--- /dev/null
+++ b/drivers/staging/imx-drm/Kconfig
@@ -0,0 +1,35 @@
+config DRM_IMX
+	tristate "DRM Support for Freescale i.MX"
+	select DRM_KMS_HELPER
+	select DRM_GEM_CMA_HELPER
+	select DRM_KMS_CMA_HELPER
+	depends on DRM && ARCH_MXC
+	help
+	  enable i.MX graphics support
+
+config DRM_IMX_FB_HELPER
+	tristate "provide legacy framebuffer /dev/fb0"
+	select DRM_KMS_CMA_HELPER
+	depends on DRM_IMX
+	help
+	  The DRM framework can provide a legacy /dev/fb0 framebuffer
+	  for your device. This is necessary to get a framebuffer console
+	  and also for appplications using the legacy framebuffer API
+
+config DRM_IMX_PARALLEL_DISPLAY
+	tristate "Support for parallel displays"
+	depends on DRM_IMX
+
+config DRM_IMX_IPUV3_CORE
+	tristate "IPUv3 core support"
+	depends on DRM_IMX
+	help
+	  Choose this if you have a i.MX5/6 system and want
+	  to use the IPU. This option only enables IPU base
+	  support.
+
+config DRM_IMX_IPUV3
+	tristate "DRM Support for i.MX IPUv3"
+	depends on DRM_IMX
+	help
+	  Choose this if you have a i.MX5 or i.MX6 processor.
diff --git a/drivers/staging/imx-drm/Makefile b/drivers/staging/imx-drm/Makefile
new file mode 100644
index 000000000000..83a9056546e6
--- /dev/null
+++ b/drivers/staging/imx-drm/Makefile
@@ -0,0 +1,9 @@
+
+imxdrm-objs := imx-drm-core.o imx-fb.o
+
+obj-$(CONFIG_DRM_IMX) += imxdrm.o
+
+obj-$(CONFIG_DRM_IMX_PARALLEL_DISPLAY) += parallel-display.o
+obj-$(CONFIG_DRM_IMX_FB_HELPER) += imx-fbdev.o
+obj-$(CONFIG_DRM_IMX_IPUV3_CORE) += ipu-v3/
+obj-$(CONFIG_DRM_IMX_IPUV3)	+= ipuv3-crtc.o
diff --git a/drivers/staging/imx-drm/TODO b/drivers/staging/imx-drm/TODO
new file mode 100644
index 000000000000..e52adc44e607
--- /dev/null
+++ b/drivers/staging/imx-drm/TODO
@@ -0,0 +1,22 @@
+TODO:
+- get DRM Maintainer review for this code
+- Factor out more code to common helper functions
+- decide where to put the base driver. It is not specific to a subsystem
+  and would be used by DRM/KMS and media/V4L2
+- convert irq driver to irq_domain_add_linear
+
+Missing features (not necessarily for moving out of staging):
+
+- Add KMS plane support for CRTC driver
+- Add LDB (LVDS Display Bridge) support
+- Add i.MX6 HDMI support
+- Add support for IC (Image converter)
+- Add support for CSI (CMOS Sensor interface)
+- Add support for VDIC (Video Deinterlacer)
+
+Many work-in-progress patches for the above features exist. Contact
+Sascha Hauer <kernel@pengutronix.de> if you are interested in working
+on a specific feature.
+
+Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org> and
+Sascha Hauer <kernel@pengutronix.de>
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
new file mode 100644
index 000000000000..1913199ba16e
--- /dev/null
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -0,0 +1,884 @@
+/*
+ * Freescale i.MX drm driver
+ *
+ * Copyright (C) 2011 Sascha Hauer, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <drm/drm_gem_cma_helper.h>
+#include <drm/drm_fb_cma_helper.h>
+
+#include "imx-drm.h"
+
+#define MAX_CRTC	4
+
+struct crtc_cookie {
+	void *cookie;
+	int id;
+	struct list_head list;
+};
+
+struct imx_drm_device {
+	struct drm_device			*drm;
+	struct device				*dev;
+	struct list_head			crtc_list;
+	struct list_head			encoder_list;
+	struct list_head			connector_list;
+	struct mutex				mutex;
+	int					references;
+	int					pipes;
+	struct drm_fbdev_cma			*fbhelper;
+};
+
+struct imx_drm_crtc {
+	struct drm_crtc				*crtc;
+	struct list_head			list;
+	struct imx_drm_device			*imxdrm;
+	int					pipe;
+	struct imx_drm_crtc_helper_funcs	imx_drm_helper_funcs;
+	struct module				*owner;
+	struct crtc_cookie			cookie;
+};
+
+struct imx_drm_encoder {
+	struct drm_encoder			*encoder;
+	struct list_head			list;
+	struct module				*owner;
+	struct list_head			possible_crtcs;
+};
+
+struct imx_drm_connector {
+	struct drm_connector			*connector;
+	struct list_head			list;
+	struct module				*owner;
+};
+
+static int imx_drm_driver_firstopen(struct drm_device *drm)
+{
+	if (!imx_drm_device_get())
+		return -EINVAL;
+
+	return 0;
+}
+
+static void imx_drm_driver_lastclose(struct drm_device *drm)
+{
+	struct imx_drm_device *imxdrm = drm->dev_private;
+
+	if (imxdrm->fbhelper)
+		drm_fbdev_cma_restore_mode(imxdrm->fbhelper);
+
+	imx_drm_device_put();
+}
+
+static int imx_drm_driver_unload(struct drm_device *drm)
+{
+	struct imx_drm_device *imxdrm = drm->dev_private;
+
+	drm_mode_config_cleanup(imxdrm->drm);
+	drm_kms_helper_poll_fini(imxdrm->drm);
+
+	return 0;
+}
+
+/*
+ * We don't care at all for crtc numbers, but the core expects the
+ * crtcs to be numbered
+ */
+static struct imx_drm_crtc *imx_drm_crtc_by_num(struct imx_drm_device *imxdrm,
+		int num)
+{
+	struct imx_drm_crtc *imx_drm_crtc;
+
+	list_for_each_entry(imx_drm_crtc, &imxdrm->crtc_list, list)
+		if (imx_drm_crtc->pipe == num)
+			return imx_drm_crtc;
+	return NULL;
+}
+
+int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
+		u32 interface_pix_fmt)
+{
+	struct imx_drm_device *imxdrm = crtc->dev->dev_private;
+	struct imx_drm_crtc *imx_crtc;
+	struct imx_drm_crtc_helper_funcs *helper;
+
+	mutex_lock(&imxdrm->mutex);
+
+	list_for_each_entry(imx_crtc, &imxdrm->crtc_list, list)
+		if (imx_crtc->crtc == crtc)
+			goto found;
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return -EINVAL;
+found:
+	mutex_unlock(&imxdrm->mutex);
+
+	helper = &imx_crtc->imx_drm_helper_funcs;
+	if (helper->set_interface_pix_fmt)
+		return helper->set_interface_pix_fmt(crtc,
+				encoder_type, interface_pix_fmt);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_crtc_panel_format);
+
+int imx_drm_crtc_vblank_get(struct imx_drm_crtc *imx_drm_crtc)
+{
+	return drm_vblank_get(imx_drm_crtc->imxdrm->drm, imx_drm_crtc->pipe);
+}
+EXPORT_SYMBOL_GPL(imx_drm_crtc_vblank_get);
+
+void imx_drm_crtc_vblank_put(struct imx_drm_crtc *imx_drm_crtc)
+{
+	drm_vblank_put(imx_drm_crtc->imxdrm->drm, imx_drm_crtc->pipe);
+}
+EXPORT_SYMBOL_GPL(imx_drm_crtc_vblank_put);
+
+void imx_drm_handle_vblank(struct imx_drm_crtc *imx_drm_crtc)
+{
+	drm_handle_vblank(imx_drm_crtc->imxdrm->drm, imx_drm_crtc->pipe);
+}
+EXPORT_SYMBOL_GPL(imx_drm_handle_vblank);
+
+static int imx_drm_enable_vblank(struct drm_device *drm, int crtc)
+{
+	struct imx_drm_device *imxdrm = drm->dev_private;
+	struct imx_drm_crtc *imx_drm_crtc;
+	int ret;
+
+	imx_drm_crtc = imx_drm_crtc_by_num(imxdrm, crtc);
+	if (!imx_drm_crtc)
+		return -EINVAL;
+
+	if (!imx_drm_crtc->imx_drm_helper_funcs.enable_vblank)
+		return -ENOSYS;
+
+	ret = imx_drm_crtc->imx_drm_helper_funcs.enable_vblank(
+			imx_drm_crtc->crtc);
+
+	return ret;
+}
+
+static void imx_drm_disable_vblank(struct drm_device *drm, int crtc)
+{
+	struct imx_drm_device *imxdrm = drm->dev_private;
+	struct imx_drm_crtc *imx_drm_crtc;
+
+	imx_drm_crtc = imx_drm_crtc_by_num(imxdrm, crtc);
+	if (!imx_drm_crtc)
+		return;
+
+	if (!imx_drm_crtc->imx_drm_helper_funcs.disable_vblank)
+		return;
+
+	imx_drm_crtc->imx_drm_helper_funcs.disable_vblank(imx_drm_crtc->crtc);
+}
+
+static const struct file_operations imx_drm_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_gem_cma_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+	.llseek = noop_llseek,
+};
+
+static struct imx_drm_device *imx_drm_device;
+
+static struct imx_drm_device *__imx_drm_device(void)
+{
+	return imx_drm_device;
+}
+
+struct drm_device *imx_drm_device_get(void)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_encoder *enc;
+	struct imx_drm_connector *con;
+	struct imx_drm_crtc *crtc;
+
+	mutex_lock(&imxdrm->mutex);
+
+	list_for_each_entry(enc, &imxdrm->encoder_list, list) {
+		if (!try_module_get(enc->owner)) {
+			dev_err(imxdrm->dev, "could not get module %s\n",
+					module_name(enc->owner));
+			goto unwind_enc;
+		}
+	}
+
+	list_for_each_entry(con, &imxdrm->connector_list, list) {
+		if (!try_module_get(con->owner)) {
+			dev_err(imxdrm->dev, "could not get module %s\n",
+					module_name(con->owner));
+			goto unwind_con;
+		}
+	}
+
+	list_for_each_entry(crtc, &imxdrm->crtc_list, list) {
+		if (!try_module_get(crtc->owner)) {
+			dev_err(imxdrm->dev, "could not get module %s\n",
+					module_name(crtc->owner));
+			goto unwind_crtc;
+		}
+	}
+
+	imxdrm->references++;
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return imxdrm->drm;
+
+unwind_crtc:
+	list_for_each_entry_continue_reverse(crtc, &imxdrm->crtc_list, list)
+		module_put(crtc->owner);
+unwind_con:
+	list_for_each_entry_continue_reverse(con, &imxdrm->connector_list, list)
+		module_put(con->owner);
+unwind_enc:
+	list_for_each_entry_continue_reverse(enc, &imxdrm->encoder_list, list)
+		module_put(enc->owner);
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return NULL;
+
+}
+EXPORT_SYMBOL_GPL(imx_drm_device_get);
+
+void imx_drm_device_put(void)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_encoder *enc;
+	struct imx_drm_connector *con;
+	struct imx_drm_crtc *crtc;
+
+	mutex_lock(&imxdrm->mutex);
+
+	list_for_each_entry(crtc, &imxdrm->crtc_list, list)
+		module_put(crtc->owner);
+
+	list_for_each_entry(con, &imxdrm->connector_list, list)
+		module_put(con->owner);
+
+	list_for_each_entry(enc, &imxdrm->encoder_list, list)
+		module_put(enc->owner);
+
+	imxdrm->references--;
+
+	mutex_unlock(&imxdrm->mutex);
+}
+EXPORT_SYMBOL_GPL(imx_drm_device_put);
+
+static int drm_mode_group_reinit(struct drm_device *dev)
+{
+	struct drm_mode_group *group = &dev->primary->mode_group;
+	uint32_t *id_list = group->id_list;
+	int ret;
+
+	ret = drm_mode_group_init_legacy_group(dev, group);
+	if (ret < 0)
+		return ret;
+
+	kfree(id_list);
+	return 0;
+}
+
+/*
+ * register an encoder to the drm core
+ */
+static int imx_drm_encoder_register(struct imx_drm_encoder *imx_drm_encoder)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	INIT_LIST_HEAD(&imx_drm_encoder->possible_crtcs);
+
+	drm_encoder_init(imxdrm->drm, imx_drm_encoder->encoder,
+			imx_drm_encoder->encoder->funcs,
+			imx_drm_encoder->encoder->encoder_type);
+
+	drm_mode_group_reinit(imxdrm->drm);
+
+	return 0;
+}
+
+/*
+ * unregister an encoder from the drm core
+ */
+static void imx_drm_encoder_unregister(struct imx_drm_encoder
+		*imx_drm_encoder)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	drm_encoder_cleanup(imx_drm_encoder->encoder);
+
+	drm_mode_group_reinit(imxdrm->drm);
+}
+
+/*
+ * register a connector to the drm core
+ */
+static int imx_drm_connector_register(
+		struct imx_drm_connector *imx_drm_connector)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	drm_connector_init(imxdrm->drm, imx_drm_connector->connector,
+			imx_drm_connector->connector->funcs,
+			imx_drm_connector->connector->connector_type);
+	drm_mode_group_reinit(imxdrm->drm);
+
+	return drm_sysfs_connector_add(imx_drm_connector->connector);
+}
+
+/*
+ * unregister a connector from the drm core
+ */
+static void imx_drm_connector_unregister(
+		struct imx_drm_connector *imx_drm_connector)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	drm_sysfs_connector_remove(imx_drm_connector->connector);
+	drm_connector_cleanup(imx_drm_connector->connector);
+
+	drm_mode_group_reinit(imxdrm->drm);
+}
+
+/*
+ * register a crtc to the drm core
+ */
+static int imx_drm_crtc_register(struct imx_drm_crtc *imx_drm_crtc)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	int ret;
+
+	drm_crtc_init(imxdrm->drm, imx_drm_crtc->crtc,
+			imx_drm_crtc->imx_drm_helper_funcs.crtc_funcs);
+	ret = drm_mode_crtc_set_gamma_size(imx_drm_crtc->crtc, 256);
+	if (ret)
+		return ret;
+
+	drm_crtc_helper_add(imx_drm_crtc->crtc,
+			imx_drm_crtc->imx_drm_helper_funcs.crtc_helper_funcs);
+
+	drm_mode_group_reinit(imxdrm->drm);
+
+	return 0;
+}
+
+/*
+ * Called by the CRTC driver when all CRTCs are registered. This
+ * puts all the pieces together and initializes the driver.
+ * Once this is called no more CRTCs can be registered since
+ * the drm core has hardcoded the number of crtcs in several
+ * places.
+ */
+static int imx_drm_driver_load(struct drm_device *drm, unsigned long flags)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	int ret;
+
+	imxdrm->drm = drm;
+
+	drm->dev_private = imxdrm;
+
+	/*
+	 * enable drm irq mode.
+	 * - with irq_enabled = 1, we can use the vblank feature.
+	 *
+	 * P.S. note that we wouldn't use drm irq handler but
+	 *      just specific driver own one instead because
+	 *      drm framework supports only one irq handler and
+	 *      drivers can well take care of their interrupts
+	 */
+	drm->irq_enabled = 1;
+
+	drm_mode_config_init(drm);
+	imx_drm_mode_config_init(drm);
+
+	mutex_lock(&imxdrm->mutex);
+
+	drm_kms_helper_poll_init(imxdrm->drm);
+
+	/* setup the grouping for the legacy output */
+	ret = drm_mode_group_init_legacy_group(imxdrm->drm,
+			&imxdrm->drm->primary->mode_group);
+	if (ret)
+		goto err_init;
+
+	ret = drm_vblank_init(imxdrm->drm, MAX_CRTC);
+	if (ret)
+		goto err_init;
+
+	/*
+	 * with vblank_disable_allowed = 1, vblank interrupt will be disabled
+	 * by drm timer once a current process gives up ownership of
+	 * vblank event.(after drm_vblank_put function is called)
+	 */
+	imxdrm->drm->vblank_disable_allowed = 1;
+
+	ret = 0;
+
+err_init:
+	mutex_unlock(&imxdrm->mutex);
+
+	return ret;
+}
+
+static void imx_drm_update_possible_crtcs(void)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_crtc *imx_drm_crtc;
+	struct imx_drm_encoder *enc;
+	struct crtc_cookie *cookie;
+
+	list_for_each_entry(enc, &imxdrm->encoder_list, list) {
+		u32 possible_crtcs = 0;
+
+		list_for_each_entry(cookie, &enc->possible_crtcs, list) {
+			list_for_each_entry(imx_drm_crtc, &imxdrm->crtc_list, list) {
+				if (imx_drm_crtc->cookie.cookie == cookie->cookie &&
+						imx_drm_crtc->cookie.id == cookie->id) {
+					possible_crtcs |= 1 << imx_drm_crtc->pipe;
+				}
+			}
+		}
+		enc->encoder->possible_crtcs = possible_crtcs;
+		enc->encoder->possible_clones = possible_crtcs;
+	}
+}
+
+/*
+ * imx_drm_add_crtc - add a new crtc
+ *
+ * The return value if !NULL is a cookie for the caller to pass to
+ * imx_drm_remove_crtc later.
+ */
+int imx_drm_add_crtc(struct drm_crtc *crtc,
+		struct imx_drm_crtc **new_crtc,
+		const struct imx_drm_crtc_helper_funcs *imx_drm_helper_funcs,
+		struct module *owner, void *cookie, int id)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_crtc *imx_drm_crtc;
+	const struct drm_crtc_funcs *crtc_funcs;
+	int ret;
+
+	mutex_lock(&imxdrm->mutex);
+
+	if (imxdrm->references) {
+		ret = -EBUSY;
+		goto err_busy;
+	}
+
+	imx_drm_crtc = kzalloc(sizeof(*imx_drm_crtc), GFP_KERNEL);
+	if (!imx_drm_crtc) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	imx_drm_crtc->imx_drm_helper_funcs = *imx_drm_helper_funcs;
+	imx_drm_crtc->pipe = imxdrm->pipes++;
+	imx_drm_crtc->cookie.cookie = cookie;
+	imx_drm_crtc->cookie.id = id;
+
+	crtc_funcs = imx_drm_helper_funcs->crtc_funcs;
+
+	imx_drm_crtc->crtc = crtc;
+	imx_drm_crtc->imxdrm = imxdrm;
+
+	imx_drm_crtc->owner = owner;
+
+	list_add_tail(&imx_drm_crtc->list, &imxdrm->crtc_list);
+
+	*new_crtc = imx_drm_crtc;
+
+	ret = imx_drm_crtc_register(imx_drm_crtc);
+	if (ret)
+		goto err_register;
+
+	imx_drm_update_possible_crtcs();
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return 0;
+
+err_register:
+	kfree(imx_drm_crtc);
+err_alloc:
+err_busy:
+	mutex_unlock(&imxdrm->mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_drm_add_crtc);
+
+/*
+ * imx_drm_remove_crtc - remove a crtc
+ */
+int imx_drm_remove_crtc(struct imx_drm_crtc *imx_drm_crtc)
+{
+	struct imx_drm_device *imxdrm = imx_drm_crtc->imxdrm;
+
+	mutex_lock(&imxdrm->mutex);
+
+	drm_crtc_cleanup(imx_drm_crtc->crtc);
+
+	list_del(&imx_drm_crtc->list);
+
+	drm_mode_group_reinit(imxdrm->drm);
+
+	mutex_unlock(&imxdrm->mutex);
+
+	kfree(imx_drm_crtc);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_remove_crtc);
+
+/*
+ * imx_drm_add_encoder - add a new encoder
+ */
+int imx_drm_add_encoder(struct drm_encoder *encoder,
+		struct imx_drm_encoder **newenc, struct module *owner)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_encoder *imx_drm_encoder;
+	int ret;
+
+	mutex_lock(&imxdrm->mutex);
+
+	if (imxdrm->references) {
+		ret = -EBUSY;
+		goto err_busy;
+	}
+
+	imx_drm_encoder = kzalloc(sizeof(*imx_drm_encoder), GFP_KERNEL);
+	if (!imx_drm_encoder) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	imx_drm_encoder->encoder = encoder;
+	imx_drm_encoder->owner = owner;
+
+	ret = imx_drm_encoder_register(imx_drm_encoder);
+	if (ret) {
+		kfree(imx_drm_encoder);
+		ret = -ENOMEM;
+		goto err_register;
+	}
+
+	list_add_tail(&imx_drm_encoder->list, &imxdrm->encoder_list);
+
+	*newenc = imx_drm_encoder;
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return 0;
+
+err_register:
+	kfree(imx_drm_encoder);
+err_alloc:
+err_busy:
+	mutex_unlock(&imxdrm->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_drm_add_encoder);
+
+int imx_drm_encoder_add_possible_crtcs(
+		struct imx_drm_encoder *imx_drm_encoder,
+		struct device_node *np)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct of_phandle_args args;
+	struct crtc_cookie *c;
+	int ret = 0;
+	int i;
+
+	if (!list_empty(&imx_drm_encoder->possible_crtcs))
+		return -EBUSY;
+
+	for (i = 0; !ret; i++) {
+		ret = of_parse_phandle_with_args(np, "crtcs",
+				"#crtc-cells", i, &args);
+		if (ret < 0)
+			break;
+
+		c = kzalloc(sizeof(*c), GFP_KERNEL);
+		if (!c) {
+			of_node_put(args.np);
+			return -ENOMEM;
+		}
+
+		c->cookie = args.np;
+		c->id = args.args_count > 0 ? args.args[0] : 0;
+
+		of_node_put(args.np);
+
+		mutex_lock(&imxdrm->mutex);
+
+		list_add_tail(&c->list, &imx_drm_encoder->possible_crtcs);
+
+		mutex_unlock(&imxdrm->mutex);
+	}
+
+	imx_drm_update_possible_crtcs();
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_encoder_add_possible_crtcs);
+
+int imx_drm_encoder_get_mux_id(struct imx_drm_encoder *imx_drm_encoder,
+		struct drm_crtc *crtc)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_crtc *imx_crtc;
+	int i = 0;
+
+	mutex_lock(&imxdrm->mutex);
+
+	list_for_each_entry(imx_crtc, &imxdrm->crtc_list, list) {
+		if (imx_crtc->crtc == crtc)
+			goto found;
+		i++;
+	}
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return -EINVAL;
+found:
+	mutex_unlock(&imxdrm->mutex);
+
+	return i;
+}
+
+/*
+ * imx_drm_remove_encoder - remove an encoder
+ */
+int imx_drm_remove_encoder(struct imx_drm_encoder *imx_drm_encoder)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct crtc_cookie *c, *tmp;
+
+	mutex_lock(&imxdrm->mutex);
+
+	imx_drm_encoder_unregister(imx_drm_encoder);
+
+	list_del(&imx_drm_encoder->list);
+
+	list_for_each_entry_safe(c, tmp, &imx_drm_encoder->possible_crtcs,
+			list)
+		kfree(c);
+
+	mutex_unlock(&imxdrm->mutex);
+
+	kfree(imx_drm_encoder);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_remove_encoder);
+
+/*
+ * imx_drm_add_connector - add a connector
+ */
+int imx_drm_add_connector(struct drm_connector *connector,
+		struct imx_drm_connector **new_con,
+		struct module *owner)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+	struct imx_drm_connector *imx_drm_connector;
+	int ret;
+
+	mutex_lock(&imxdrm->mutex);
+
+	if (imxdrm->references) {
+		ret = -EBUSY;
+		goto err_busy;
+	}
+
+	imx_drm_connector = kzalloc(sizeof(*imx_drm_connector), GFP_KERNEL);
+	if (!imx_drm_connector) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	imx_drm_connector->connector = connector;
+	imx_drm_connector->owner = owner;
+
+	ret = imx_drm_connector_register(imx_drm_connector);
+	if (ret)
+		goto err_register;
+
+	list_add_tail(&imx_drm_connector->list, &imxdrm->connector_list);
+
+	*new_con = imx_drm_connector;
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return 0;
+
+err_register:
+	kfree(imx_drm_connector);
+err_alloc:
+err_busy:
+	mutex_unlock(&imxdrm->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_drm_add_connector);
+
+void imx_drm_fb_helper_set(struct drm_fbdev_cma *fbdev_helper)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	imxdrm->fbhelper = fbdev_helper;
+}
+EXPORT_SYMBOL_GPL(imx_drm_fb_helper_set);
+
+/*
+ * imx_drm_remove_connector - remove a connector
+ */
+int imx_drm_remove_connector(struct imx_drm_connector *imx_drm_connector)
+{
+	struct imx_drm_device *imxdrm = __imx_drm_device();
+
+	mutex_lock(&imxdrm->mutex);
+
+	imx_drm_connector_unregister(imx_drm_connector);
+
+	list_del(&imx_drm_connector->list);
+
+	mutex_unlock(&imxdrm->mutex);
+
+	kfree(imx_drm_connector);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_remove_connector);
+
+static struct drm_ioctl_desc imx_drm_ioctls[] = {
+	/* none so far */
+};
+
+static struct drm_driver imx_drm_driver = {
+	.driver_features	= DRIVER_MODESET | DRIVER_GEM,
+	.load			= imx_drm_driver_load,
+	.unload			= imx_drm_driver_unload,
+	.firstopen		= imx_drm_driver_firstopen,
+	.lastclose		= imx_drm_driver_lastclose,
+	.gem_free_object	= drm_gem_cma_free_object,
+	.gem_vm_ops		= &drm_gem_cma_vm_ops,
+	.dumb_create		= drm_gem_cma_dumb_create,
+	.dumb_map_offset	= drm_gem_cma_dumb_map_offset,
+	.dumb_destroy		= drm_gem_cma_dumb_destroy,
+
+	.get_vblank_counter	= drm_vblank_count,
+	.enable_vblank		= imx_drm_enable_vblank,
+	.disable_vblank		= imx_drm_disable_vblank,
+	.ioctls			= imx_drm_ioctls,
+	.num_ioctls		= ARRAY_SIZE(imx_drm_ioctls),
+	.fops			= &imx_drm_driver_fops,
+	.name			= "imx-drm",
+	.desc			= "i.MX DRM graphics",
+	.date			= "20120507",
+	.major			= 1,
+	.minor			= 0,
+	.patchlevel		= 0,
+};
+
+static int imx_drm_platform_probe(struct platform_device *pdev)
+{
+	imx_drm_device->dev = &pdev->dev;
+
+	return drm_platform_init(&imx_drm_driver, pdev);
+}
+
+static int imx_drm_platform_remove(struct platform_device *pdev)
+{
+	drm_platform_exit(&imx_drm_driver, pdev);
+
+	return 0;
+}
+
+static struct platform_driver imx_drm_pdrv = {
+	.probe		= imx_drm_platform_probe,
+	.remove		= __devexit_p(imx_drm_platform_remove),
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "imx-drm",
+	},
+};
+
+static struct platform_device *imx_drm_pdev;
+
+static int __init imx_drm_init(void)
+{
+	int ret;
+
+	imx_drm_device = kzalloc(sizeof(*imx_drm_device), GFP_KERNEL);
+	if (!imx_drm_device)
+		return -ENOMEM;
+
+	mutex_init(&imx_drm_device->mutex);
+	INIT_LIST_HEAD(&imx_drm_device->crtc_list);
+	INIT_LIST_HEAD(&imx_drm_device->connector_list);
+	INIT_LIST_HEAD(&imx_drm_device->encoder_list);
+
+	imx_drm_pdev = platform_device_register_simple("imx-drm", -1, NULL, 0);
+	if (!imx_drm_pdev) {
+		ret = -EINVAL;
+		goto err_pdev;
+	}
+
+	imx_drm_pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32),
+
+	ret = platform_driver_register(&imx_drm_pdrv);
+	if (ret)
+		goto err_pdrv;
+
+	return 0;
+
+err_pdrv:
+	platform_device_unregister(imx_drm_pdev);
+err_pdev:
+	kfree(imx_drm_device);
+
+	return ret;
+}
+
+static void __exit imx_drm_exit(void)
+{
+	platform_device_unregister(imx_drm_pdev);
+	platform_driver_unregister(&imx_drm_pdrv);
+
+	kfree(imx_drm_device);
+}
+
+module_init(imx_drm_init);
+module_exit(imx_drm_exit);
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_DESCRIPTION("i.MX drm driver core");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/imx-drm/imx-drm.h b/drivers/staging/imx-drm/imx-drm.h
new file mode 100644
index 000000000000..ae28a490c445
--- /dev/null
+++ b/drivers/staging/imx-drm/imx-drm.h
@@ -0,0 +1,58 @@
+#ifndef _IMX_DRM_H_
+#define _IMX_DRM_H_
+
+struct imx_drm_crtc;
+struct drm_fbdev_cma;
+
+struct imx_drm_crtc_helper_funcs {
+	int (*enable_vblank)(struct drm_crtc *crtc);
+	void (*disable_vblank)(struct drm_crtc *crtc);
+	int (*set_interface_pix_fmt)(struct drm_crtc *crtc, u32 encoder_type,
+			u32 pix_fmt);
+	const struct drm_crtc_helper_funcs *crtc_helper_funcs;
+	const struct drm_crtc_funcs *crtc_funcs;
+};
+
+int imx_drm_add_crtc(struct drm_crtc *crtc,
+		struct imx_drm_crtc **new_crtc,
+		const struct imx_drm_crtc_helper_funcs *imx_helper_funcs,
+		struct module *owner, void *cookie, int id);
+int imx_drm_remove_crtc(struct imx_drm_crtc *);
+int imx_drm_init_drm(struct platform_device *pdev,
+		int preferred_bpp);
+int imx_drm_exit_drm(void);
+
+int imx_drm_crtc_vblank_get(struct imx_drm_crtc *imx_drm_crtc);
+void imx_drm_crtc_vblank_put(struct imx_drm_crtc *imx_drm_crtc);
+void imx_drm_handle_vblank(struct imx_drm_crtc *imx_drm_crtc);
+
+struct imx_drm_encoder;
+int imx_drm_add_encoder(struct drm_encoder *encoder,
+		struct imx_drm_encoder **new_enc,
+		struct module *owner);
+int imx_drm_remove_encoder(struct imx_drm_encoder *);
+
+struct imx_drm_connector;
+int imx_drm_add_connector(struct drm_connector *connector,
+		struct imx_drm_connector **new_con,
+		struct module *owner);
+int imx_drm_remove_connector(struct imx_drm_connector *);
+
+void imx_drm_mode_config_init(struct drm_device *drm);
+
+struct drm_gem_cma_object *imx_drm_fb_get_obj(struct drm_framebuffer *fb);
+
+struct drm_device *imx_drm_device_get(void);
+void imx_drm_device_put(void);
+int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
+		u32 interface_pix_fmt);
+void imx_drm_fb_helper_set(struct drm_fbdev_cma *fbdev_helper);
+
+struct device_node;
+
+int imx_drm_encoder_get_mux_id(struct imx_drm_encoder *imx_drm_encoder,
+		struct drm_crtc *crtc);
+int imx_drm_encoder_add_possible_crtcs(struct imx_drm_encoder *imx_drm_encoder,
+		struct device_node *np);
+
+#endif /* _IMX_DRM_H_ */
diff --git a/drivers/staging/imx-drm/imx-fb.c b/drivers/staging/imx-drm/imx-fb.c
new file mode 100644
index 000000000000..03a7b4e14f67
--- /dev/null
+++ b/drivers/staging/imx-drm/imx-fb.c
@@ -0,0 +1,47 @@
+/*
+ * i.MX drm driver
+ *
+ * Copyright (C) 2012 Sascha Hauer, Pengutronix
+ *
+ * Based on Samsung Exynos code
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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 <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_gem_cma_helper.h>
+#include <drm/drm_fb_cma_helper.h>
+
+#include "imx-drm.h"
+
+static struct drm_mode_config_funcs imx_drm_mode_config_funcs = {
+	.fb_create = drm_fb_cma_create,
+};
+
+void imx_drm_mode_config_init(struct drm_device *dev)
+{
+	dev->mode_config.min_width = 64;
+	dev->mode_config.min_height = 64;
+
+	/*
+	 * set max width and height as default value(4096x4096).
+	 * this value would be used to check framebuffer size limitation
+	 * at drm_mode_addfb().
+	 */
+	dev->mode_config.max_width = 4096;
+	dev->mode_config.max_height = 4096;
+
+	dev->mode_config.funcs = &imx_drm_mode_config_funcs;
+}
diff --git a/drivers/staging/imx-drm/imx-fbdev.c b/drivers/staging/imx-drm/imx-fbdev.c
new file mode 100644
index 000000000000..8331739c3d08
--- /dev/null
+++ b/drivers/staging/imx-drm/imx-fbdev.c
@@ -0,0 +1,74 @@
+/*
+ * i.MX drm driver
+ *
+ * Copyright (C) 2012 Sascha Hauer, Pengutronix
+ *
+ * Based on Samsung Exynos code
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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 <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_cma_helper.h>
+
+#include "imx-drm.h"
+
+#define MAX_CONNECTOR		4
+#define PREFERRED_BPP		16
+
+static struct drm_fbdev_cma *fbdev_cma;
+
+static int legacyfb_depth = 16;
+
+module_param(legacyfb_depth, int, 0444);
+
+static int __init imx_fb_helper_init(void)
+{
+	struct drm_device *drm = imx_drm_device_get();
+
+	if (!drm)
+		return -EINVAL;
+
+	if (legacyfb_depth != 16 && legacyfb_depth != 32) {
+		pr_warn("i.MX legacyfb: invalid legacyfb_depth setting. defaulting to 16bpp\n");
+		legacyfb_depth = 16;
+	}
+
+	fbdev_cma = drm_fbdev_cma_init(drm, legacyfb_depth,
+			drm->mode_config.num_crtc, MAX_CONNECTOR);
+
+	if (IS_ERR(fbdev_cma)) {
+		imx_drm_device_put();
+		return PTR_ERR(fbdev_cma);
+	}
+
+	imx_drm_fb_helper_set(fbdev_cma);
+
+	return 0;
+}
+
+static void __exit imx_fb_helper_exit(void)
+{
+	imx_drm_fb_helper_set(NULL);
+	drm_fbdev_cma_fini(fbdev_cma);
+	imx_drm_device_put();
+}
+
+late_initcall(imx_fb_helper_init);
+module_exit(imx_fb_helper_exit);
+
+MODULE_DESCRIPTION("Freescale i.MX legacy fb driver");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/imx-drm/ipu-v3/Makefile b/drivers/staging/imx-drm/ipu-v3/Makefile
new file mode 100644
index 000000000000..28ed72e98a96
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_DRM_IMX_IPUV3_CORE) += imx-ipu-v3.o
+
+imx-ipu-v3-objs := ipu-common.o ipu-dc.o ipu-di.o ipu-dp.o ipu-dmfc.o
diff --git a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
new file mode 100644
index 000000000000..74158dd73758
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
@@ -0,0 +1,318 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU Lesser General
+ * Public License.  You may obtain a copy of the GNU Lesser General
+ * Public License Version 2.1 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/lgpl-license.html
+ * http://www.gnu.org/copyleft/lgpl.html
+ */
+
+#ifndef __DRM_IPU_H__
+#define __DRM_IPU_H__
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/bitmap.h>
+#include <linux/fb.h>
+#include <linux/videodev2.h>
+
+struct ipu_soc;
+
+enum ipuv3_type {
+	IPUV3EX,
+	IPUV3M,
+	IPUV3H,
+};
+
+/*
+ * Bitfield of Display Interface signal polarities.
+ */
+struct ipu_di_signal_cfg {
+	unsigned datamask_en:1;
+	unsigned interlaced:1;
+	unsigned odd_field_first:1;
+	unsigned clksel_en:1;
+	unsigned clkidle_en:1;
+	unsigned data_pol:1;	/* true = inverted */
+	unsigned clk_pol:1;	/* true = rising edge */
+	unsigned enable_pol:1;
+	unsigned Hsync_pol:1;	/* true = active high */
+	unsigned Vsync_pol:1;
+
+	u16 width;
+	u16 height;
+	u32 pixel_fmt;
+	u16 h_start_width;
+	u16 h_sync_width;
+	u16 h_end_width;
+	u16 v_start_width;
+	u16 v_sync_width;
+	u16 v_end_width;
+	u32 v_to_h_sync;
+	unsigned long pixelclock;
+#define IPU_DI_CLKMODE_SYNC	(1 << 0)
+#define IPU_DI_CLKMODE_EXT	(1 << 1)
+	unsigned long clkflags;
+};
+
+enum ipu_color_space {
+	IPUV3_COLORSPACE_RGB,
+	IPUV3_COLORSPACE_YUV,
+	IPUV3_COLORSPACE_UNKNOWN,
+};
+
+struct ipuv3_channel;
+
+enum ipu_channel_irq {
+	IPU_IRQ_EOF = 0,
+	IPU_IRQ_NFACK = 64,
+	IPU_IRQ_NFB4EOF = 128,
+	IPU_IRQ_EOS = 192,
+};
+
+int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
+		enum ipu_channel_irq irq);
+
+#define IPU_IRQ_DP_SF_START		(448 + 2)
+#define IPU_IRQ_DP_SF_END		(448 + 3)
+#define IPU_IRQ_BG_SF_END		IPU_IRQ_DP_SF_END,
+#define IPU_IRQ_DC_FC_0			(448 + 8)
+#define IPU_IRQ_DC_FC_1			(448 + 9)
+#define IPU_IRQ_DC_FC_2			(448 + 10)
+#define IPU_IRQ_DC_FC_3			(448 + 11)
+#define IPU_IRQ_DC_FC_4			(448 + 12)
+#define IPU_IRQ_DC_FC_6			(448 + 13)
+#define IPU_IRQ_VSYNC_PRE_0		(448 + 14)
+#define IPU_IRQ_VSYNC_PRE_1		(448 + 15)
+
+/*
+ * IPU Image DMA Controller (idmac) functions
+ */
+struct ipuv3_channel *ipu_idmac_get(struct ipu_soc *ipu, unsigned channel);
+void ipu_idmac_put(struct ipuv3_channel *);
+
+int ipu_idmac_enable_channel(struct ipuv3_channel *channel);
+int ipu_idmac_disable_channel(struct ipuv3_channel *channel);
+
+void ipu_idmac_set_double_buffer(struct ipuv3_channel *channel,
+		bool doublebuffer);
+void ipu_idmac_select_buffer(struct ipuv3_channel *channel, u32 buf_num);
+
+/*
+ * IPU Display Controller (dc) functions
+ */
+struct ipu_dc;
+struct ipu_di;
+struct ipu_dc *ipu_dc_get(struct ipu_soc *ipu, int channel);
+void ipu_dc_put(struct ipu_dc *dc);
+int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
+		u32 pixel_fmt, u32 width);
+void ipu_dc_enable_channel(struct ipu_dc *dc);
+void ipu_dc_disable_channel(struct ipu_dc *dc);
+
+/*
+ * IPU Display Interface (di) functions
+ */
+struct ipu_di *ipu_di_get(struct ipu_soc *ipu, int disp);
+void ipu_di_put(struct ipu_di *);
+int ipu_di_disable(struct ipu_di *);
+int ipu_di_enable(struct ipu_di *);
+int ipu_di_get_num(struct ipu_di *);
+int ipu_di_init_sync_panel(struct ipu_di *, struct ipu_di_signal_cfg *sig);
+
+/*
+ * IPU Display Multi FIFO Controller (dmfc) functions
+ */
+struct dmfc_channel;
+int ipu_dmfc_enable_channel(struct dmfc_channel *dmfc);
+void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc);
+int ipu_dmfc_alloc_bandwidth(struct dmfc_channel *dmfc,
+		unsigned long bandwidth_mbs, int burstsize);
+void ipu_dmfc_free_bandwidth(struct dmfc_channel *dmfc);
+int ipu_dmfc_init_channel(struct dmfc_channel *dmfc, int width);
+struct dmfc_channel *ipu_dmfc_get(struct ipu_soc *ipu, int ipuv3_channel);
+void ipu_dmfc_put(struct dmfc_channel *dmfc);
+
+/*
+ * IPU Display Processor (dp) functions
+ */
+#define IPU_DP_FLOW_SYNC_BG	0
+#define IPU_DP_FLOW_SYNC_FG	1
+#define IPU_DP_FLOW_ASYNC0_BG	2
+#define IPU_DP_FLOW_ASYNC0_FG	3
+#define IPU_DP_FLOW_ASYNC1_BG	4
+#define IPU_DP_FLOW_ASYNC1_FG	5
+
+struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow);
+void ipu_dp_put(struct ipu_dp *);
+int ipu_dp_enable_channel(struct ipu_dp *dp);
+void ipu_dp_disable_channel(struct ipu_dp *dp);
+int ipu_dp_setup_channel(struct ipu_dp *dp,
+		enum ipu_color_space in, enum ipu_color_space out);
+int ipu_dp_set_window_pos(struct ipu_dp *, u16 x_pos, u16 y_pos);
+int ipu_dp_set_global_alpha(struct ipu_dp *dp, bool enable, u8 alpha,
+		bool bg_chan);
+
+#define IPU_CPMEM_WORD(word, ofs, size) ((((word) * 160 + (ofs)) << 8) | (size))
+
+#define IPU_FIELD_UBO		IPU_CPMEM_WORD(0, 46, 22)
+#define IPU_FIELD_VBO		IPU_CPMEM_WORD(0, 68, 22)
+#define IPU_FIELD_IOX		IPU_CPMEM_WORD(0, 90, 4)
+#define IPU_FIELD_RDRW		IPU_CPMEM_WORD(0, 94, 1)
+#define IPU_FIELD_SO		IPU_CPMEM_WORD(0, 113, 1)
+#define IPU_FIELD_SLY		IPU_CPMEM_WORD(1, 102, 14)
+#define IPU_FIELD_SLUV		IPU_CPMEM_WORD(1, 128, 14)
+
+#define IPU_FIELD_XV		IPU_CPMEM_WORD(0, 0, 10)
+#define IPU_FIELD_YV		IPU_CPMEM_WORD(0, 10, 9)
+#define IPU_FIELD_XB		IPU_CPMEM_WORD(0, 19, 13)
+#define IPU_FIELD_YB		IPU_CPMEM_WORD(0, 32, 12)
+#define IPU_FIELD_NSB_B		IPU_CPMEM_WORD(0, 44, 1)
+#define IPU_FIELD_CF		IPU_CPMEM_WORD(0, 45, 1)
+#define IPU_FIELD_SX		IPU_CPMEM_WORD(0, 46, 12)
+#define IPU_FIELD_SY		IPU_CPMEM_WORD(0, 58, 11)
+#define IPU_FIELD_NS		IPU_CPMEM_WORD(0, 69, 10)
+#define IPU_FIELD_SDX		IPU_CPMEM_WORD(0, 79, 7)
+#define IPU_FIELD_SM		IPU_CPMEM_WORD(0, 86, 10)
+#define IPU_FIELD_SCC		IPU_CPMEM_WORD(0, 96, 1)
+#define IPU_FIELD_SCE		IPU_CPMEM_WORD(0, 97, 1)
+#define IPU_FIELD_SDY		IPU_CPMEM_WORD(0, 98, 7)
+#define IPU_FIELD_SDRX		IPU_CPMEM_WORD(0, 105, 1)
+#define IPU_FIELD_SDRY		IPU_CPMEM_WORD(0, 106, 1)
+#define IPU_FIELD_BPP		IPU_CPMEM_WORD(0, 107, 3)
+#define IPU_FIELD_DEC_SEL	IPU_CPMEM_WORD(0, 110, 2)
+#define IPU_FIELD_DIM		IPU_CPMEM_WORD(0, 112, 1)
+#define IPU_FIELD_BNDM		IPU_CPMEM_WORD(0, 114, 3)
+#define IPU_FIELD_BM		IPU_CPMEM_WORD(0, 117, 2)
+#define IPU_FIELD_ROT		IPU_CPMEM_WORD(0, 119, 1)
+#define IPU_FIELD_HF		IPU_CPMEM_WORD(0, 120, 1)
+#define IPU_FIELD_VF		IPU_CPMEM_WORD(0, 121, 1)
+#define IPU_FIELD_THE		IPU_CPMEM_WORD(0, 122, 1)
+#define IPU_FIELD_CAP		IPU_CPMEM_WORD(0, 123, 1)
+#define IPU_FIELD_CAE		IPU_CPMEM_WORD(0, 124, 1)
+#define IPU_FIELD_FW		IPU_CPMEM_WORD(0, 125, 13)
+#define IPU_FIELD_FH		IPU_CPMEM_WORD(0, 138, 12)
+#define IPU_FIELD_EBA0		IPU_CPMEM_WORD(1, 0, 29)
+#define IPU_FIELD_EBA1		IPU_CPMEM_WORD(1, 29, 29)
+#define IPU_FIELD_ILO		IPU_CPMEM_WORD(1, 58, 20)
+#define IPU_FIELD_NPB		IPU_CPMEM_WORD(1, 78, 7)
+#define IPU_FIELD_PFS		IPU_CPMEM_WORD(1, 85, 4)
+#define IPU_FIELD_ALU		IPU_CPMEM_WORD(1, 89, 1)
+#define IPU_FIELD_ALBM		IPU_CPMEM_WORD(1, 90, 3)
+#define IPU_FIELD_ID		IPU_CPMEM_WORD(1, 93, 2)
+#define IPU_FIELD_TH		IPU_CPMEM_WORD(1, 95, 7)
+#define IPU_FIELD_SL		IPU_CPMEM_WORD(1, 102, 14)
+#define IPU_FIELD_WID0		IPU_CPMEM_WORD(1, 116, 3)
+#define IPU_FIELD_WID1		IPU_CPMEM_WORD(1, 119, 3)
+#define IPU_FIELD_WID2		IPU_CPMEM_WORD(1, 122, 3)
+#define IPU_FIELD_WID3		IPU_CPMEM_WORD(1, 125, 3)
+#define IPU_FIELD_OFS0		IPU_CPMEM_WORD(1, 128, 5)
+#define IPU_FIELD_OFS1		IPU_CPMEM_WORD(1, 133, 5)
+#define IPU_FIELD_OFS2		IPU_CPMEM_WORD(1, 138, 5)
+#define IPU_FIELD_OFS3		IPU_CPMEM_WORD(1, 143, 5)
+#define IPU_FIELD_SXYS		IPU_CPMEM_WORD(1, 148, 1)
+#define IPU_FIELD_CRE		IPU_CPMEM_WORD(1, 149, 1)
+#define IPU_FIELD_DEC_SEL2	IPU_CPMEM_WORD(1, 150, 1)
+
+struct ipu_cpmem_word {
+	u32 data[5];
+	u32 res[3];
+};
+
+struct ipu_ch_param {
+	struct ipu_cpmem_word word[2];
+};
+
+void ipu_ch_param_write_field(struct ipu_ch_param __iomem *base, u32 wbs, u32 v);
+u32 ipu_ch_param_read_field(struct ipu_ch_param __iomem *base, u32 wbs);
+struct ipu_ch_param __iomem *ipu_get_cpmem(struct ipuv3_channel *channel);
+void ipu_ch_param_dump(struct ipu_ch_param __iomem *p);
+
+static inline void ipu_ch_param_zero(struct ipu_ch_param __iomem *p)
+{
+	int i;
+	void __iomem *base = p;
+
+	for (i = 0; i < sizeof(*p) / sizeof(u32); i++)
+		writel(0, base + i * sizeof(u32));
+}
+
+static inline void ipu_cpmem_set_buffer(struct ipu_ch_param __iomem *p,
+		int bufnum, dma_addr_t buf)
+{
+	if (bufnum)
+		ipu_ch_param_write_field(p, IPU_FIELD_EBA1, buf >> 3);
+	else
+		ipu_ch_param_write_field(p, IPU_FIELD_EBA0, buf >> 3);
+}
+
+static inline void ipu_cpmem_set_resolution(struct ipu_ch_param __iomem *p,
+		int xres, int yres)
+{
+	ipu_ch_param_write_field(p, IPU_FIELD_FW, xres - 1);
+	ipu_ch_param_write_field(p, IPU_FIELD_FH, yres - 1);
+}
+
+static inline void ipu_cpmem_set_stride(struct ipu_ch_param __iomem *p,
+		int stride)
+{
+	ipu_ch_param_write_field(p, IPU_FIELD_SLY, stride - 1);
+}
+
+void ipu_cpmem_set_high_priority(struct ipuv3_channel *channel);
+
+struct ipu_rgb {
+	struct fb_bitfield	red;
+	struct fb_bitfield	green;
+	struct fb_bitfield	blue;
+	struct fb_bitfield	transp;
+	int			bits_per_pixel;
+};
+
+struct ipu_image {
+	struct v4l2_pix_format pix;
+	struct v4l2_rect rect;
+	dma_addr_t phys;
+};
+
+int ipu_cpmem_set_format_passthrough(struct ipu_ch_param __iomem *p,
+		int width);
+
+int ipu_cpmem_set_format_rgb(struct ipu_ch_param __iomem *,
+		struct ipu_rgb *rgb);
+
+static inline void ipu_cpmem_interlaced_scan(struct ipu_ch_param *p,
+		int stride)
+{
+	ipu_ch_param_write_field(p, IPU_FIELD_SO, 1);
+	ipu_ch_param_write_field(p, IPU_FIELD_ILO, stride / 8);
+	ipu_ch_param_write_field(p, IPU_FIELD_SLY, (stride * 2) - 1);
+};
+
+void ipu_cpmem_set_yuv_planar(struct ipu_ch_param __iomem *p, u32 pixel_format,
+			int stride, int height);
+void ipu_cpmem_set_yuv_planar_full(struct ipu_ch_param __iomem *p,
+		u32 pixel_format, int stride, int u_offset, int v_offset);
+int ipu_cpmem_set_fmt(struct ipu_ch_param __iomem *cpmem, u32 pixelformat);
+int ipu_cpmem_set_image(struct ipu_ch_param __iomem *cpmem,
+		struct ipu_image *image);
+
+enum ipu_color_space ipu_pixelformat_to_colorspace(u32 pixelformat);
+
+static inline void ipu_cpmem_set_burstsize(struct ipu_ch_param __iomem *p,
+		int burstsize)
+{
+	ipu_ch_param_write_field(p, IPU_FIELD_NPB, burstsize - 1);
+};
+
+struct ipu_client_platformdata {
+	int di;
+	int dc;
+	int dp;
+	int dmfc;
+	int dma[2];
+};
+
+#endif /* __DRM_IPU_H__ */
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-common.c b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
new file mode 100644
index 000000000000..f381960f42b0
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
@@ -0,0 +1,1143 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#include <linux/module.h>
+#include <linux/export.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/list.h>
+#include <linux/irq.h>
+#include <linux/of_device.h>
+#include <asm/mach/irq.h>
+
+#include "imx-ipu-v3.h"
+#include "ipu-prv.h"
+
+static inline u32 ipu_cm_read(struct ipu_soc *ipu, unsigned offset)
+{
+	return readl(ipu->cm_reg + offset);
+}
+
+static inline void ipu_cm_write(struct ipu_soc *ipu, u32 value, unsigned offset)
+{
+	writel(value, ipu->cm_reg + offset);
+}
+
+static inline u32 ipu_idmac_read(struct ipu_soc *ipu, unsigned offset)
+{
+	return readl(ipu->idmac_reg + offset);
+}
+
+static inline void ipu_idmac_write(struct ipu_soc *ipu, u32 value,
+		unsigned offset)
+{
+	writel(value, ipu->idmac_reg + offset);
+}
+
+void ipu_srm_dp_sync_update(struct ipu_soc *ipu)
+{
+	u32 val;
+
+	val = ipu_cm_read(ipu, IPU_SRM_PRI2);
+	val |= 0x8;
+	ipu_cm_write(ipu, val, IPU_SRM_PRI2);
+}
+EXPORT_SYMBOL_GPL(ipu_srm_dp_sync_update);
+
+struct ipu_ch_param __iomem *ipu_get_cpmem(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+
+	return ipu->cpmem_base + channel->num;
+}
+EXPORT_SYMBOL_GPL(ipu_get_cpmem);
+
+void ipu_cpmem_set_high_priority(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	struct ipu_ch_param __iomem *p = ipu_get_cpmem(channel);
+	u32 val;
+
+	if (ipu->ipu_type == IPUV3EX)
+		ipu_ch_param_write_field(p, IPU_FIELD_ID, 1);
+
+	val = ipu_idmac_read(ipu, IDMAC_CHA_PRI(channel->num));
+	val |= 1 << (channel->num % 32);
+	ipu_idmac_write(ipu, val, IDMAC_CHA_PRI(channel->num));
+};
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_high_priority);
+
+void ipu_ch_param_write_field(struct ipu_ch_param __iomem *base, u32 wbs, u32 v)
+{
+	u32 bit = (wbs >> 8) % 160;
+	u32 size = wbs & 0xff;
+	u32 word = (wbs >> 8) / 160;
+	u32 i = bit / 32;
+	u32 ofs = bit % 32;
+	u32 mask = (1 << size) - 1;
+	u32 val;
+
+	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
+
+	val = readl(&base->word[word].data[i]);
+	val &= ~(mask << ofs);
+	val |= v << ofs;
+	writel(val, &base->word[word].data[i]);
+
+	if ((bit + size - 1) / 32 > i) {
+		val = readl(&base->word[word].data[i + 1]);
+		val &= ~(mask >> (ofs ? (32 - ofs) : 0));
+		val |= v >> (ofs ? (32 - ofs) : 0);
+		writel(val, &base->word[word].data[i + 1]);
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_ch_param_write_field);
+
+u32 ipu_ch_param_read_field(struct ipu_ch_param __iomem *base, u32 wbs)
+{
+	u32 bit = (wbs >> 8) % 160;
+	u32 size = wbs & 0xff;
+	u32 word = (wbs >> 8) / 160;
+	u32 i = bit / 32;
+	u32 ofs = bit % 32;
+	u32 mask = (1 << size) - 1;
+	u32 val = 0;
+
+	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
+
+	val = (readl(&base->word[word].data[i]) >> ofs) & mask;
+
+	if ((bit + size - 1) / 32 > i) {
+		u32 tmp;
+		tmp = readl(&base->word[word].data[i + 1]);
+		tmp &= mask >> (ofs ? (32 - ofs) : 0);
+		val |= tmp << (ofs ? (32 - ofs) : 0);
+	}
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(ipu_ch_param_read_field);
+
+int ipu_cpmem_set_format_rgb(struct ipu_ch_param __iomem *p,
+		struct ipu_rgb *rgb)
+{
+	int bpp = 0, npb = 0, ro, go, bo, to;
+
+	ro = rgb->bits_per_pixel - rgb->red.length - rgb->red.offset;
+	go = rgb->bits_per_pixel - rgb->green.length - rgb->green.offset;
+	bo = rgb->bits_per_pixel - rgb->blue.length - rgb->blue.offset;
+	to = rgb->bits_per_pixel - rgb->transp.length - rgb->transp.offset;
+
+	ipu_ch_param_write_field(p, IPU_FIELD_WID0, rgb->red.length - 1);
+	ipu_ch_param_write_field(p, IPU_FIELD_OFS0, ro);
+	ipu_ch_param_write_field(p, IPU_FIELD_WID1, rgb->green.length - 1);
+	ipu_ch_param_write_field(p, IPU_FIELD_OFS1, go);
+	ipu_ch_param_write_field(p, IPU_FIELD_WID2, rgb->blue.length - 1);
+	ipu_ch_param_write_field(p, IPU_FIELD_OFS2, bo);
+
+	if (rgb->transp.length) {
+		ipu_ch_param_write_field(p, IPU_FIELD_WID3,
+				rgb->transp.length - 1);
+		ipu_ch_param_write_field(p, IPU_FIELD_OFS3, to);
+	} else {
+		ipu_ch_param_write_field(p, IPU_FIELD_WID3, 7);
+		ipu_ch_param_write_field(p, IPU_FIELD_OFS3,
+				rgb->bits_per_pixel);
+	}
+
+	switch (rgb->bits_per_pixel) {
+	case 32:
+		bpp = 0;
+		npb = 15;
+		break;
+	case 24:
+		bpp = 1;
+		npb = 19;
+		break;
+	case 16:
+		bpp = 3;
+		npb = 31;
+		break;
+	case 8:
+		bpp = 5;
+		npb = 63;
+		break;
+	default:
+		return -EINVAL;
+	}
+	ipu_ch_param_write_field(p, IPU_FIELD_BPP, bpp);
+	ipu_ch_param_write_field(p, IPU_FIELD_NPB, npb);
+	ipu_ch_param_write_field(p, IPU_FIELD_PFS, 7); /* rgb mode */
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_rgb);
+
+int ipu_cpmem_set_format_passthrough(struct ipu_ch_param __iomem *p,
+		int width)
+{
+	int bpp = 0, npb = 0;
+
+	switch (width) {
+	case 32:
+		bpp = 0;
+		npb = 15;
+		break;
+	case 24:
+		bpp = 1;
+		npb = 19;
+		break;
+	case 16:
+		bpp = 3;
+		npb = 31;
+		break;
+	case 8:
+		bpp = 5;
+		npb = 63;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ipu_ch_param_write_field(p, IPU_FIELD_BPP, bpp);
+	ipu_ch_param_write_field(p, IPU_FIELD_NPB, npb);
+	ipu_ch_param_write_field(p, IPU_FIELD_PFS, 6); /* raw mode */
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_passthrough);
+
+void ipu_cpmem_set_yuv_planar_full(struct ipu_ch_param __iomem *p,
+		u32 pixel_format, int stride, int u_offset, int v_offset)
+{
+	switch (pixel_format) {
+	case V4L2_PIX_FMT_YUV420:
+		ipu_ch_param_write_field(p, IPU_FIELD_SLUV, (stride / 2) - 1);
+		ipu_ch_param_write_field(p, IPU_FIELD_UBO, u_offset / 8);
+		ipu_ch_param_write_field(p, IPU_FIELD_VBO, v_offset / 8);
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar_full);
+
+void ipu_cpmem_set_yuv_planar(struct ipu_ch_param __iomem *p, u32 pixel_format,
+		int stride, int height)
+{
+	int u_offset, v_offset;
+	int uv_stride = 0;
+
+	switch (pixel_format) {
+	case V4L2_PIX_FMT_YUV420:
+		uv_stride = stride / 2;
+		u_offset = stride * height;
+		v_offset = u_offset + (uv_stride * height / 2);
+		ipu_cpmem_set_yuv_planar_full(p, V4L2_PIX_FMT_YUV420, stride,
+				u_offset, v_offset);
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar);
+
+static struct ipu_rgb def_rgb_32 = {
+	.red	= { .offset = 16, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset =  0, .length = 8, },
+	.transp = { .offset = 24, .length = 8, },
+	.bits_per_pixel = 32,
+};
+
+static struct ipu_rgb def_bgr_32 = {
+	.red	= { .offset = 16, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset =  0, .length = 8, },
+	.transp = { .offset = 24, .length = 8, },
+	.bits_per_pixel = 32,
+};
+
+static struct ipu_rgb def_rgb_24 = {
+	.red	= { .offset =  0, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset = 16, .length = 8, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 24,
+};
+
+static struct ipu_rgb def_bgr_24 = {
+	.red	= { .offset = 16, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset =  0, .length = 8, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 24,
+};
+
+static struct ipu_rgb def_rgb_16 = {
+	.red	= { .offset = 11, .length = 5, },
+	.green	= { .offset =  5, .length = 6, },
+	.blue	= { .offset =  0, .length = 5, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 16,
+};
+
+#define Y_OFFSET(pix, x, y)	((x) + pix->width * (y))
+#define U_OFFSET(pix, x, y)	((pix->width * pix->height) + \
+					(pix->width * (y) / 4) + (x) / 2)
+#define V_OFFSET(pix, x, y)	((pix->width * pix->height) + \
+					(pix->width * pix->height / 4) + \
+					(pix->width * (y) / 4) + (x) / 2)
+
+int ipu_cpmem_set_fmt(struct ipu_ch_param __iomem *cpmem, u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		/* pix format */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 2);
+		/* burst size */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 63);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		/* bits/pixel */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_BPP, 3);
+		/* pix format */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 0xA);
+		/* burst size */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 31);
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		/* bits/pixel */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_BPP, 3);
+		/* pix format */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 0x8);
+		/* burst size */
+		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 31);
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_32);
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_16);
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		ipu_cpmem_set_format_rgb(cpmem, &def_bgr_32);
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_24);
+		break;
+	case V4L2_PIX_FMT_BGR24:
+		ipu_cpmem_set_format_rgb(cpmem, &def_bgr_24);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_fmt);
+
+int ipu_cpmem_set_image(struct ipu_ch_param __iomem *cpmem,
+		struct ipu_image *image)
+{
+	struct v4l2_pix_format *pix = &image->pix;
+	int y_offset, u_offset, v_offset;
+
+	pr_debug("%s: resolution: %dx%d stride: %d\n",
+			__func__, pix->width, pix->height,
+			pix->bytesperline);
+
+	ipu_cpmem_set_resolution(cpmem, image->rect.width,
+			image->rect.height);
+	ipu_cpmem_set_stride(cpmem, pix->bytesperline);
+
+	ipu_cpmem_set_fmt(cpmem, pix->pixelformat);
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		y_offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
+		u_offset = U_OFFSET(pix, image->rect.left,
+				image->rect.top) - y_offset;
+		v_offset = V_OFFSET(pix, image->rect.left,
+				image->rect.top) - y_offset;
+
+		ipu_cpmem_set_yuv_planar_full(cpmem, pix->pixelformat,
+				pix->bytesperline, u_offset, v_offset);
+		ipu_cpmem_set_buffer(cpmem, 0, image->phys + y_offset);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
+				image->rect.left * 2 +
+				image->rect.top * image->pix.bytesperline);
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
+				image->rect.left * 4 +
+				image->rect.top * image->pix.bytesperline);
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
+				image->rect.left * 2 +
+				image->rect.top * image->pix.bytesperline);
+		break;
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
+				image->rect.left * 3 +
+				image->rect.top * image->pix.bytesperline);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_image);
+
+enum ipu_color_space ipu_pixelformat_to_colorspace(u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+		return IPUV3_COLORSPACE_YUV;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB565:
+		return IPUV3_COLORSPACE_RGB;
+	default:
+		return IPUV3_COLORSPACE_UNKNOWN;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_pixelformat_to_colorspace);
+
+struct ipuv3_channel *ipu_idmac_get(struct ipu_soc *ipu, unsigned num)
+{
+	struct ipuv3_channel *channel;
+
+	dev_dbg(ipu->dev, "%s %d\n", __func__, num);
+
+	if (num > 63)
+		return ERR_PTR(-ENODEV);
+
+	mutex_lock(&ipu->channel_lock);
+
+	channel = &ipu->channel[num];
+
+	if (channel->busy) {
+		channel = ERR_PTR(-EBUSY);
+		goto out;
+	}
+
+	channel->busy = 1;
+	channel->num = num;
+
+out:
+	mutex_unlock(&ipu->channel_lock);
+
+	return channel;
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_get);
+
+void ipu_idmac_put(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+
+	dev_dbg(ipu->dev, "%s %d\n", __func__, channel->num);
+
+	mutex_lock(&ipu->channel_lock);
+
+	channel->busy = 0;
+
+	mutex_unlock(&ipu->channel_lock);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_put);
+
+#define idma_mask(ch)			(1 << (ch & 0x1f))
+
+void ipu_idmac_set_double_buffer(struct ipuv3_channel *channel,
+		bool doublebuffer)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	reg = ipu_cm_read(ipu, IPU_CHA_DB_MODE_SEL(channel->num));
+	if (doublebuffer)
+		reg |= idma_mask(channel->num);
+	else
+		reg &= ~idma_mask(channel->num);
+	ipu_cm_write(ipu, reg, IPU_CHA_DB_MODE_SEL(channel->num));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_set_double_buffer);
+
+int ipu_module_enable(struct ipu_soc *ipu, u32 mask)
+{
+	unsigned long lock_flags;
+	u32 val;
+
+	spin_lock_irqsave(&ipu->lock, lock_flags);
+
+	val = ipu_cm_read(ipu, IPU_DISP_GEN);
+
+	if (mask & IPU_CONF_DI0_EN)
+		val |= IPU_DI0_COUNTER_RELEASE;
+	if (mask & IPU_CONF_DI1_EN)
+		val |= IPU_DI1_COUNTER_RELEASE;
+
+	ipu_cm_write(ipu, val, IPU_DISP_GEN);
+
+	val = ipu_cm_read(ipu, IPU_CONF);
+	val |= mask;
+	ipu_cm_write(ipu, val, IPU_CONF);
+
+	spin_unlock_irqrestore(&ipu->lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_module_enable);
+
+int ipu_module_disable(struct ipu_soc *ipu, u32 mask)
+{
+	unsigned long lock_flags;
+	u32 val;
+
+	spin_lock_irqsave(&ipu->lock, lock_flags);
+
+	val = ipu_cm_read(ipu, IPU_CONF);
+	val &= ~mask;
+	ipu_cm_write(ipu, val, IPU_CONF);
+
+	val = ipu_cm_read(ipu, IPU_DISP_GEN);
+
+	if (mask & IPU_CONF_DI0_EN)
+		val &= ~IPU_DI0_COUNTER_RELEASE;
+	if (mask & IPU_CONF_DI1_EN)
+		val &= ~IPU_DI1_COUNTER_RELEASE;
+
+	ipu_cm_write(ipu, val, IPU_DISP_GEN);
+
+	spin_unlock_irqrestore(&ipu->lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_module_disable);
+
+void ipu_idmac_select_buffer(struct ipuv3_channel *channel, u32 buf_num)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned int chno = channel->num;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	/* Mark buffer as ready. */
+	if (buf_num == 0)
+		ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_BUF0_RDY(chno));
+	else
+		ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_BUF1_RDY(chno));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_select_buffer);
+
+int ipu_idmac_enable_channel(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	val = ipu_idmac_read(ipu, IDMAC_CHA_EN(channel->num));
+	val |= idma_mask(channel->num);
+	ipu_idmac_write(ipu, val, IDMAC_CHA_EN(channel->num));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_enable_channel);
+
+int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	u32 val;
+	unsigned long flags;
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(50);
+	while (ipu_idmac_read(ipu, IDMAC_CHA_BUSY(channel->num)) &
+			idma_mask(channel->num)) {
+		if (time_after(jiffies, timeout)) {
+			dev_warn(ipu->dev, "disabling busy idmac channel %d\n",
+					channel->num);
+			break;
+		}
+		cpu_relax();
+	}
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	/* Disable DMA channel(s) */
+	val = ipu_idmac_read(ipu, IDMAC_CHA_EN(channel->num));
+	val &= ~idma_mask(channel->num);
+	ipu_idmac_write(ipu, val, IDMAC_CHA_EN(channel->num));
+
+	/* Set channel buffers NOT to be ready */
+	ipu_cm_write(ipu, 0xf0000000, IPU_GPR); /* write one to clear */
+
+	if (ipu_cm_read(ipu, IPU_CHA_BUF0_RDY(channel->num)) &
+			idma_mask(channel->num)) {
+		ipu_cm_write(ipu, idma_mask(channel->num),
+			     IPU_CHA_BUF0_RDY(channel->num));
+	}
+
+	if (ipu_cm_read(ipu, IPU_CHA_BUF1_RDY(channel->num)) &
+			idma_mask(channel->num)) {
+		ipu_cm_write(ipu, idma_mask(channel->num),
+			     IPU_CHA_BUF1_RDY(channel->num));
+	}
+
+	ipu_cm_write(ipu, 0x0, IPU_GPR); /* write one to set */
+
+	/* Reset the double buffer */
+	val = ipu_cm_read(ipu, IPU_CHA_DB_MODE_SEL(channel->num));
+	val &= ~idma_mask(channel->num);
+	ipu_cm_write(ipu, val, IPU_CHA_DB_MODE_SEL(channel->num));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_disable_channel);
+
+static int ipu_reset(struct ipu_soc *ipu)
+{
+	unsigned long timeout;
+
+	ipu_cm_write(ipu, 0x807FFFFF, IPU_MEM_RST);
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+	while (ipu_cm_read(ipu, IPU_MEM_RST) & 0x80000000) {
+		if (time_after(jiffies, timeout))
+			return -ETIME;
+		cpu_relax();
+	}
+
+	mdelay(300);
+
+	return 0;
+}
+
+struct ipu_devtype {
+	const char *name;
+	unsigned long cm_ofs;
+	unsigned long cpmem_ofs;
+	unsigned long srm_ofs;
+	unsigned long tpm_ofs;
+	unsigned long disp0_ofs;
+	unsigned long disp1_ofs;
+	unsigned long dc_tmpl_ofs;
+	unsigned long vdi_ofs;
+	enum ipuv3_type type;
+};
+
+static struct ipu_devtype ipu_type_imx51 = {
+	.name = "IPUv3EX",
+	.cm_ofs = 0x1e000000,
+	.cpmem_ofs = 0x1f000000,
+	.srm_ofs = 0x1f040000,
+	.tpm_ofs = 0x1f060000,
+	.disp0_ofs = 0x1e040000,
+	.disp1_ofs = 0x1e048000,
+	.dc_tmpl_ofs = 0x1f080000,
+	.vdi_ofs = 0x1e068000,
+	.type = IPUV3EX,
+};
+
+static struct ipu_devtype ipu_type_imx53 = {
+	.name = "IPUv3M",
+	.cm_ofs = 0x06000000,
+	.cpmem_ofs = 0x07000000,
+	.srm_ofs = 0x07040000,
+	.tpm_ofs = 0x07060000,
+	.disp0_ofs = 0x06040000,
+	.disp1_ofs = 0x06048000,
+	.dc_tmpl_ofs = 0x07080000,
+	.vdi_ofs = 0x06068000,
+	.type = IPUV3M,
+};
+
+static struct ipu_devtype ipu_type_imx6q = {
+	.name = "IPUv3H",
+	.cm_ofs = 0x00200000,
+	.cpmem_ofs = 0x00300000,
+	.srm_ofs = 0x00340000,
+	.tpm_ofs = 0x00360000,
+	.disp0_ofs = 0x00240000,
+	.disp1_ofs = 0x00248000,
+	.dc_tmpl_ofs = 0x00380000,
+	.vdi_ofs = 0x00268000,
+	.type = IPUV3H,
+};
+
+static const struct of_device_id imx_ipu_dt_ids[] = {
+	{ .compatible = "fsl,imx51-ipu", .data = &ipu_type_imx51, },
+	{ .compatible = "fsl,imx53-ipu", .data = &ipu_type_imx53, },
+	{ .compatible = "fsl,imx6q-ipu", .data = &ipu_type_imx6q, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx_ipu_dt_ids);
+
+static int ipu_submodules_init(struct ipu_soc *ipu,
+		struct platform_device *pdev, unsigned long ipu_base,
+		struct clk *ipu_clk)
+{
+	char *unit;
+	int ret;
+	struct device *dev = &pdev->dev;
+	const struct ipu_devtype *devtype = ipu->devtype;
+
+	ret = ipu_di_init(ipu, dev, 0, ipu_base + devtype->disp0_ofs,
+			IPU_CONF_DI0_EN, ipu_clk);
+	if (ret) {
+		unit = "di0";
+		goto err_di_0;
+	}
+
+	ret = ipu_di_init(ipu, dev, 1, ipu_base + devtype->disp1_ofs,
+			IPU_CONF_DI1_EN, ipu_clk);
+	if (ret) {
+		unit = "di1";
+		goto err_di_1;
+	}
+
+	ret = ipu_dc_init(ipu, dev, ipu_base + devtype->cm_ofs +
+			IPU_CM_DC_REG_OFS, ipu_base + devtype->dc_tmpl_ofs);
+	if (ret) {
+		unit = "dc_template";
+		goto err_dc;
+	}
+
+	ret = ipu_dmfc_init(ipu, dev, ipu_base +
+			devtype->cm_ofs + IPU_CM_DMFC_REG_OFS, ipu_clk);
+	if (ret) {
+		unit = "dmfc";
+		goto err_dmfc;
+	}
+
+	ret = ipu_dp_init(ipu, dev, ipu_base + devtype->srm_ofs);
+	if (ret) {
+		unit = "dp";
+		goto err_dp;
+	}
+
+	return 0;
+
+err_dp:
+	ipu_dmfc_exit(ipu);
+err_dmfc:
+	ipu_dc_exit(ipu);
+err_dc:
+	ipu_di_exit(ipu, 1);
+err_di_1:
+	ipu_di_exit(ipu, 0);
+err_di_0:
+	dev_err(&pdev->dev, "init %s failed with %d\n", unit, ret);
+	return ret;
+}
+
+static void ipu_irq_handle(struct ipu_soc *ipu, const int *regs, int num_regs)
+{
+	unsigned long status;
+	int i, bit, irq_base;
+
+	for (i = 0; i < num_regs; i++) {
+
+		status = ipu_cm_read(ipu, IPU_INT_STAT(regs[i]));
+		status &= ipu_cm_read(ipu, IPU_INT_CTRL(regs[i]));
+
+		irq_base = ipu->irq_start + regs[i] * 32;
+		for_each_set_bit(bit, &status, 32)
+			generic_handle_irq(irq_base + bit);
+	}
+}
+
+static void ipu_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct ipu_soc *ipu = irq_desc_get_handler_data(desc);
+	const int int_reg[] = { 0, 1, 2, 3, 10, 11, 12, 13, 14};
+	struct irq_chip *chip = irq_get_chip(irq);
+
+	chained_irq_enter(chip, desc);
+
+	ipu_irq_handle(ipu, int_reg, ARRAY_SIZE(int_reg));
+
+	chained_irq_exit(chip, desc);
+}
+
+static void ipu_err_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct ipu_soc *ipu = irq_desc_get_handler_data(desc);
+	const int int_reg[] = { 4, 5, 8, 9};
+	struct irq_chip *chip = irq_get_chip(irq);
+
+	chained_irq_enter(chip, desc);
+
+	ipu_irq_handle(ipu, int_reg, ARRAY_SIZE(int_reg));
+
+	chained_irq_exit(chip, desc);
+}
+
+static void ipu_ack_irq(struct irq_data *d)
+{
+	struct ipu_soc *ipu = irq_data_get_irq_chip_data(d);
+	unsigned int irq = d->irq - ipu->irq_start;
+
+	ipu_cm_write(ipu, 1 << (irq % 32), IPU_INT_STAT(irq / 32));
+}
+
+static void ipu_unmask_irq(struct irq_data *d)
+{
+	struct ipu_soc *ipu = irq_data_get_irq_chip_data(d);
+	unsigned int irq = d->irq - ipu->irq_start;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	reg = ipu_cm_read(ipu, IPU_INT_CTRL(irq / 32));
+	reg |= 1 << (irq % 32);
+	ipu_cm_write(ipu, reg, IPU_INT_CTRL(irq / 32));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+
+static void ipu_mask_irq(struct irq_data *d)
+{
+	struct ipu_soc *ipu = irq_data_get_irq_chip_data(d);
+	unsigned int irq = d->irq - ipu->irq_start;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	reg = ipu_cm_read(ipu, IPU_INT_CTRL(irq / 32));
+	reg &= ~(1 << (irq % 32));
+	ipu_cm_write(ipu, reg, IPU_INT_CTRL(irq / 32));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+
+static struct irq_chip ipu_irq_chip = {
+	.name = "IPU",
+	.irq_ack = ipu_ack_irq,
+	.irq_mask = ipu_mask_irq,
+	.irq_unmask = ipu_unmask_irq,
+};
+
+int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
+		enum ipu_channel_irq irq_type)
+{
+	return ipu->irq_start + irq_type + channel->num;
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_channel_irq);
+
+static void ipu_submodules_exit(struct ipu_soc *ipu)
+{
+	ipu_dp_exit(ipu);
+	ipu_dmfc_exit(ipu);
+	ipu_dc_exit(ipu);
+	ipu_di_exit(ipu, 1);
+	ipu_di_exit(ipu, 0);
+}
+
+static int platform_remove_devices_fn(struct device *dev, void *unused)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	platform_device_unregister(pdev);
+
+	return 0;
+}
+
+static void platform_device_unregister_children(struct platform_device *pdev)
+{
+	device_for_each_child(&pdev->dev, NULL, platform_remove_devices_fn);
+}
+
+struct ipu_platform_reg {
+	struct ipu_client_platformdata pdata;
+	const char *name;
+};
+
+static const struct ipu_platform_reg client_reg[] = {
+	{
+		.pdata = {
+			.di = 0,
+			.dc = 5,
+			.dp = IPU_DP_FLOW_SYNC_BG,
+			.dma[0] = IPUV3_CHANNEL_MEM_BG_SYNC,
+			.dma[1] = -EINVAL,
+		},
+		.name = "imx-ipuv3-crtc",
+	}, {
+		.pdata = {
+			.di = 1,
+			.dc = 1,
+			.dp = -EINVAL,
+			.dma[0] = IPUV3_CHANNEL_MEM_DC_SYNC,
+			.dma[1] = -EINVAL,
+		},
+		.name = "imx-ipuv3-crtc",
+	},
+};
+
+static int ipu_client_id;
+
+static int ipu_add_subdevice_pdata(struct device *dev,
+		const struct ipu_platform_reg *reg)
+{
+	struct platform_device *pdev;
+
+	pdev = platform_device_register_data(dev, reg->name, ipu_client_id++,
+			&reg->pdata, sizeof(struct ipu_platform_reg));
+
+	return pdev ? 0 : -EINVAL;
+}
+
+static int ipu_add_client_devices(struct ipu_soc *ipu)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(client_reg); i++) {
+		const struct ipu_platform_reg *reg = &client_reg[i];
+		ret = ipu_add_subdevice_pdata(ipu->dev, reg);
+		if (ret)
+			goto err_register;
+	}
+
+	return 0;
+
+err_register:
+	platform_device_unregister_children(to_platform_device(ipu->dev));
+
+	return ret;
+}
+
+static int ipu_irq_init(struct ipu_soc *ipu)
+{
+	int i;
+
+	ipu->irq_start = irq_alloc_descs(-1, 0, IPU_NUM_IRQS, 0);
+	if (ipu->irq_start < 0)
+		return ipu->irq_start;
+
+	for (i = ipu->irq_start; i < ipu->irq_start + IPU_NUM_IRQS; i++) {
+		irq_set_chip_and_handler(i, &ipu_irq_chip, handle_level_irq);
+		set_irq_flags(i, IRQF_VALID);
+		irq_set_chip_data(i, ipu);
+	}
+
+	irq_set_chained_handler(ipu->irq_sync, ipu_irq_handler);
+	irq_set_handler_data(ipu->irq_sync, ipu);
+	irq_set_chained_handler(ipu->irq_err, ipu_err_irq_handler);
+	irq_set_handler_data(ipu->irq_err, ipu);
+
+	return 0;
+}
+
+static void ipu_irq_exit(struct ipu_soc *ipu)
+{
+	int i;
+
+	irq_set_chained_handler(ipu->irq_err, NULL);
+	irq_set_handler_data(ipu->irq_err, NULL);
+	irq_set_chained_handler(ipu->irq_sync, NULL);
+	irq_set_handler_data(ipu->irq_sync, NULL);
+
+	for (i = ipu->irq_start; i < ipu->irq_start + IPU_NUM_IRQS; i++) {
+		set_irq_flags(i, 0);
+		irq_set_chip(i, NULL);
+		irq_set_chip_data(i, NULL);
+	}
+
+	irq_free_descs(ipu->irq_start, IPU_NUM_IRQS);
+}
+
+static int __devinit ipu_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id =
+			of_match_device(imx_ipu_dt_ids, &pdev->dev);
+	struct ipu_soc *ipu;
+	struct resource *res;
+	unsigned long ipu_base;
+	int i, ret, irq_sync, irq_err;
+	const struct ipu_devtype *devtype;
+
+	devtype = of_id->data;
+
+	dev_info(&pdev->dev, "Initializing %s\n", devtype->name);
+
+	irq_sync = platform_get_irq(pdev, 0);
+	irq_err = platform_get_irq(pdev, 1);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	dev_info(&pdev->dev, "irq_sync: %d irq_err: %d\n",
+			irq_sync, irq_err);
+
+	if (!res || irq_sync < 0 || irq_err < 0)
+		return -ENODEV;
+
+	ipu_base = res->start;
+
+	ipu = devm_kzalloc(&pdev->dev, sizeof(*ipu), GFP_KERNEL);
+	if (!ipu)
+		return -ENODEV;
+
+	for (i = 0; i < 64; i++)
+		ipu->channel[i].ipu = ipu;
+	ipu->devtype = devtype;
+	ipu->ipu_type = devtype->type;
+
+	spin_lock_init(&ipu->lock);
+	mutex_init(&ipu->channel_lock);
+
+	dev_info(&pdev->dev, "cm_reg:   0x%08lx\n",
+			ipu_base + devtype->cm_ofs);
+	dev_info(&pdev->dev, "idmac:    0x%08lx\n",
+			ipu_base + devtype->cm_ofs + IPU_CM_IDMAC_REG_OFS);
+	dev_info(&pdev->dev, "cpmem:    0x%08lx\n",
+			ipu_base + devtype->cpmem_ofs);
+	dev_info(&pdev->dev, "disp0:    0x%08lx\n",
+			ipu_base + devtype->disp0_ofs);
+	dev_info(&pdev->dev, "disp1:    0x%08lx\n",
+			ipu_base + devtype->disp1_ofs);
+	dev_info(&pdev->dev, "srm:      0x%08lx\n",
+			ipu_base + devtype->srm_ofs);
+	dev_info(&pdev->dev, "tpm:      0x%08lx\n",
+			ipu_base + devtype->tpm_ofs);
+	dev_info(&pdev->dev, "dc:       0x%08lx\n",
+			ipu_base + devtype->cm_ofs + IPU_CM_DC_REG_OFS);
+	dev_info(&pdev->dev, "ic:       0x%08lx\n",
+			ipu_base + devtype->cm_ofs + IPU_CM_IC_REG_OFS);
+	dev_info(&pdev->dev, "dmfc:     0x%08lx\n",
+			ipu_base + devtype->cm_ofs + IPU_CM_DMFC_REG_OFS);
+	dev_info(&pdev->dev, "vdi:      0x%08lx\n",
+			ipu_base + devtype->vdi_ofs);
+
+	ipu->cm_reg = devm_ioremap(&pdev->dev,
+			ipu_base + devtype->cm_ofs, PAGE_SIZE);
+	ipu->idmac_reg = devm_ioremap(&pdev->dev,
+			ipu_base + devtype->cm_ofs + IPU_CM_IDMAC_REG_OFS,
+			PAGE_SIZE);
+	ipu->cpmem_base = devm_ioremap(&pdev->dev,
+			ipu_base + devtype->cpmem_ofs, PAGE_SIZE);
+
+	if (!ipu->cm_reg || !ipu->idmac_reg || !ipu->cpmem_base) {
+		ret = -ENOMEM;
+		goto failed_ioremap;
+	}
+
+	ipu->clk = devm_clk_get(&pdev->dev, "bus");
+	if (IS_ERR(ipu->clk)) {
+		ret = PTR_ERR(ipu->clk);
+		dev_err(&pdev->dev, "clk_get failed with %d", ret);
+		goto failed_clk_get;
+	}
+
+	platform_set_drvdata(pdev, ipu);
+
+	clk_prepare_enable(ipu->clk);
+
+	ipu->dev = &pdev->dev;
+	ipu->irq_sync = irq_sync;
+	ipu->irq_err = irq_err;
+
+	ret = ipu_irq_init(ipu);
+	if (ret)
+		goto out_failed_irq;
+
+	ipu_reset(ipu);
+
+	/* Set MCU_T to divide MCU access window into 2 */
+	ipu_cm_write(ipu, 0x00400000L | (IPU_MCU_T_DEFAULT << 18),
+			IPU_DISP_GEN);
+
+	ret = ipu_submodules_init(ipu, pdev, ipu_base, ipu->clk);
+	if (ret)
+		goto failed_submodules_init;
+
+	ret = ipu_add_client_devices(ipu);
+	if (ret) {
+		dev_err(&pdev->dev, "adding client devices failed with %d\n",
+				ret);
+		goto failed_add_clients;
+	}
+
+	return 0;
+
+failed_add_clients:
+	ipu_submodules_exit(ipu);
+failed_submodules_init:
+	ipu_irq_exit(ipu);
+out_failed_irq:
+	clk_disable_unprepare(ipu->clk);
+failed_clk_get:
+failed_ioremap:
+	return ret;
+}
+
+static int __devexit ipu_remove(struct platform_device *pdev)
+{
+	struct ipu_soc *ipu = platform_get_drvdata(pdev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	platform_device_unregister_children(pdev);
+	ipu_submodules_exit(ipu);
+	ipu_irq_exit(ipu);
+
+	clk_disable_unprepare(ipu->clk);
+
+	return 0;
+}
+
+static struct platform_driver imx_ipu_driver = {
+	.driver = {
+		.name = "imx-ipuv3",
+		.of_match_table = imx_ipu_dt_ids,
+	},
+	.probe = ipu_probe,
+	.remove = __devexit_p(ipu_remove),
+};
+
+module_platform_driver(imx_ipu_driver);
+
+MODULE_DESCRIPTION("i.MX IPU v3 driver");
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
new file mode 100644
index 000000000000..93c7579417be
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
@@ -0,0 +1,372 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include "imx-ipu-v3.h"
+#include "ipu-prv.h"
+
+#define DC_MAP_CONF_PTR(n)	(0x108 + ((n) & ~0x1) * 2)
+#define DC_MAP_CONF_VAL(n)	(0x144 + ((n) & ~0x1) * 2)
+
+#define DC_EVT_NF		0
+#define DC_EVT_NL		1
+#define DC_EVT_EOF		2
+#define DC_EVT_NFIELD		3
+#define DC_EVT_EOL		4
+#define DC_EVT_EOFIELD		5
+#define DC_EVT_NEW_ADDR		6
+#define DC_EVT_NEW_CHAN		7
+#define DC_EVT_NEW_DATA		8
+
+#define DC_EVT_NEW_ADDR_W_0	0
+#define DC_EVT_NEW_ADDR_W_1	1
+#define DC_EVT_NEW_CHAN_W_0	2
+#define DC_EVT_NEW_CHAN_W_1	3
+#define DC_EVT_NEW_DATA_W_0	4
+#define DC_EVT_NEW_DATA_W_1	5
+#define DC_EVT_NEW_ADDR_R_0	6
+#define DC_EVT_NEW_ADDR_R_1	7
+#define DC_EVT_NEW_CHAN_R_0	8
+#define DC_EVT_NEW_CHAN_R_1	9
+#define DC_EVT_NEW_DATA_R_0	10
+#define DC_EVT_NEW_DATA_R_1	11
+
+#define DC_WR_CH_CONF		0x0
+#define DC_WR_CH_ADDR		0x4
+#define DC_RL_CH(evt)		(8 + ((evt) & ~0x1) * 2)
+
+#define DC_GEN			0xd4
+#define DC_DISP_CONF1(disp)	(0xd8 + (disp) * 4)
+#define DC_DISP_CONF2(disp)	(0xe8 + (disp) * 4)
+#define DC_STAT			0x1c8
+
+#define WROD(lf)		(0x18 | ((lf) << 1))
+#define WRG			0x01
+
+#define SYNC_WAVE 0
+
+#define DC_GEN_SYNC_1_6_SYNC	(2 << 1)
+#define DC_GEN_SYNC_PRIORITY_1	(1 << 7)
+
+#define DC_WR_CH_CONF_WORD_SIZE_8		(0 << 0)
+#define DC_WR_CH_CONF_WORD_SIZE_16		(1 << 0)
+#define DC_WR_CH_CONF_WORD_SIZE_24		(2 << 0)
+#define DC_WR_CH_CONF_WORD_SIZE_32		(3 << 0)
+#define DC_WR_CH_CONF_DISP_ID_PARALLEL(i)	(((i) & 0x1) << 3)
+#define DC_WR_CH_CONF_DISP_ID_SERIAL		(2 << 3)
+#define DC_WR_CH_CONF_DISP_ID_ASYNC		(3 << 4)
+#define DC_WR_CH_CONF_FIELD_MODE		(1 << 9)
+#define DC_WR_CH_CONF_PROG_TYPE_NORMAL		(4 << 5)
+#define DC_WR_CH_CONF_PROG_TYPE_MASK		(7 << 5)
+#define DC_WR_CH_CONF_PROG_DI_ID		(1 << 2)
+#define DC_WR_CH_CONF_PROG_DISP_ID(i)		(((i) & 0x1) << 3)
+
+#define IPU_DC_NUM_CHANNELS	10
+
+struct ipu_dc_priv;
+
+enum ipu_dc_map {
+	IPU_DC_MAP_RGB24,
+	IPU_DC_MAP_RGB565,
+};
+
+struct ipu_dc {
+	/* The display interface number assigned to this dc channel */
+	unsigned int		di;
+	void __iomem		*base;
+	struct ipu_dc_priv	*priv;
+	int			chno;
+	bool			in_use;
+};
+
+struct ipu_dc_priv {
+	void __iomem		*dc_reg;
+	void __iomem		*dc_tmpl_reg;
+	struct ipu_soc		*ipu;
+	struct device		*dev;
+	struct ipu_dc		channels[IPU_DC_NUM_CHANNELS];
+	struct mutex		mutex;
+};
+
+static void dc_link_event(struct ipu_dc *dc, int event, int addr, int priority)
+{
+	u32 reg;
+
+	reg = readl(dc->base + DC_RL_CH(event));
+	reg &= ~(0xffff << (16 * (event & 0x1)));
+	reg |= ((addr << 8) | priority) << (16 * (event & 0x1));
+	writel(reg, dc->base + DC_RL_CH(event));
+}
+
+static void dc_write_tmpl(struct ipu_dc *dc, int word, u32 opcode, u32 operand,
+		int map, int wave, int glue, int sync)
+{
+	struct ipu_dc_priv *priv = dc->priv;
+	u32 reg;
+	int stop = 1;
+
+	reg = sync | glue << 4 | ++wave << 11 | ++map << 15 | ((operand << 20) & 0xfff00000);
+	writel(reg, priv->dc_tmpl_reg + word * 8);
+	reg = operand >> 12 | opcode << 4 | stop << 9;
+	writel(reg, priv->dc_tmpl_reg + word * 8 + 4);
+}
+
+static int ipu_pixfmt_to_map(u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_RGB24:
+		return IPU_DC_MAP_RGB24;
+	case V4L2_PIX_FMT_RGB565:
+		return IPU_DC_MAP_RGB565;
+	default:
+		return -EINVAL;
+	}
+}
+
+int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
+		u32 pixel_fmt, u32 width)
+{
+	struct ipu_dc_priv *priv = dc->priv;
+	u32 reg = 0, map;
+
+	dc->di = ipu_di_get_num(di);
+
+	map = ipu_pixfmt_to_map(pixel_fmt);
+	if (map < 0) {
+		dev_dbg(priv->dev, "IPU_DISP: No MAP\n");
+		return -EINVAL;
+	}
+
+	if (interlaced) {
+		dc_link_event(dc, DC_EVT_NL, 0, 3);
+		dc_link_event(dc, DC_EVT_EOL, 0, 2);
+		dc_link_event(dc, DC_EVT_NEW_DATA, 0, 1);
+
+		/* Init template microcode */
+		dc_write_tmpl(dc, 0, WROD(0), 0, map, SYNC_WAVE, 0, 8);
+	} else {
+		if (dc->di) {
+			dc_link_event(dc, DC_EVT_NL, 2, 3);
+			dc_link_event(dc, DC_EVT_EOL, 3, 2);
+			dc_link_event(dc, DC_EVT_NEW_DATA, 4, 1);
+			/* Init template microcode */
+			dc_write_tmpl(dc, 2, WROD(0), 0, map, SYNC_WAVE, 8, 5);
+			dc_write_tmpl(dc, 3, WROD(0), 0, map, SYNC_WAVE, 4, 5);
+			dc_write_tmpl(dc, 4, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+		} else {
+			dc_link_event(dc, DC_EVT_NL, 5, 3);
+			dc_link_event(dc, DC_EVT_EOL, 6, 2);
+			dc_link_event(dc, DC_EVT_NEW_DATA, 7, 1);
+			/* Init template microcode */
+			dc_write_tmpl(dc, 5, WROD(0), 0, map, SYNC_WAVE, 8, 5);
+			dc_write_tmpl(dc, 6, WROD(0), 0, map, SYNC_WAVE, 4, 5);
+			dc_write_tmpl(dc, 7, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+		}
+	}
+	dc_link_event(dc, DC_EVT_NF, 0, 0);
+	dc_link_event(dc, DC_EVT_NFIELD, 0, 0);
+	dc_link_event(dc, DC_EVT_EOF, 0, 0);
+	dc_link_event(dc, DC_EVT_EOFIELD, 0, 0);
+	dc_link_event(dc, DC_EVT_NEW_CHAN, 0, 0);
+	dc_link_event(dc, DC_EVT_NEW_ADDR, 0, 0);
+
+	reg = readl(dc->base + DC_WR_CH_CONF);
+	if (interlaced)
+		reg |= DC_WR_CH_CONF_FIELD_MODE;
+	else
+		reg &= ~DC_WR_CH_CONF_FIELD_MODE;
+	writel(reg, dc->base + DC_WR_CH_CONF);
+
+	writel(0x0, dc->base + DC_WR_CH_ADDR);
+	writel(width, priv->dc_reg + DC_DISP_CONF2(dc->di));
+
+	ipu_module_enable(priv->ipu, IPU_CONF_DC_EN);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dc_init_sync);
+
+void ipu_dc_enable_channel(struct ipu_dc *dc)
+{
+	int di;
+	u32 reg;
+
+	di = dc->di;
+
+	reg = readl(dc->base + DC_WR_CH_CONF);
+	reg |= DC_WR_CH_CONF_PROG_TYPE_NORMAL;
+	writel(reg, dc->base + DC_WR_CH_CONF);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_enable_channel);
+
+void ipu_dc_disable_channel(struct ipu_dc *dc)
+{
+	struct ipu_dc_priv *priv = dc->priv;
+	u32 val;
+	int irq = 0, timeout = 50;
+
+	if (dc->chno == 1)
+		irq = IPU_IRQ_DC_FC_1;
+	else if (dc->chno == 5)
+		irq = IPU_IRQ_DP_SF_END;
+	else
+		return;
+
+	/* should wait for the interrupt here */
+	mdelay(50);
+
+	if (dc->di == 0)
+		val = 0x00000002;
+	else
+		val = 0x00000020;
+
+	/* Wait for DC triple buffer to empty */
+	while ((readl(priv->dc_reg + DC_STAT) & val) != val) {
+		msleep(2);
+		timeout -= 2;
+		if (timeout <= 0)
+			break;
+	}
+
+	val = readl(dc->base + DC_WR_CH_CONF);
+	val &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+	writel(val, dc->base + DC_WR_CH_CONF);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_disable_channel);
+
+static void ipu_dc_map_config(struct ipu_dc_priv *priv, enum ipu_dc_map map,
+		int byte_num, int offset, int mask)
+{
+	int ptr = map * 3 + byte_num;
+	u32 reg;
+
+	reg = readl(priv->dc_reg + DC_MAP_CONF_VAL(ptr));
+	reg &= ~(0xffff << (16 * (ptr & 0x1)));
+	reg |= ((offset << 8) | mask) << (16 * (ptr & 0x1));
+	writel(reg, priv->dc_reg + DC_MAP_CONF_VAL(ptr));
+
+	reg = readl(priv->dc_reg + DC_MAP_CONF_PTR(map));
+	reg &= ~(0x1f << ((16 * (map & 0x1)) + (5 * byte_num)));
+	reg |= ptr << ((16 * (map & 0x1)) + (5 * byte_num));
+	writel(reg, priv->dc_reg + DC_MAP_CONF_PTR(map));
+}
+
+static void ipu_dc_map_clear(struct ipu_dc_priv *priv, int map)
+{
+	u32 reg = readl(priv->dc_reg + DC_MAP_CONF_PTR(map));
+
+	writel(reg & ~(0xffff << (16 * (map & 0x1))),
+		     priv->dc_reg + DC_MAP_CONF_PTR(map));
+}
+
+struct ipu_dc *ipu_dc_get(struct ipu_soc *ipu, int channel)
+{
+	struct ipu_dc_priv *priv = ipu->dc_priv;
+	struct ipu_dc *dc;
+
+	if (channel >= IPU_DC_NUM_CHANNELS)
+		return ERR_PTR(-ENODEV);
+
+	dc = &priv->channels[channel];
+
+	mutex_lock(&priv->mutex);
+
+	if (dc->in_use) {
+		mutex_unlock(&priv->mutex);
+		return ERR_PTR(-EBUSY);
+	}
+
+	dc->in_use = 1;
+
+	mutex_unlock(&priv->mutex);
+
+	return dc;
+}
+EXPORT_SYMBOL_GPL(ipu_dc_get);
+
+void ipu_dc_put(struct ipu_dc *dc)
+{
+	struct ipu_dc_priv *priv = dc->priv;
+
+	mutex_lock(&priv->mutex);
+	dc->in_use = 0;
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_put);
+
+int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
+		unsigned long base, unsigned long template_base)
+{
+	struct ipu_dc_priv *priv;
+	static int channel_offsets[] = { 0, 0x1c, 0x38, 0x54, 0x58, 0x5c,
+		0x78, 0, 0x94, 0xb4};
+	int i;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	mutex_init(&priv->mutex);
+
+	priv->dev = dev;
+	priv->ipu = ipu;
+	priv->dc_reg = devm_ioremap(dev, base, PAGE_SIZE);
+	priv->dc_tmpl_reg = devm_ioremap(dev, template_base, PAGE_SIZE);
+	if (!priv->dc_reg || !priv->dc_tmpl_reg)
+		return -ENOMEM;
+
+	for (i = 0; i < IPU_DC_NUM_CHANNELS; i++) {
+		priv->channels[i].chno = i;
+		priv->channels[i].priv = priv;
+		priv->channels[i].base = priv->dc_reg + channel_offsets[i];
+	}
+
+	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(1) |
+			DC_WR_CH_CONF_PROG_DI_ID,
+			priv->channels[1].base + DC_WR_CH_CONF);
+	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(0),
+			priv->channels[5].base + DC_WR_CH_CONF);
+
+	writel(DC_GEN_SYNC_1_6_SYNC | DC_GEN_SYNC_PRIORITY_1, priv->dc_reg + DC_GEN);
+
+	ipu->dc_priv = priv;
+
+	dev_dbg(dev, "DC base: 0x%08lx template base: 0x%08lx\n",
+			base, template_base);
+
+	/* rgb24 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_RGB24);
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB24, 0, 7, 0xff); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB24, 1, 15, 0xff); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB24, 2, 23, 0xff); /* red */
+
+	/* rgb565 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_RGB565);
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB565, 0, 4, 0xf8); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB565, 1, 10, 0xfc); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_RGB565, 2, 15, 0xf8); /* red */
+
+	return 0;
+}
+
+void ipu_dc_exit(struct ipu_soc *ipu)
+{
+}
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-di.c b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
new file mode 100644
index 000000000000..67d974f7be36
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
@@ -0,0 +1,700 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+
+#include "imx-ipu-v3.h"
+#include "ipu-prv.h"
+
+struct ipu_di {
+	void __iomem *base;
+	int id;
+	u32 module;
+	struct clk *clk_di;	/* display input clock */
+	struct clk *clk_ipu;	/* IPU bus clock */
+	struct clk *clk_di_pixel; /* resulting pixel clock */
+	struct clk_hw clk_hw_out;
+	char *clk_name;
+	bool inuse;
+	unsigned long clkflags;
+	struct ipu_soc *ipu;
+};
+
+static DEFINE_MUTEX(di_mutex);
+
+struct di_sync_config {
+	int run_count;
+	int run_src;
+	int offset_count;
+	int offset_src;
+	int repeat_count;
+	int cnt_clr_src;
+	int cnt_polarity_gen_en;
+	int cnt_polarity_clr_src;
+	int cnt_polarity_trigger_src;
+	int cnt_up;
+	int cnt_down;
+};
+
+enum di_pins {
+	DI_PIN11 = 0,
+	DI_PIN12 = 1,
+	DI_PIN13 = 2,
+	DI_PIN14 = 3,
+	DI_PIN15 = 4,
+	DI_PIN16 = 5,
+	DI_PIN17 = 6,
+	DI_PIN_CS = 7,
+
+	DI_PIN_SER_CLK = 0,
+	DI_PIN_SER_RS = 1,
+};
+
+enum di_sync_wave {
+	DI_SYNC_NONE = 0,
+	DI_SYNC_CLK = 1,
+	DI_SYNC_INT_HSYNC = 2,
+	DI_SYNC_HSYNC = 3,
+	DI_SYNC_VSYNC = 4,
+	DI_SYNC_DE = 6,
+};
+
+#define SYNC_WAVE 0
+
+#define DI_GENERAL		0x0000
+#define DI_BS_CLKGEN0		0x0004
+#define DI_BS_CLKGEN1		0x0008
+#define DI_SW_GEN0(gen)		(0x000c + 4 * ((gen) - 1))
+#define DI_SW_GEN1(gen)		(0x0030 + 4 * ((gen) - 1))
+#define DI_STP_REP(gen)		(0x0148 + 4 * (((gen) - 1)/2))
+#define DI_SYNC_AS_GEN		0x0054
+#define DI_DW_GEN(gen)		(0x0058 + 4 * (gen))
+#define DI_DW_SET(gen, set)	(0x0088 + 4 * ((gen) + 0xc * (set)))
+#define DI_SER_CONF		0x015c
+#define DI_SSC			0x0160
+#define DI_POL			0x0164
+#define DI_AW0			0x0168
+#define DI_AW1			0x016c
+#define DI_SCR_CONF		0x0170
+#define DI_STAT			0x0174
+
+#define DI_SW_GEN0_RUN_COUNT(x)			((x) << 19)
+#define DI_SW_GEN0_RUN_SRC(x)			((x) << 16)
+#define DI_SW_GEN0_OFFSET_COUNT(x)		((x) << 3)
+#define DI_SW_GEN0_OFFSET_SRC(x)		((x) << 0)
+
+#define DI_SW_GEN1_CNT_POL_GEN_EN(x)		((x) << 29)
+#define DI_SW_GEN1_CNT_CLR_SRC(x)		((x) << 25)
+#define DI_SW_GEN1_CNT_POL_TRIGGER_SRC(x)	((x) << 12)
+#define DI_SW_GEN1_CNT_POL_CLR_SRC(x)		((x) << 9)
+#define DI_SW_GEN1_CNT_DOWN(x)			((x) << 16)
+#define DI_SW_GEN1_CNT_UP(x)			(x)
+#define DI_SW_GEN1_AUTO_RELOAD			(0x10000000)
+
+#define DI_DW_GEN_ACCESS_SIZE_OFFSET		24
+#define DI_DW_GEN_COMPONENT_SIZE_OFFSET		16
+
+#define DI_GEN_POLARITY_1			(1 << 0)
+#define DI_GEN_POLARITY_2			(1 << 1)
+#define DI_GEN_POLARITY_3			(1 << 2)
+#define DI_GEN_POLARITY_4			(1 << 3)
+#define DI_GEN_POLARITY_5			(1 << 4)
+#define DI_GEN_POLARITY_6			(1 << 5)
+#define DI_GEN_POLARITY_7			(1 << 6)
+#define DI_GEN_POLARITY_8			(1 << 7)
+#define DI_GEN_POLARITY_DISP_CLK		(1 << 17)
+#define DI_GEN_DI_CLK_EXT			(1 << 20)
+#define DI_GEN_DI_VSYNC_EXT			(1 << 21)
+
+#define DI_POL_DRDY_DATA_POLARITY		(1 << 7)
+#define DI_POL_DRDY_POLARITY_15			(1 << 4)
+
+#define DI_VSYNC_SEL_OFFSET			13
+
+static inline u32 ipu_di_read(struct ipu_di *di, unsigned offset)
+{
+	return readl(di->base + offset);
+}
+
+static inline void ipu_di_write(struct ipu_di *di, u32 value, unsigned offset)
+{
+	writel(value, di->base + offset);
+}
+
+static int ipu_di_clk_calc_div(unsigned long inrate, unsigned long outrate)
+{
+	u64 tmp = inrate;
+	int div;
+
+	tmp *= 16;
+
+	do_div(tmp, outrate);
+
+	div = tmp;
+
+	if (div < 0x10)
+		div = 0x10;
+
+#ifdef WTF_IS_THIS
+	/*
+	 * Freescale has this in their Kernel. It is neither clear what
+	 * it does nor why it does it
+	 */
+	if (div & 0x10)
+		div &= ~0x7;
+	else {
+		/* Round up divider if it gets us closer to desired pix clk */
+		if ((div & 0xC) == 0xC) {
+			div += 0x10;
+			div &= ~0xF;
+		}
+	}
+#endif
+	return div;
+}
+
+static unsigned long clk_di_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct ipu_di *di = container_of(hw, struct ipu_di, clk_hw_out);
+	unsigned long outrate;
+	u32 div = ipu_di_read(di, DI_BS_CLKGEN0);
+
+	if (div < 0x10)
+		div = 0x10;
+
+	outrate = (parent_rate / div) * 16;
+
+	return outrate;
+}
+
+static long clk_di_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct ipu_di *di = container_of(hw, struct ipu_di, clk_hw_out);
+	unsigned long outrate;
+	int div;
+	u32 val;
+
+	div = ipu_di_clk_calc_div(*prate, rate);
+
+	outrate = (*prate / div) * 16;
+
+	val = ipu_di_read(di, DI_GENERAL);
+
+	if (!(val & DI_GEN_DI_CLK_EXT) && outrate > *prate / 2)
+		outrate = *prate / 2;
+
+	dev_dbg(di->ipu->dev,
+		"%s: inrate: %ld div: 0x%08x outrate: %ld wanted: %ld\n",
+			__func__, *prate, div, outrate, rate);
+
+	return outrate;
+}
+
+static int clk_di_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct ipu_di *di = container_of(hw, struct ipu_di, clk_hw_out);
+	int div;
+	u32 clkgen0;
+
+	clkgen0 = ipu_di_read(di, DI_BS_CLKGEN0) & ~0xfff;
+
+	div = ipu_di_clk_calc_div(parent_rate, rate);
+
+	ipu_di_write(di, clkgen0 | div, DI_BS_CLKGEN0);
+
+	dev_dbg(di->ipu->dev, "%s: inrate: %ld desired: %ld div: 0x%08x\n",
+			__func__, parent_rate, rate, div);
+	return 0;
+}
+
+static u8 clk_di_get_parent(struct clk_hw *hw)
+{
+	struct ipu_di *di = container_of(hw, struct ipu_di, clk_hw_out);
+	u32 val;
+
+	val = ipu_di_read(di, DI_GENERAL);
+
+	return val & DI_GEN_DI_CLK_EXT ? 1 : 0;
+}
+
+static int clk_di_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct ipu_di *di = container_of(hw, struct ipu_di, clk_hw_out);
+	u32 val;
+
+	val = ipu_di_read(di, DI_GENERAL);
+
+	if (index)
+		val |= DI_GEN_DI_CLK_EXT;
+	else
+		val &= ~DI_GEN_DI_CLK_EXT;
+
+	ipu_di_write(di, val, DI_GENERAL);
+
+	return 0;
+}
+
+static struct clk_ops clk_di_ops = {
+	.round_rate = clk_di_round_rate,
+	.set_rate = clk_di_set_rate,
+	.recalc_rate = clk_di_recalc_rate,
+	.set_parent = clk_di_set_parent,
+	.get_parent = clk_di_get_parent,
+};
+
+static void ipu_di_data_wave_config(struct ipu_di *di,
+				     int wave_gen,
+				     int access_size, int component_size)
+{
+	u32 reg;
+	reg = (access_size << DI_DW_GEN_ACCESS_SIZE_OFFSET) |
+	    (component_size << DI_DW_GEN_COMPONENT_SIZE_OFFSET);
+	ipu_di_write(di, reg, DI_DW_GEN(wave_gen));
+}
+
+static void ipu_di_data_pin_config(struct ipu_di *di, int wave_gen, int di_pin,
+		int set, int up, int down)
+{
+	u32 reg;
+
+	reg = ipu_di_read(di, DI_DW_GEN(wave_gen));
+	reg &= ~(0x3 << (di_pin * 2));
+	reg |= set << (di_pin * 2);
+	ipu_di_write(di, reg, DI_DW_GEN(wave_gen));
+
+	ipu_di_write(di, (down << 16) | up, DI_DW_SET(wave_gen, set));
+}
+
+static void ipu_di_sync_config(struct ipu_di *di, struct di_sync_config *config,
+		int start, int count)
+{
+	u32 reg;
+	int i;
+
+	for (i = 0; i < count; i++) {
+		struct di_sync_config *c = &config[i];
+		int wave_gen = start + i + 1;
+
+		if ((c->run_count >= 0x1000) || (c->offset_count >= 0x1000) ||
+				(c->repeat_count >= 0x1000) ||
+				(c->cnt_up >= 0x400) ||
+				(c->cnt_down >= 0x400)) {
+			dev_err(di->ipu->dev, "DI%d counters out of range.\n",
+					di->id);
+			return;
+		}
+
+		reg = DI_SW_GEN0_RUN_COUNT(c->run_count) |
+			DI_SW_GEN0_RUN_SRC(c->run_src) |
+			DI_SW_GEN0_OFFSET_COUNT(c->offset_count) |
+			DI_SW_GEN0_OFFSET_SRC(c->offset_src);
+		ipu_di_write(di, reg, DI_SW_GEN0(wave_gen));
+
+		reg = DI_SW_GEN1_CNT_POL_GEN_EN(c->cnt_polarity_gen_en) |
+			DI_SW_GEN1_CNT_CLR_SRC(c->cnt_clr_src) |
+			DI_SW_GEN1_CNT_POL_TRIGGER_SRC(
+					c->cnt_polarity_trigger_src) |
+			DI_SW_GEN1_CNT_POL_CLR_SRC(c->cnt_polarity_clr_src) |
+			DI_SW_GEN1_CNT_DOWN(c->cnt_down) |
+			DI_SW_GEN1_CNT_UP(c->cnt_up);
+
+		/* Enable auto reload */
+		if (c->repeat_count == 0)
+			reg |= DI_SW_GEN1_AUTO_RELOAD;
+
+		ipu_di_write(di, reg, DI_SW_GEN1(wave_gen));
+
+		reg = ipu_di_read(di, DI_STP_REP(wave_gen));
+		reg &= ~(0xffff << (16 * ((wave_gen - 1) & 0x1)));
+		reg |= c->repeat_count << (16 * ((wave_gen - 1) & 0x1));
+		ipu_di_write(di, reg, DI_STP_REP(wave_gen));
+	}
+}
+
+static void ipu_di_sync_config_interlaced(struct ipu_di *di,
+		struct ipu_di_signal_cfg *sig)
+{
+	u32 h_total = sig->width + sig->h_sync_width +
+		sig->h_start_width + sig->h_end_width;
+	u32 v_total = sig->height + sig->v_sync_width +
+		sig->v_start_width + sig->v_end_width;
+	u32 reg;
+	struct di_sync_config cfg[] = {
+		{
+			.run_count = h_total / 2 - 1,
+			.run_src = DI_SYNC_CLK,
+		}, {
+			.run_count = h_total - 11,
+			.run_src = DI_SYNC_CLK,
+			.cnt_down = 4,
+		}, {
+			.run_count = v_total * 2 - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+			.offset_count = 1,
+			.offset_src = DI_SYNC_INT_HSYNC,
+			.cnt_down = 4,
+		}, {
+			.run_count = v_total / 2 - 1,
+			.run_src = DI_SYNC_HSYNC,
+			.offset_count = sig->v_start_width,
+			.offset_src = DI_SYNC_HSYNC,
+			.repeat_count = 2,
+			.cnt_clr_src = DI_SYNC_VSYNC,
+		}, {
+			.run_src = DI_SYNC_HSYNC,
+			.repeat_count = sig->height / 2,
+			.cnt_clr_src = 4,
+		}, {
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_HSYNC,
+		}, {
+			.run_count = v_total / 2 - 1,
+			.run_src = DI_SYNC_HSYNC,
+			.offset_count = 9,
+			.offset_src = DI_SYNC_HSYNC,
+			.repeat_count = 2,
+			.cnt_clr_src = DI_SYNC_VSYNC,
+		}, {
+			.run_src = DI_SYNC_CLK,
+			.offset_count = sig->h_start_width,
+			.offset_src = DI_SYNC_CLK,
+			.repeat_count = sig->width,
+			.cnt_clr_src = 5,
+		}, {
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+			.offset_count = v_total / 2,
+			.offset_src = DI_SYNC_INT_HSYNC,
+			.cnt_clr_src = DI_SYNC_HSYNC,
+			.cnt_down = 4,
+		}
+	};
+
+	ipu_di_sync_config(di, cfg, 0, ARRAY_SIZE(cfg));
+
+	/* set gentime select and tag sel */
+	reg = ipu_di_read(di, DI_SW_GEN1(9));
+	reg &= 0x1FFFFFFF;
+	reg |= (3 - 1) << 29 | 0x00008000;
+	ipu_di_write(di, reg, DI_SW_GEN1(9));
+
+	ipu_di_write(di, v_total / 2 - 1, DI_SCR_CONF);
+}
+
+static void ipu_di_sync_config_noninterlaced(struct ipu_di *di,
+		struct ipu_di_signal_cfg *sig, int div)
+{
+	u32 h_total = sig->width + sig->h_sync_width + sig->h_start_width +
+		sig->h_end_width;
+	u32 v_total = sig->height + sig->v_sync_width + sig->v_start_width +
+		sig->v_end_width;
+	struct di_sync_config cfg[] = {
+		{
+			.run_count = h_total - 1,
+			.run_src = DI_SYNC_CLK,
+		} , {
+			.run_count = h_total - 1,
+			.run_src = DI_SYNC_CLK,
+			.offset_count = div * sig->v_to_h_sync,
+			.offset_src = DI_SYNC_CLK,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_CLK,
+			.cnt_down = sig->h_sync_width * 2,
+		} , {
+			.run_count = v_total - 1,
+			.run_src = DI_SYNC_INT_HSYNC,
+			.cnt_polarity_gen_en = 1,
+			.cnt_polarity_trigger_src = DI_SYNC_INT_HSYNC,
+			.cnt_down = sig->v_sync_width * 2,
+		} , {
+			.run_src = DI_SYNC_HSYNC,
+			.offset_count = sig->v_sync_width + sig->v_start_width,
+			.offset_src = DI_SYNC_HSYNC,
+			.repeat_count = sig->height,
+			.cnt_clr_src = DI_SYNC_VSYNC,
+		} , {
+			.run_src = DI_SYNC_CLK,
+			.offset_count = sig->h_sync_width + sig->h_start_width,
+			.offset_src = DI_SYNC_CLK,
+			.repeat_count = sig->width,
+			.cnt_clr_src = 5,
+		} , {
+			/* unused */
+		} , {
+			/* unused */
+		} , {
+			/* unused */
+		} , {
+			/* unused */
+		},
+	};
+
+	ipu_di_write(di, v_total - 1, DI_SCR_CONF);
+	ipu_di_sync_config(di, cfg, 0, ARRAY_SIZE(cfg));
+}
+
+int ipu_di_init_sync_panel(struct ipu_di *di, struct ipu_di_signal_cfg *sig)
+{
+	u32 reg;
+	u32 di_gen, vsync_cnt;
+	u32 div;
+	u32 h_total, v_total;
+	int ret;
+	unsigned long round;
+	struct clk *parent;
+
+	dev_dbg(di->ipu->dev, "disp %d: panel size = %d x %d\n",
+		di->id, sig->width, sig->height);
+
+	if ((sig->v_sync_width == 0) || (sig->h_sync_width == 0))
+		return -EINVAL;
+
+	if (sig->clkflags & IPU_DI_CLKMODE_EXT)
+		parent = di->clk_di;
+	else
+		parent = di->clk_ipu;
+
+	ret = clk_set_parent(di->clk_di_pixel, parent);
+	if (ret) {
+		dev_err(di->ipu->dev,
+			"setting pixel clock to parent %s failed with %d\n",
+				__clk_get_name(parent), ret);
+		return ret;
+	}
+
+	if (sig->clkflags & IPU_DI_CLKMODE_SYNC)
+		round = clk_get_rate(parent);
+	else
+		round = clk_round_rate(di->clk_di_pixel, sig->pixelclock);
+
+	ret = clk_set_rate(di->clk_di_pixel, round);
+
+	h_total = sig->width + sig->h_sync_width + sig->h_start_width +
+		sig->h_end_width;
+	v_total = sig->height + sig->v_sync_width + sig->v_start_width +
+		sig->v_end_width;
+
+	mutex_lock(&di_mutex);
+
+	div = ipu_di_read(di, DI_BS_CLKGEN0) & 0xfff;
+	div = div / 16;		/* Now divider is integer portion */
+
+	/* Setup pixel clock timing */
+	/* Down time is half of period */
+	ipu_di_write(di, (div << 16), DI_BS_CLKGEN1);
+
+	ipu_di_data_wave_config(di, SYNC_WAVE, div - 1, div - 1);
+	ipu_di_data_pin_config(di, SYNC_WAVE, DI_PIN15, 3, 0, div * 2);
+
+	di_gen = ipu_di_read(di, DI_GENERAL) & DI_GEN_DI_CLK_EXT;
+	di_gen |= DI_GEN_DI_VSYNC_EXT;
+
+	if (sig->interlaced) {
+		ipu_di_sync_config_interlaced(di, sig);
+
+		/* set y_sel = 1 */
+		di_gen |= 0x10000000;
+		di_gen |= DI_GEN_POLARITY_5;
+		di_gen |= DI_GEN_POLARITY_8;
+
+		vsync_cnt = 7;
+
+		if (sig->Hsync_pol)
+			di_gen |= DI_GEN_POLARITY_3;
+		if (sig->Vsync_pol)
+			di_gen |= DI_GEN_POLARITY_2;
+	} else {
+		ipu_di_sync_config_noninterlaced(di, sig, div);
+
+		vsync_cnt = 3;
+
+		if (sig->Hsync_pol)
+			di_gen |= DI_GEN_POLARITY_2;
+		if (sig->Vsync_pol)
+			di_gen |= DI_GEN_POLARITY_3;
+	}
+
+	if (!sig->clk_pol)
+		di_gen |= DI_GEN_POLARITY_DISP_CLK;
+
+	ipu_di_write(di, di_gen, DI_GENERAL);
+
+	ipu_di_write(di, (--vsync_cnt << DI_VSYNC_SEL_OFFSET) | 0x00000002,
+		     DI_SYNC_AS_GEN);
+
+	reg = ipu_di_read(di, DI_POL);
+	reg &= ~(DI_POL_DRDY_DATA_POLARITY | DI_POL_DRDY_POLARITY_15);
+
+	if (sig->enable_pol)
+		reg |= DI_POL_DRDY_POLARITY_15;
+	if (sig->data_pol)
+		reg |= DI_POL_DRDY_DATA_POLARITY;
+
+	ipu_di_write(di, reg, DI_POL);
+
+	mutex_unlock(&di_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_di_init_sync_panel);
+
+int ipu_di_enable(struct ipu_di *di)
+{
+	clk_prepare_enable(di->clk_di_pixel);
+
+	ipu_module_enable(di->ipu, di->module);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_di_enable);
+
+int ipu_di_disable(struct ipu_di *di)
+{
+	ipu_module_disable(di->ipu, di->module);
+
+	clk_disable_unprepare(di->clk_di_pixel);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_di_disable);
+
+int ipu_di_get_num(struct ipu_di *di)
+{
+	return di->id;
+}
+EXPORT_SYMBOL_GPL(ipu_di_get_num);
+
+static DEFINE_MUTEX(ipu_di_lock);
+
+struct ipu_di *ipu_di_get(struct ipu_soc *ipu, int disp)
+{
+	struct ipu_di *di;
+
+	if (disp > 1)
+		return ERR_PTR(-EINVAL);
+
+	di = ipu->di_priv[disp];
+
+	mutex_lock(&ipu_di_lock);
+
+	if (di->inuse) {
+		di = ERR_PTR(-EBUSY);
+		goto out;
+	}
+
+	di->inuse = true;
+out:
+	mutex_unlock(&ipu_di_lock);
+
+	return di;
+}
+EXPORT_SYMBOL_GPL(ipu_di_get);
+
+void ipu_di_put(struct ipu_di *di)
+{
+	mutex_lock(&ipu_di_lock);
+
+	di->inuse = false;
+
+	mutex_unlock(&ipu_di_lock);
+}
+EXPORT_SYMBOL_GPL(ipu_di_put);
+
+int ipu_di_init(struct ipu_soc *ipu, struct device *dev, int id,
+		unsigned long base,
+		u32 module, struct clk *clk_ipu)
+{
+	struct ipu_di *di;
+	int ret;
+	const char *di_parent[2];
+	struct clk_init_data init = {
+		.ops = &clk_di_ops,
+		.num_parents = 2,
+		.flags = 0,
+	};
+
+	if (id > 1)
+		return -ENODEV;
+
+	di = devm_kzalloc(dev, sizeof(*di), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	ipu->di_priv[id] = di;
+
+	di->clk_di = devm_clk_get(dev, id ? "di1" : "di0");
+	if (IS_ERR(di->clk_di))
+		return PTR_ERR(di->clk_di);
+
+	di->module = module;
+	di->id = id;
+	di->clk_ipu = clk_ipu;
+	di->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!di->base)
+		return -ENOMEM;
+
+	di_parent[0] = __clk_get_name(di->clk_ipu);
+	di_parent[1] = __clk_get_name(di->clk_di);
+
+	ipu_di_write(di, 0x10, DI_BS_CLKGEN0);
+
+	init.parent_names = (const char **)&di_parent;
+	di->clk_name = kasprintf(GFP_KERNEL, "%s_di%d_pixel",
+			dev_name(dev), id);
+	if (!di->clk_name)
+		return -ENOMEM;
+
+	init.name = di->clk_name;
+
+	di->clk_hw_out.init = &init;
+	di->clk_di_pixel = clk_register(dev, &di->clk_hw_out);
+
+	if (IS_ERR(di->clk_di_pixel)) {
+		ret = PTR_ERR(di->clk_di_pixel);
+		goto failed_clk_register;
+	}
+
+	dev_info(dev, "DI%d base: 0x%08lx remapped to %p\n",
+			id, base, di->base);
+	di->inuse = false;
+	di->ipu = ipu;
+
+	return 0;
+
+failed_clk_register:
+
+	kfree(di->clk_name);
+
+	return ret;
+}
+
+void ipu_di_exit(struct ipu_soc *ipu, int id)
+{
+	struct ipu_di *di = ipu->di_priv[id];
+
+	clk_unregister(di->clk_di_pixel);
+	kfree(di->clk_name);
+}
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
new file mode 100644
index 000000000000..91821bc30f41
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
@@ -0,0 +1,408 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#include <linux/export.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+
+#include "imx-ipu-v3.h"
+#include "ipu-prv.h"
+
+#define DMFC_RD_CHAN		0x0000
+#define DMFC_WR_CHAN		0x0004
+#define DMFC_WR_CHAN_DEF	0x0008
+#define DMFC_DP_CHAN		0x000c
+#define DMFC_DP_CHAN_DEF	0x0010
+#define DMFC_GENERAL1		0x0014
+#define DMFC_GENERAL2		0x0018
+#define DMFC_IC_CTRL		0x001c
+#define DMFC_STAT		0x0020
+
+#define DMFC_WR_CHAN_1_28		0
+#define DMFC_WR_CHAN_2_41		8
+#define DMFC_WR_CHAN_1C_42		16
+#define DMFC_WR_CHAN_2C_43		24
+
+#define DMFC_DP_CHAN_5B_23		0
+#define DMFC_DP_CHAN_5F_27		8
+#define DMFC_DP_CHAN_6B_24		16
+#define DMFC_DP_CHAN_6F_29		24
+
+#define DMFC_FIFO_SIZE_64		(3 << 3)
+#define DMFC_FIFO_SIZE_128		(2 << 3)
+#define DMFC_FIFO_SIZE_256		(1 << 3)
+#define DMFC_FIFO_SIZE_512		(0 << 3)
+
+#define DMFC_SEGMENT(x)			((x & 0x7) << 0)
+#define DMFC_BURSTSIZE_128		(0 << 6)
+#define DMFC_BURSTSIZE_64		(1 << 6)
+#define DMFC_BURSTSIZE_32		(2 << 6)
+#define DMFC_BURSTSIZE_16		(3 << 6)
+
+struct dmfc_channel_data {
+	int		ipu_channel;
+	unsigned long	channel_reg;
+	unsigned long	shift;
+	unsigned	eot_shift;
+	unsigned	max_fifo_lines;
+};
+
+static const struct dmfc_channel_data dmfcdata[] = {
+	{
+		.ipu_channel	= 23,
+		.channel_reg	= DMFC_DP_CHAN,
+		.shift		= DMFC_DP_CHAN_5B_23,
+		.eot_shift	= 20,
+		.max_fifo_lines	= 3,
+	}, {
+		.ipu_channel	= 24,
+		.channel_reg	= DMFC_DP_CHAN,
+		.shift		= DMFC_DP_CHAN_6B_24,
+		.eot_shift	= 22,
+		.max_fifo_lines	= 1,
+	}, {
+		.ipu_channel	= 27,
+		.channel_reg	= DMFC_DP_CHAN,
+		.shift		= DMFC_DP_CHAN_5F_27,
+		.eot_shift	= 21,
+		.max_fifo_lines	= 2,
+	}, {
+		.ipu_channel	= 28,
+		.channel_reg	= DMFC_WR_CHAN,
+		.shift		= DMFC_WR_CHAN_1_28,
+		.eot_shift	= 16,
+		.max_fifo_lines	= 2,
+	}, {
+		.ipu_channel	= 29,
+		.channel_reg	= DMFC_DP_CHAN,
+		.shift		= DMFC_DP_CHAN_6F_29,
+		.eot_shift	= 23,
+		.max_fifo_lines	= 1,
+	},
+};
+
+#define DMFC_NUM_CHANNELS	ARRAY_SIZE(dmfcdata)
+
+struct ipu_dmfc_priv;
+
+struct dmfc_channel {
+	unsigned			slots;
+	unsigned			slotmask;
+	unsigned			segment;
+	int				burstsize;
+	struct ipu_soc			*ipu;
+	struct ipu_dmfc_priv		*priv;
+	const struct dmfc_channel_data	*data;
+};
+
+struct ipu_dmfc_priv {
+	struct ipu_soc *ipu;
+	struct device *dev;
+	struct dmfc_channel channels[DMFC_NUM_CHANNELS];
+	struct mutex mutex;
+	unsigned long bandwidth_per_slot;
+	void __iomem *base;
+	int use_count;
+};
+
+int ipu_dmfc_enable_channel(struct dmfc_channel *dmfc)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+	mutex_lock(&priv->mutex);
+
+	if (!priv->use_count)
+		ipu_module_enable(priv->ipu, IPU_CONF_DMFC_EN);
+
+	priv->use_count++;
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_enable_channel);
+
+void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+
+	mutex_lock(&priv->mutex);
+
+	priv->use_count--;
+
+	if (!priv->use_count)
+		ipu_module_disable(priv->ipu, IPU_CONF_DMFC_EN);
+
+	if (priv->use_count < 0)
+		priv->use_count = 0;
+
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_disable_channel);
+
+static int ipu_dmfc_setup_channel(struct dmfc_channel *dmfc, int slots,
+		int segment, int burstsize)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+	u32 val, field;
+
+	dev_dbg(priv->dev,
+			"dmfc: using %d slots starting from segment %d for IPU channel %d\n",
+			slots, segment, dmfc->data->ipu_channel);
+
+	if (!dmfc)
+		return -EINVAL;
+
+	switch (slots) {
+	case 1:
+		field = DMFC_FIFO_SIZE_64;
+		break;
+	case 2:
+		field = DMFC_FIFO_SIZE_128;
+		break;
+	case 4:
+		field = DMFC_FIFO_SIZE_256;
+		break;
+	case 8:
+		field = DMFC_FIFO_SIZE_512;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (burstsize) {
+	case 16:
+		field |= DMFC_BURSTSIZE_16;
+		break;
+	case 32:
+		field |= DMFC_BURSTSIZE_32;
+		break;
+	case 64:
+		field |= DMFC_BURSTSIZE_64;
+		break;
+	case 128:
+		field |= DMFC_BURSTSIZE_128;
+		break;
+	}
+
+	field |= DMFC_SEGMENT(segment);
+
+	val = readl(priv->base + dmfc->data->channel_reg);
+
+	val &= ~(0xff << dmfc->data->shift);
+	val |= field << dmfc->data->shift;
+
+	writel(val, priv->base + dmfc->data->channel_reg);
+
+	dmfc->slots = slots;
+	dmfc->segment = segment;
+	dmfc->burstsize = burstsize;
+	dmfc->slotmask = ((1 << slots) - 1) << segment;
+
+	return 0;
+}
+
+static int dmfc_bandwidth_to_slots(struct ipu_dmfc_priv *priv,
+		unsigned long bandwidth)
+{
+	int slots = 1;
+
+	while (slots * priv->bandwidth_per_slot < bandwidth)
+		slots *= 2;
+
+	return slots;
+}
+
+static int dmfc_find_slots(struct ipu_dmfc_priv *priv, int slots)
+{
+	unsigned slotmask_need, slotmask_used = 0;
+	int i, segment = 0;
+
+	slotmask_need = (1 << slots) - 1;
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++)
+		slotmask_used |= priv->channels[i].slotmask;
+
+	while (slotmask_need <= 0xff) {
+		if (!(slotmask_used & slotmask_need))
+			return segment;
+
+		slotmask_need <<= 1;
+		segment++;
+	}
+
+	return -EBUSY;
+}
+
+void ipu_dmfc_free_bandwidth(struct dmfc_channel *dmfc)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+	int i;
+
+	dev_dbg(priv->dev, "dmfc: freeing %d slots starting from segment %d\n",
+			dmfc->slots, dmfc->segment);
+
+	mutex_lock(&priv->mutex);
+
+	if (!dmfc->slots)
+		goto out;
+
+	dmfc->slotmask = 0;
+	dmfc->slots = 0;
+	dmfc->segment = 0;
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++)
+		priv->channels[i].slotmask = 0;
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++) {
+		if (priv->channels[i].slots > 0) {
+			priv->channels[i].segment =
+				dmfc_find_slots(priv, priv->channels[i].slots);
+			priv->channels[i].slotmask =
+				((1 << priv->channels[i].slots) - 1) <<
+				priv->channels[i].segment;
+		}
+	}
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++) {
+		if (priv->channels[i].slots > 0)
+			ipu_dmfc_setup_channel(&priv->channels[i],
+					priv->channels[i].slots,
+					priv->channels[i].segment,
+					priv->channels[i].burstsize);
+	}
+out:
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_free_bandwidth);
+
+int ipu_dmfc_alloc_bandwidth(struct dmfc_channel *dmfc,
+		unsigned long bandwidth_pixel_per_second, int burstsize)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+	int slots = dmfc_bandwidth_to_slots(priv, bandwidth_pixel_per_second);
+	int segment = 0, ret = 0;
+
+	dev_dbg(priv->dev, "dmfc: trying to allocate %ldMpixel/s for IPU channel %d\n",
+			bandwidth_pixel_per_second / 1000000,
+			dmfc->data->ipu_channel);
+
+	ipu_dmfc_free_bandwidth(dmfc);
+
+	mutex_lock(&priv->mutex);
+
+	if (slots > 8) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	segment = dmfc_find_slots(priv, slots);
+	if (segment < 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ipu_dmfc_setup_channel(dmfc, slots, segment, burstsize);
+
+out:
+	mutex_unlock(&priv->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_alloc_bandwidth);
+
+int ipu_dmfc_init_channel(struct dmfc_channel *dmfc, int width)
+{
+	struct ipu_dmfc_priv *priv = dmfc->priv;
+	u32 dmfc_gen1;
+
+	dmfc_gen1 = readl(priv->base + DMFC_GENERAL1);
+
+	if ((dmfc->slots * 64 * 4) / width > dmfc->data->max_fifo_lines)
+		dmfc_gen1 |= 1 << dmfc->data->eot_shift;
+	else
+		dmfc_gen1 &= ~(1 << dmfc->data->eot_shift);
+
+	writel(dmfc_gen1, priv->base + DMFC_GENERAL1);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_init_channel);
+
+struct dmfc_channel *ipu_dmfc_get(struct ipu_soc *ipu, int ipu_channel)
+{
+	struct ipu_dmfc_priv *priv = ipu->dmfc_priv;
+	int i;
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++)
+		if (dmfcdata[i].ipu_channel == ipu_channel)
+			return &priv->channels[i];
+	return ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_get);
+
+void ipu_dmfc_put(struct dmfc_channel *dmfc)
+{
+	ipu_dmfc_free_bandwidth(dmfc);
+}
+EXPORT_SYMBOL_GPL(ipu_dmfc_put);
+
+int ipu_dmfc_init(struct ipu_soc *ipu, struct device *dev, unsigned long base,
+		struct clk *ipu_clk)
+{
+	struct ipu_dmfc_priv *priv;
+	int i;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!priv->base)
+		return -ENOMEM;
+
+	priv->dev = dev;
+	priv->ipu = ipu;
+	mutex_init(&priv->mutex);
+
+	ipu->dmfc_priv = priv;
+
+	for (i = 0; i < DMFC_NUM_CHANNELS; i++) {
+		priv->channels[i].priv = priv;
+		priv->channels[i].ipu = ipu;
+		priv->channels[i].data = &dmfcdata[i];
+	}
+
+	writel(0x0, priv->base + DMFC_WR_CHAN);
+	writel(0x0, priv->base + DMFC_DP_CHAN);
+
+	/*
+	 * We have a total bandwidth of clkrate * 4pixel divided
+	 * into 8 slots.
+	 */
+	priv->bandwidth_per_slot = clk_get_rate(ipu_clk) / 8;
+
+	dev_dbg(dev, "dmfc: 8 slots with %ldMpixel/s bandwidth each\n",
+			priv->bandwidth_per_slot / 1000000);
+
+	writel(0x202020f6, priv->base + DMFC_WR_CHAN_DEF);
+	writel(0x2020f6f6, priv->base + DMFC_DP_CHAN_DEF);
+	writel(0x00000003, priv->base + DMFC_GENERAL1);
+
+	return 0;
+}
+
+void ipu_dmfc_exit(struct ipu_soc *ipu)
+{
+}
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
new file mode 100644
index 000000000000..26aecaf9677f
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
@@ -0,0 +1,336 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#include "imx-ipu-v3.h"
+#include "ipu-prv.h"
+
+#define DP_SYNC 0
+#define DP_ASYNC0 0x60
+#define DP_ASYNC1 0xBC
+
+#define DP_COM_CONF		0x0
+#define DP_GRAPH_WIND_CTRL	0x0004
+#define DP_FG_POS		0x0008
+#define DP_CSC_A_0		0x0044
+#define DP_CSC_A_1		0x0048
+#define DP_CSC_A_2		0x004C
+#define DP_CSC_A_3		0x0050
+#define DP_CSC_0		0x0054
+#define DP_CSC_1		0x0058
+
+#define DP_COM_CONF_FG_EN		(1 << 0)
+#define DP_COM_CONF_GWSEL		(1 << 1)
+#define DP_COM_CONF_GWAM		(1 << 2)
+#define DP_COM_CONF_GWCKE		(1 << 3)
+#define DP_COM_CONF_CSC_DEF_MASK	(3 << 8)
+#define DP_COM_CONF_CSC_DEF_OFFSET	8
+#define DP_COM_CONF_CSC_DEF_FG		(3 << 8)
+#define DP_COM_CONF_CSC_DEF_BG		(2 << 8)
+#define DP_COM_CONF_CSC_DEF_BOTH	(1 << 8)
+
+struct ipu_dp_priv;
+
+struct ipu_dp {
+	u32 flow;
+	bool in_use;
+	bool foreground;
+	enum ipu_color_space in_cs;
+};
+
+struct ipu_flow {
+	struct ipu_dp foreground;
+	struct ipu_dp background;
+	enum ipu_color_space out_cs;
+	void __iomem *base;
+	struct ipu_dp_priv *priv;
+};
+
+struct ipu_dp_priv {
+	struct ipu_soc *ipu;
+	struct device *dev;
+	void __iomem *base;
+	struct ipu_flow flow[3];
+	struct mutex mutex;
+	int use_count;
+};
+
+static u32 ipu_dp_flow_base[] = {DP_SYNC, DP_ASYNC0, DP_ASYNC1};
+
+static inline struct ipu_flow *to_flow(struct ipu_dp *dp)
+{
+	if (dp->foreground)
+		return container_of(dp, struct ipu_flow, foreground);
+	else
+		return container_of(dp, struct ipu_flow, background);
+}
+
+int ipu_dp_set_global_alpha(struct ipu_dp *dp, bool enable,
+		u8 alpha, bool bg_chan)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+	u32 reg;
+
+	mutex_lock(&priv->mutex);
+
+	reg = readl(flow->base + DP_COM_CONF);
+	if (bg_chan)
+		reg &= ~DP_COM_CONF_GWSEL;
+	else
+		reg |= DP_COM_CONF_GWSEL;
+	writel(reg, flow->base + DP_COM_CONF);
+
+	if (enable) {
+		reg = readl(flow->base + DP_GRAPH_WIND_CTRL) & 0x00FFFFFFL;
+		writel(reg | ((u32) alpha << 24),
+			     flow->base + DP_GRAPH_WIND_CTRL);
+
+		reg = readl(flow->base + DP_COM_CONF);
+		writel(reg | DP_COM_CONF_GWAM, flow->base + DP_COM_CONF);
+	} else {
+		reg = readl(flow->base + DP_COM_CONF);
+		writel(reg & ~DP_COM_CONF_GWAM, flow->base + DP_COM_CONF);
+	}
+
+	ipu_srm_dp_sync_update(priv->ipu);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_set_global_alpha);
+
+int ipu_dp_set_window_pos(struct ipu_dp *dp, u16 x_pos, u16 y_pos)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+
+	writel((x_pos << 16) | y_pos, flow->base + DP_FG_POS);
+
+	ipu_srm_dp_sync_update(priv->ipu);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_set_window_pos);
+
+static void ipu_dp_csc_init(struct ipu_flow *flow,
+		enum ipu_color_space in,
+		enum ipu_color_space out,
+		u32 place)
+{
+	u32 reg;
+
+	reg = readl(flow->base + DP_COM_CONF);
+	reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+
+	if (in == out) {
+		writel(reg, flow->base + DP_COM_CONF);
+		return;
+	}
+
+	if (in == IPUV3_COLORSPACE_RGB && out == IPUV3_COLORSPACE_YUV) {
+		writel(0x099 | (0x12d << 16), flow->base + DP_CSC_A_0);
+		writel(0x03a | (0x3a9 << 16), flow->base + DP_CSC_A_1);
+		writel(0x356 | (0x100 << 16), flow->base + DP_CSC_A_2);
+		writel(0x100 | (0x329 << 16), flow->base + DP_CSC_A_3);
+		writel(0x3d6 | (0x0000 << 16) | (2 << 30),
+				flow->base + DP_CSC_0);
+		writel(0x200 | (2 << 14) | (0x200 << 16) | (2 << 30),
+				flow->base + DP_CSC_1);
+	} else {
+		writel(0x095 | (0x000 << 16), flow->base + DP_CSC_A_0);
+		writel(0x0cc | (0x095 << 16), flow->base + DP_CSC_A_1);
+		writel(0x3ce | (0x398 << 16), flow->base + DP_CSC_A_2);
+		writel(0x095 | (0x0ff << 16), flow->base + DP_CSC_A_3);
+		writel(0x000 | (0x3e42 << 16) | (1 << 30),
+				flow->base + DP_CSC_0);
+		writel(0x10a | (1 << 14) | (0x3dd6 << 16) | (1 << 30),
+				flow->base + DP_CSC_1);
+	}
+
+	reg |= place;
+
+	writel(reg, flow->base + DP_COM_CONF);
+}
+
+int ipu_dp_setup_channel(struct ipu_dp *dp,
+		enum ipu_color_space in,
+		enum ipu_color_space out)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+
+	mutex_lock(&priv->mutex);
+
+	dp->in_cs = in;
+
+	if (!dp->foreground)
+		flow->out_cs = out;
+
+	if (flow->foreground.in_cs == flow->background.in_cs) {
+		/*
+		 * foreground and background are of same colorspace, put
+		 * colorspace converter after combining unit.
+		 */
+		ipu_dp_csc_init(flow, flow->foreground.in_cs, flow->out_cs,
+				DP_COM_CONF_CSC_DEF_BOTH);
+	} else {
+		if (flow->foreground.in_cs == flow->out_cs)
+			/*
+			 * foreground identical to output, apply color
+			 * conversion on background
+			 */
+			ipu_dp_csc_init(flow, flow->background.in_cs,
+					flow->out_cs, DP_COM_CONF_CSC_DEF_BG);
+		else
+			ipu_dp_csc_init(flow, flow->foreground.in_cs,
+					flow->out_cs, DP_COM_CONF_CSC_DEF_FG);
+	}
+
+	ipu_srm_dp_sync_update(priv->ipu);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_setup_channel);
+
+int ipu_dp_enable_channel(struct ipu_dp *dp)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+
+	mutex_lock(&priv->mutex);
+
+	if (!priv->use_count)
+		ipu_module_enable(priv->ipu, IPU_CONF_DP_EN);
+
+	priv->use_count++;
+
+	if (dp->foreground) {
+		u32 reg;
+
+		reg = readl(flow->base + DP_COM_CONF);
+		reg |= DP_COM_CONF_FG_EN;
+		writel(reg, flow->base + DP_COM_CONF);
+
+		ipu_srm_dp_sync_update(priv->ipu);
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_enable_channel);
+
+void ipu_dp_disable_channel(struct ipu_dp *dp)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+
+	mutex_lock(&priv->mutex);
+
+	priv->use_count--;
+
+	if (dp->foreground) {
+		u32 reg, csc;
+
+		reg = readl(flow->base + DP_COM_CONF);
+		csc = reg & DP_COM_CONF_CSC_DEF_MASK;
+		if (csc == DP_COM_CONF_CSC_DEF_FG)
+			reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+
+		reg &= ~DP_COM_CONF_FG_EN;
+		writel(reg, flow->base + DP_COM_CONF);
+
+		writel(0, flow->base + DP_FG_POS);
+		ipu_srm_dp_sync_update(priv->ipu);
+	}
+
+	if (!priv->use_count)
+		ipu_module_disable(priv->ipu, IPU_CONF_DP_EN);
+
+	if (priv->use_count < 0)
+		priv->use_count = 0;
+
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dp_disable_channel);
+
+struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow)
+{
+	struct ipu_dp_priv *priv = ipu->dp_priv;
+	struct ipu_dp *dp;
+
+	if (flow > 5)
+		return ERR_PTR(-EINVAL);
+
+	if (flow & 1)
+		dp = &priv->flow[flow >> 1].foreground;
+	else
+		dp = &priv->flow[flow >> 1].background;
+
+	if (dp->in_use)
+		return ERR_PTR(-EBUSY);
+
+	dp->in_use = true;
+
+	return dp;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_get);
+
+void ipu_dp_put(struct ipu_dp *dp)
+{
+	dp->in_use = false;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_put);
+
+int ipu_dp_init(struct ipu_soc *ipu, struct device *dev, unsigned long base)
+{
+	struct ipu_dp_priv *priv;
+	int i;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	priv->dev = dev;
+	priv->ipu = ipu;
+
+	ipu->dp_priv = priv;
+
+	priv->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!priv->base) {
+		kfree(priv);
+		return -ENOMEM;
+	}
+
+	mutex_init(&priv->mutex);
+
+	for (i = 0; i < 3; i++) {
+		priv->flow[i].foreground.foreground = 1;
+		priv->flow[i].base = priv->base + ipu_dp_flow_base[i];
+		priv->flow[i].priv = priv;
+	}
+
+	return 0;
+}
+
+void ipu_dp_exit(struct ipu_soc *ipu)
+{
+}
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-prv.h b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
new file mode 100644
index 000000000000..551802863fd5
--- /dev/null
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
@@ -0,0 +1,206 @@
+/*
+ * Copyright (c) 2010 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#ifndef __IPU_PRV_H__
+#define __IPU_PRV_H__
+
+struct ipu_soc;
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#include "imx-ipu-v3.h"
+
+#define IPUV3_CHANNEL_CSI0			 0
+#define IPUV3_CHANNEL_CSI1			 1
+#define IPUV3_CHANNEL_CSI2			 2
+#define IPUV3_CHANNEL_CSI3			 3
+#define IPUV3_CHANNEL_MEM_BG_SYNC		23
+#define IPUV3_CHANNEL_MEM_FG_SYNC		27
+#define IPUV3_CHANNEL_MEM_DC_SYNC		28
+#define IPUV3_CHANNEL_MEM_FG_SYNC_ALPHA		31
+#define IPUV3_CHANNEL_MEM_DC_ASYNC		41
+#define IPUV3_CHANNEL_ROT_ENC_MEM		45
+#define IPUV3_CHANNEL_ROT_VF_MEM		46
+#define IPUV3_CHANNEL_ROT_PP_MEM		47
+#define IPUV3_CHANNEL_ROT_ENC_MEM_OUT		48
+#define IPUV3_CHANNEL_ROT_VF_MEM_OUT		49
+#define IPUV3_CHANNEL_ROT_PP_MEM_OUT		50
+#define IPUV3_CHANNEL_MEM_BG_SYNC_ALPHA		51
+
+#define IPU_MCU_T_DEFAULT	8
+#define IPU_CM_IDMAC_REG_OFS	0x00008000
+#define IPU_CM_IC_REG_OFS	0x00020000
+#define IPU_CM_IRT_REG_OFS	0x00028000
+#define IPU_CM_CSI0_REG_OFS	0x00030000
+#define IPU_CM_CSI1_REG_OFS	0x00038000
+#define IPU_CM_SMFC_REG_OFS	0x00050000
+#define IPU_CM_DC_REG_OFS	0x00058000
+#define IPU_CM_DMFC_REG_OFS	0x00060000
+
+/* Register addresses */
+/* IPU Common registers */
+#define IPU_CM_REG(offset)	(offset)
+
+#define IPU_CONF			IPU_CM_REG(0)
+
+#define IPU_SRM_PRI1			IPU_CM_REG(0x00a0)
+#define IPU_SRM_PRI2			IPU_CM_REG(0x00a4)
+#define IPU_FS_PROC_FLOW1		IPU_CM_REG(0x00a8)
+#define IPU_FS_PROC_FLOW2		IPU_CM_REG(0x00ac)
+#define IPU_FS_PROC_FLOW3		IPU_CM_REG(0x00b0)
+#define IPU_FS_DISP_FLOW1		IPU_CM_REG(0x00b4)
+#define IPU_FS_DISP_FLOW2		IPU_CM_REG(0x00b8)
+#define IPU_SKIP			IPU_CM_REG(0x00bc)
+#define IPU_DISP_ALT_CONF		IPU_CM_REG(0x00c0)
+#define IPU_DISP_GEN			IPU_CM_REG(0x00c4)
+#define IPU_DISP_ALT1			IPU_CM_REG(0x00c8)
+#define IPU_DISP_ALT2			IPU_CM_REG(0x00cc)
+#define IPU_DISP_ALT3			IPU_CM_REG(0x00d0)
+#define IPU_DISP_ALT4			IPU_CM_REG(0x00d4)
+#define IPU_SNOOP			IPU_CM_REG(0x00d8)
+#define IPU_MEM_RST			IPU_CM_REG(0x00dc)
+#define IPU_PM				IPU_CM_REG(0x00e0)
+#define IPU_GPR				IPU_CM_REG(0x00e4)
+#define IPU_CHA_DB_MODE_SEL(ch)		IPU_CM_REG(0x0150 + 4 * ((ch) / 32))
+#define IPU_ALT_CHA_DB_MODE_SEL(ch)	IPU_CM_REG(0x0168 + 4 * ((ch) / 32))
+#define IPU_CHA_CUR_BUF(ch)		IPU_CM_REG(0x023C + 4 * ((ch) / 32))
+#define IPU_ALT_CUR_BUF0		IPU_CM_REG(0x0244)
+#define IPU_ALT_CUR_BUF1		IPU_CM_REG(0x0248)
+#define IPU_SRM_STAT			IPU_CM_REG(0x024C)
+#define IPU_PROC_TASK_STAT		IPU_CM_REG(0x0250)
+#define IPU_DISP_TASK_STAT		IPU_CM_REG(0x0254)
+#define IPU_CHA_BUF0_RDY(ch)		IPU_CM_REG(0x0268 + 4 * ((ch) / 32))
+#define IPU_CHA_BUF1_RDY(ch)		IPU_CM_REG(0x0270 + 4 * ((ch) / 32))
+#define IPU_ALT_CHA_BUF0_RDY(ch)	IPU_CM_REG(0x0278 + 4 * ((ch) / 32))
+#define IPU_ALT_CHA_BUF1_RDY(ch)	IPU_CM_REG(0x0280 + 4 * ((ch) / 32))
+
+#define IPU_INT_CTRL(n)		IPU_CM_REG(0x003C + 4 * (n))
+#define IPU_INT_STAT(n)		IPU_CM_REG(0x0200 + 4 * (n))
+
+#define IPU_DI0_COUNTER_RELEASE			(1 << 24)
+#define IPU_DI1_COUNTER_RELEASE			(1 << 25)
+
+#define IPU_IDMAC_REG(offset)	(offset)
+
+#define IDMAC_CONF			IPU_IDMAC_REG(0x0000)
+#define IDMAC_CHA_EN(ch)		IPU_IDMAC_REG(0x0004 + 4 * ((ch) / 32))
+#define IDMAC_SEP_ALPHA			IPU_IDMAC_REG(0x000c)
+#define IDMAC_ALT_SEP_ALPHA		IPU_IDMAC_REG(0x0010)
+#define IDMAC_CHA_PRI(ch)		IPU_IDMAC_REG(0x0014 + 4 * ((ch) / 32))
+#define IDMAC_WM_EN(ch)			IPU_IDMAC_REG(0x001c + 4 * ((ch) / 32))
+#define IDMAC_CH_LOCK_EN_1		IPU_IDMAC_REG(0x0024)
+#define IDMAC_CH_LOCK_EN_2		IPU_IDMAC_REG(0x0028)
+#define IDMAC_SUB_ADDR_0		IPU_IDMAC_REG(0x002c)
+#define IDMAC_SUB_ADDR_1		IPU_IDMAC_REG(0x0030)
+#define IDMAC_SUB_ADDR_2		IPU_IDMAC_REG(0x0034)
+#define IDMAC_BAND_EN(ch)		IPU_IDMAC_REG(0x0040 + 4 * ((ch) / 32))
+#define IDMAC_CHA_BUSY(ch)		IPU_IDMAC_REG(0x0100 + 4 * ((ch) / 32))
+
+#define IPU_NUM_IRQS	(32 * 5)
+
+enum ipu_modules {
+	IPU_CONF_CSI0_EN		= (1 << 0),
+	IPU_CONF_CSI1_EN		= (1 << 1),
+	IPU_CONF_IC_EN			= (1 << 2),
+	IPU_CONF_ROT_EN			= (1 << 3),
+	IPU_CONF_ISP_EN			= (1 << 4),
+	IPU_CONF_DP_EN			= (1 << 5),
+	IPU_CONF_DI0_EN			= (1 << 6),
+	IPU_CONF_DI1_EN			= (1 << 7),
+	IPU_CONF_SMFC_EN		= (1 << 8),
+	IPU_CONF_DC_EN			= (1 << 9),
+	IPU_CONF_DMFC_EN		= (1 << 10),
+
+	IPU_CONF_VDI_EN			= (1 << 12),
+
+	IPU_CONF_IDMAC_DIS		= (1 << 22),
+
+	IPU_CONF_IC_DMFC_SEL		= (1 << 25),
+	IPU_CONF_IC_DMFC_SYNC		= (1 << 26),
+	IPU_CONF_VDI_DMFC_SYNC		= (1 << 27),
+
+	IPU_CONF_CSI0_DATA_SOURCE	= (1 << 28),
+	IPU_CONF_CSI1_DATA_SOURCE	= (1 << 29),
+	IPU_CONF_IC_INPUT		= (1 << 30),
+	IPU_CONF_CSI_SEL		= (1 << 31),
+};
+
+struct ipuv3_channel {
+	unsigned int num;
+
+	bool enabled;
+	bool busy;
+
+	struct ipu_soc *ipu;
+};
+
+struct ipu_dc_priv;
+struct ipu_dmfc_priv;
+struct ipu_di;
+struct ipu_devtype;
+
+struct ipu_soc {
+	struct device		*dev;
+	const struct ipu_devtype	*devtype;
+	enum ipuv3_type		ipu_type;
+	spinlock_t		lock;
+	struct mutex		channel_lock;
+
+	void __iomem		*cm_reg;
+	void __iomem		*idmac_reg;
+	struct ipu_ch_param __iomem	*cpmem_base;
+
+	int			usecount;
+
+	struct clk		*clk;
+
+	struct ipuv3_channel	channel[64];
+
+	int			irq_start;
+	int			irq_sync;
+	int			irq_err;
+
+	struct ipu_dc_priv	*dc_priv;
+	struct ipu_dp_priv	*dp_priv;
+	struct ipu_dmfc_priv	*dmfc_priv;
+	struct ipu_di		*di_priv[2];
+};
+
+void ipu_srm_dp_sync_update(struct ipu_soc *ipu);
+
+int ipu_module_enable(struct ipu_soc *ipu, u32 mask);
+int ipu_module_disable(struct ipu_soc *ipu, u32 mask);
+
+int ipu_di_init(struct ipu_soc *ipu, struct device *dev, int id,
+		unsigned long base, u32 module, struct clk *ipu_clk);
+void ipu_di_exit(struct ipu_soc *ipu, int id);
+
+int ipu_dmfc_init(struct ipu_soc *ipu, struct device *dev, unsigned long base,
+		struct clk *ipu_clk);
+void ipu_dmfc_exit(struct ipu_soc *ipu);
+
+int ipu_dp_init(struct ipu_soc *ipu, struct device *dev, unsigned long base);
+void ipu_dp_exit(struct ipu_soc *ipu);
+
+int ipu_dc_init(struct ipu_soc *ipu, struct device *dev, unsigned long base,
+		unsigned long template_base);
+void ipu_dc_exit(struct ipu_soc *ipu);
+
+int ipu_cpmem_init(struct ipu_soc *ipu, struct device *dev, unsigned long base);
+void ipu_cpmem_exit(struct ipu_soc *ipu);
+
+#endif				/* __IPU_PRV_H__ */
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
new file mode 100644
index 000000000000..78d3edac75c1
--- /dev/null
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -0,0 +1,579 @@
+/*
+ * i.MX IPUv3 Graphics driver
+ *
+ * Copyright (C) 2011 Sascha Hauer, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+#include <linux/module.h>
+#include <linux/export.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <drm/drm_gem_cma_helper.h>
+#include <drm/drm_fb_cma_helper.h>
+
+#include "ipu-v3/imx-ipu-v3.h"
+#include "imx-drm.h"
+
+#define DRIVER_DESC		"i.MX IPUv3 Graphics"
+
+struct ipu_framebuffer {
+	struct drm_framebuffer	base;
+	void			*virt;
+	dma_addr_t		phys;
+	size_t			len;
+};
+
+struct ipu_crtc {
+	struct drm_fb_helper	fb_helper;
+	struct ipu_framebuffer	ifb;
+	int			num_crtcs;
+	struct device		*dev;
+	struct drm_crtc		base;
+	struct imx_drm_crtc	*imx_crtc;
+	struct ipuv3_channel	*ipu_ch;
+	struct ipu_dc		*dc;
+	struct ipu_dp		*dp;
+	struct dmfc_channel	*dmfc;
+	struct ipu_di		*di;
+	int			enabled;
+	struct ipu_priv		*ipu_priv;
+	struct drm_pending_vblank_event *page_flip_event;
+	struct drm_framebuffer	*newfb;
+	int			irq;
+	u32			interface_pix_fmt;
+	unsigned long		di_clkflags;
+};
+
+#define to_ipu_crtc(x) container_of(x, struct ipu_crtc, base)
+
+static int calc_vref(struct drm_display_mode *mode)
+{
+	unsigned long htotal, vtotal;
+
+	htotal = mode->htotal;
+	vtotal = mode->vtotal;
+
+	if (!htotal || !vtotal)
+		return 60;
+
+	return mode->clock * 1000 / vtotal / htotal;
+}
+
+static int calc_bandwidth(struct drm_display_mode *mode, unsigned int vref)
+{
+	return mode->hdisplay * mode->vdisplay * vref;
+}
+
+static void ipu_fb_enable(struct ipu_crtc *ipu_crtc)
+{
+	if (ipu_crtc->enabled)
+		return;
+
+	ipu_di_enable(ipu_crtc->di);
+	ipu_dmfc_enable_channel(ipu_crtc->dmfc);
+	ipu_idmac_enable_channel(ipu_crtc->ipu_ch);
+	ipu_dc_enable_channel(ipu_crtc->dc);
+	if (ipu_crtc->dp)
+		ipu_dp_enable_channel(ipu_crtc->dp);
+
+	ipu_crtc->enabled = 1;
+}
+
+static void ipu_fb_disable(struct ipu_crtc *ipu_crtc)
+{
+	if (!ipu_crtc->enabled)
+		return;
+
+	if (ipu_crtc->dp)
+		ipu_dp_disable_channel(ipu_crtc->dp);
+	ipu_dc_disable_channel(ipu_crtc->dc);
+	ipu_idmac_disable_channel(ipu_crtc->ipu_ch);
+	ipu_dmfc_disable_channel(ipu_crtc->dmfc);
+	ipu_di_disable(ipu_crtc->di);
+
+	ipu_crtc->enabled = 0;
+}
+
+static void ipu_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	dev_info(ipu_crtc->dev, "%s mode: %d\n", __func__, mode);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		ipu_fb_enable(ipu_crtc);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		ipu_fb_disable(ipu_crtc);
+		break;
+	}
+}
+
+static int ipu_page_flip(struct drm_crtc *crtc,
+		struct drm_framebuffer *fb,
+		struct drm_pending_vblank_event *event)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+	int ret;
+
+	if (ipu_crtc->newfb)
+		return -EBUSY;
+
+	ret = imx_drm_crtc_vblank_get(ipu_crtc->imx_crtc);
+	if (ret) {
+		dev_dbg(ipu_crtc->dev, "failed to acquire vblank counter\n");
+		list_del(&event->base.link);
+
+		return ret;
+	}
+
+	ipu_crtc->newfb = fb;
+	ipu_crtc->page_flip_event = event;
+
+	return 0;
+}
+
+static const struct drm_crtc_funcs ipu_crtc_funcs = {
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = drm_crtc_cleanup,
+	.page_flip = ipu_page_flip,
+};
+
+static int ipu_drm_set_base(struct drm_crtc *crtc, int x, int y)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+	struct drm_gem_cma_object *cma_obj;
+	struct drm_framebuffer *fb = crtc->fb;
+	unsigned long phys;
+
+	cma_obj = drm_fb_cma_get_gem_obj(fb, 0);
+	if (!cma_obj) {
+		DRM_LOG_KMS("entry is null.\n");
+		return -EFAULT;
+	}
+
+	phys = cma_obj->paddr;
+	phys += x * (fb->bits_per_pixel >> 3);
+	phys += y * fb->pitches[0];
+
+	dev_dbg(ipu_crtc->dev, "%s: phys: 0x%lx\n", __func__, phys);
+	dev_dbg(ipu_crtc->dev, "%s: xy: %dx%d\n", __func__, x, y);
+
+	ipu_cpmem_set_stride(ipu_get_cpmem(ipu_crtc->ipu_ch), fb->pitches[0]);
+	ipu_cpmem_set_buffer(ipu_get_cpmem(ipu_crtc->ipu_ch),
+			  0, phys);
+
+	return 0;
+}
+
+static int ipu_crtc_mode_set(struct drm_crtc *crtc,
+			       struct drm_display_mode *orig_mode,
+			       struct drm_display_mode *mode,
+			       int x, int y,
+			       struct drm_framebuffer *old_fb)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+	struct drm_framebuffer *fb = ipu_crtc->base.fb;
+	int ret;
+	struct ipu_di_signal_cfg sig_cfg = {};
+	u32 out_pixel_fmt;
+	struct ipu_ch_param __iomem *cpmem = ipu_get_cpmem(ipu_crtc->ipu_ch);
+	int bpp;
+	u32 v4l2_fmt;
+
+	dev_dbg(ipu_crtc->dev, "%s: mode->hdisplay: %d\n", __func__,
+			mode->hdisplay);
+	dev_dbg(ipu_crtc->dev, "%s: mode->vdisplay: %d\n", __func__,
+			mode->vdisplay);
+
+	ipu_ch_param_zero(cpmem);
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_ARGB8888:
+		v4l2_fmt = V4L2_PIX_FMT_RGB32;
+		bpp = 32;
+		break;
+	case DRM_FORMAT_RGB565:
+		v4l2_fmt = V4L2_PIX_FMT_RGB565;
+		bpp = 16;
+		break;
+	case DRM_FORMAT_RGB888:
+		v4l2_fmt = V4L2_PIX_FMT_RGB24;
+		bpp = 24;
+		break;
+	default:
+		dev_err(ipu_crtc->dev, "unsupported pixel format 0x%08x\n",
+				fb->pixel_format);
+		return -EINVAL;
+	}
+
+	out_pixel_fmt = ipu_crtc->interface_pix_fmt;
+
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+		sig_cfg.interlaced = 1;
+	if (mode->flags & DRM_MODE_FLAG_PHSYNC)
+		sig_cfg.Hsync_pol = 1;
+	if (mode->flags & DRM_MODE_FLAG_PVSYNC)
+		sig_cfg.Vsync_pol = 1;
+
+	sig_cfg.enable_pol = 1;
+	sig_cfg.clk_pol = 0;
+	sig_cfg.width = mode->hdisplay;
+	sig_cfg.height = mode->vdisplay;
+	sig_cfg.pixel_fmt = out_pixel_fmt;
+	sig_cfg.h_start_width = mode->htotal - mode->hsync_end;
+	sig_cfg.h_sync_width = mode->hsync_end - mode->hsync_start;
+	sig_cfg.h_end_width = mode->hsync_start - mode->hdisplay;
+
+	sig_cfg.v_start_width = mode->vtotal - mode->vsync_end;
+	sig_cfg.v_sync_width = mode->vsync_end - mode->vsync_start;
+	sig_cfg.v_end_width = mode->vsync_start - mode->vdisplay;
+	sig_cfg.pixelclock = mode->clock * 1000;
+	sig_cfg.clkflags = ipu_crtc->di_clkflags;
+
+	sig_cfg.v_to_h_sync = 0;
+
+	if (ipu_crtc->dp) {
+		ret = ipu_dp_setup_channel(ipu_crtc->dp, IPUV3_COLORSPACE_RGB,
+				IPUV3_COLORSPACE_RGB);
+		if (ret) {
+			dev_err(ipu_crtc->dev,
+				"initializing display processor failed with %d\n",
+				ret);
+			return ret;
+		}
+		ipu_dp_set_global_alpha(ipu_crtc->dp, 1, 0, 1);
+	}
+
+	ret = ipu_dc_init_sync(ipu_crtc->dc, ipu_crtc->di, sig_cfg.interlaced,
+			out_pixel_fmt, mode->hdisplay);
+	if (ret) {
+		dev_err(ipu_crtc->dev,
+				"initializing display controller failed with %d\n",
+				ret);
+		return ret;
+	}
+
+	ret = ipu_di_init_sync_panel(ipu_crtc->di, &sig_cfg);
+	if (ret) {
+		dev_err(ipu_crtc->dev,
+				"initializing panel failed with %d\n", ret);
+		return ret;
+	}
+
+	ipu_cpmem_set_resolution(cpmem, mode->hdisplay, mode->vdisplay);
+	ipu_cpmem_set_fmt(cpmem, v4l2_fmt);
+	ipu_cpmem_set_high_priority(ipu_crtc->ipu_ch);
+
+	ret = ipu_dmfc_init_channel(ipu_crtc->dmfc, mode->hdisplay);
+	if (ret) {
+		dev_err(ipu_crtc->dev,
+				"initializing dmfc channel failed with %d\n",
+				ret);
+		return ret;
+	}
+
+	ret = ipu_dmfc_alloc_bandwidth(ipu_crtc->dmfc,
+			calc_bandwidth(mode, calc_vref(mode)), 64);
+	if (ret) {
+		dev_err(ipu_crtc->dev,
+				"allocating dmfc bandwidth failed with %d\n",
+				ret);
+		return ret;
+	}
+
+	ipu_drm_set_base(crtc, x, y);
+
+	return 0;
+}
+
+static void ipu_crtc_handle_pageflip(struct ipu_crtc *ipu_crtc)
+{
+	struct drm_pending_vblank_event *e;
+	struct timeval now;
+	unsigned long flags;
+	struct drm_device *drm = ipu_crtc->base.dev;
+
+	spin_lock_irqsave(&drm->event_lock, flags);
+
+	e = ipu_crtc->page_flip_event;
+	if (!e) {
+		spin_unlock_irqrestore(&drm->event_lock, flags);
+		return;
+	}
+
+	do_gettimeofday(&now);
+	e->event.sequence = 0;
+	e->event.tv_sec = now.tv_sec;
+	e->event.tv_usec = now.tv_usec;
+	ipu_crtc->page_flip_event = NULL;
+
+	imx_drm_crtc_vblank_put(ipu_crtc->imx_crtc);
+
+	list_add_tail(&e->base.link, &e->base.file_priv->event_list);
+
+	wake_up_interruptible(&e->base.file_priv->event_wait);
+
+	spin_unlock_irqrestore(&drm->event_lock, flags);
+}
+
+static irqreturn_t ipu_irq_handler(int irq, void *dev_id)
+{
+	struct ipu_crtc *ipu_crtc = dev_id;
+
+	imx_drm_handle_vblank(ipu_crtc->imx_crtc);
+
+	if (ipu_crtc->newfb) {
+		ipu_crtc->base.fb = ipu_crtc->newfb;
+		ipu_crtc->newfb = NULL;
+		ipu_drm_set_base(&ipu_crtc->base, 0, 0);
+		ipu_crtc_handle_pageflip(ipu_crtc);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static bool ipu_crtc_mode_fixup(struct drm_crtc *crtc,
+				  const struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void ipu_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	ipu_fb_disable(ipu_crtc);
+}
+
+static void ipu_crtc_commit(struct drm_crtc *crtc)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	ipu_fb_enable(ipu_crtc);
+}
+
+static void ipu_crtc_load_lut(struct drm_crtc *crtc)
+{
+}
+
+static struct drm_crtc_helper_funcs ipu_helper_funcs = {
+	.dpms = ipu_crtc_dpms,
+	.mode_fixup = ipu_crtc_mode_fixup,
+	.mode_set = ipu_crtc_mode_set,
+	.prepare = ipu_crtc_prepare,
+	.commit = ipu_crtc_commit,
+	.load_lut = ipu_crtc_load_lut,
+};
+
+static int ipu_enable_vblank(struct drm_crtc *crtc)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	enable_irq(ipu_crtc->irq);
+
+	return 0;
+}
+
+static void ipu_disable_vblank(struct drm_crtc *crtc)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	disable_irq(ipu_crtc->irq);
+}
+
+static int ipu_set_interface_pix_fmt(struct drm_crtc *crtc, u32 encoder_type,
+		u32 pixfmt)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	ipu_crtc->interface_pix_fmt = pixfmt;
+
+	switch (encoder_type) {
+	case DRM_MODE_ENCODER_LVDS:
+		ipu_crtc->di_clkflags = IPU_DI_CLKMODE_SYNC |
+			IPU_DI_CLKMODE_EXT;
+		break;
+	case DRM_MODE_ENCODER_NONE:
+		ipu_crtc->di_clkflags = 0;
+		break;
+	}
+
+	return 0;
+}
+
+static const struct imx_drm_crtc_helper_funcs ipu_crtc_helper_funcs = {
+	.enable_vblank = ipu_enable_vblank,
+	.disable_vblank = ipu_disable_vblank,
+	.set_interface_pix_fmt = ipu_set_interface_pix_fmt,
+	.crtc_funcs = &ipu_crtc_funcs,
+	.crtc_helper_funcs = &ipu_helper_funcs,
+};
+
+static void ipu_put_resources(struct ipu_crtc *ipu_crtc)
+{
+	if (!IS_ERR_OR_NULL(ipu_crtc->ipu_ch))
+		ipu_idmac_put(ipu_crtc->ipu_ch);
+	if (!IS_ERR_OR_NULL(ipu_crtc->dmfc))
+		ipu_dmfc_put(ipu_crtc->dmfc);
+	if (!IS_ERR_OR_NULL(ipu_crtc->dp))
+		ipu_dp_put(ipu_crtc->dp);
+	if (!IS_ERR_OR_NULL(ipu_crtc->di))
+		ipu_di_put(ipu_crtc->di);
+}
+
+static int ipu_get_resources(struct ipu_crtc *ipu_crtc,
+		struct ipu_client_platformdata *pdata)
+{
+	struct ipu_soc *ipu = dev_get_drvdata(ipu_crtc->dev->parent);
+	int ret;
+
+	ipu_crtc->ipu_ch = ipu_idmac_get(ipu, pdata->dma[0]);
+	if (IS_ERR_OR_NULL(ipu_crtc->ipu_ch)) {
+		ret = PTR_ERR(ipu_crtc->ipu_ch);
+		goto err_out;
+	}
+
+	ipu_crtc->dc = ipu_dc_get(ipu, pdata->dc);
+	if (IS_ERR(ipu_crtc->dc)) {
+		ret = PTR_ERR(ipu_crtc->dc);
+		goto err_out;
+	}
+
+	ipu_crtc->dmfc = ipu_dmfc_get(ipu, pdata->dma[0]);
+	if (IS_ERR(ipu_crtc->dmfc)) {
+		ret = PTR_ERR(ipu_crtc->dmfc);
+		goto err_out;
+	}
+
+	if (pdata->dp >= 0) {
+		ipu_crtc->dp = ipu_dp_get(ipu, pdata->dp);
+		if (IS_ERR(ipu_crtc->dp)) {
+			ret = PTR_ERR(ipu_crtc->ipu_ch);
+			goto err_out;
+		}
+	}
+
+	ipu_crtc->di = ipu_di_get(ipu, pdata->di);
+	if (IS_ERR(ipu_crtc->di)) {
+		ret = PTR_ERR(ipu_crtc->di);
+		goto err_out;
+	}
+
+	ipu_crtc->irq = ipu_idmac_channel_irq(ipu, ipu_crtc->ipu_ch,
+			IPU_IRQ_EOF);
+	ret = devm_request_irq(ipu_crtc->dev, ipu_crtc->irq, ipu_irq_handler, 0,
+			"imx_drm", ipu_crtc);
+	if (ret < 0) {
+		dev_err(ipu_crtc->dev, "irq request failed with %d.\n", ret);
+		goto err_out;
+	}
+
+	disable_irq(ipu_crtc->irq);
+
+	return 0;
+err_out:
+	ipu_put_resources(ipu_crtc);
+
+	return ret;
+}
+
+static int ipu_crtc_init(struct ipu_crtc *ipu_crtc,
+		struct ipu_client_platformdata *pdata)
+{
+	int ret;
+
+	ret = ipu_get_resources(ipu_crtc, pdata);
+	if (ret) {
+		dev_err(ipu_crtc->dev, "getting resources failed with %d.\n",
+				ret);
+		return ret;
+	}
+
+	ret = imx_drm_add_crtc(&ipu_crtc->base,
+			&ipu_crtc->imx_crtc,
+			&ipu_crtc_helper_funcs, THIS_MODULE,
+			ipu_crtc->dev->parent->of_node, pdata->di);
+	if (ret) {
+		dev_err(ipu_crtc->dev, "adding crtc failed with %d.\n", ret);
+		goto err_put_resources;
+	}
+
+	return 0;
+
+err_put_resources:
+	ipu_put_resources(ipu_crtc);
+
+	return ret;
+}
+
+static int __devinit ipu_drm_probe(struct platform_device *pdev)
+{
+	struct ipu_client_platformdata *pdata = pdev->dev.platform_data;
+	struct ipu_crtc *ipu_crtc;
+	int ret;
+
+	if (!pdata)
+		return -EINVAL;
+
+	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	ipu_crtc = devm_kzalloc(&pdev->dev, sizeof(*ipu_crtc), GFP_KERNEL);
+	if (!ipu_crtc)
+		return -ENOMEM;
+
+	ipu_crtc->dev = &pdev->dev;
+
+	ret = ipu_crtc_init(ipu_crtc, pdata);
+
+	platform_set_drvdata(pdev, ipu_crtc);
+
+	return 0;
+}
+
+static int __devexit ipu_drm_remove(struct platform_device *pdev)
+{
+	struct ipu_crtc *ipu_crtc = platform_get_drvdata(pdev);
+
+	imx_drm_remove_crtc(ipu_crtc->imx_crtc);
+
+	ipu_put_resources(ipu_crtc);
+
+	return 0;
+}
+
+static struct platform_driver ipu_drm_driver = {
+	.driver = {
+		.name = "imx-ipuv3-crtc",
+	},
+	.probe = ipu_drm_probe,
+	.remove = __devexit_p(ipu_drm_remove),
+};
+module_platform_driver(ipu_drm_driver);
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/staging/imx-drm/parallel-display.c
new file mode 100644
index 000000000000..9b51d732eefa
--- /dev/null
+++ b/drivers/staging/imx-drm/parallel-display.c
@@ -0,0 +1,261 @@
+/*
+ * i.MX drm driver - parallel display implementation
+ *
+ * Copyright (C) 2012 Sascha Hauer, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+#include <linux/videodev2.h>
+
+#include "imx-drm.h"
+
+#define con_to_imxpd(x) container_of(x, struct imx_parallel_display, connector)
+#define enc_to_imxpd(x) container_of(x, struct imx_parallel_display, encoder)
+
+struct imx_parallel_display {
+	struct drm_connector connector;
+	struct imx_drm_connector *imx_drm_connector;
+	struct drm_encoder encoder;
+	struct imx_drm_encoder *imx_drm_encoder;
+	struct device *dev;
+	void *edid;
+	int edid_len;
+	u32 interface_pix_fmt;
+	int mode_valid;
+	struct drm_display_mode mode;
+};
+
+static enum drm_connector_status imx_pd_connector_detect(
+		struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void imx_pd_connector_destroy(struct drm_connector *connector)
+{
+	/* do not free here */
+}
+
+static int imx_pd_connector_get_modes(struct drm_connector *connector)
+{
+	struct imx_parallel_display *imxpd = con_to_imxpd(connector);
+	int num_modes = 0;
+
+	if (imxpd->edid) {
+		drm_mode_connector_update_edid_property(connector, imxpd->edid);
+		num_modes = drm_add_edid_modes(connector, imxpd->edid);
+	}
+
+	if (imxpd->mode_valid) {
+		struct drm_display_mode *mode = drm_mode_create(connector->dev);
+		drm_mode_copy(mode, &imxpd->mode);
+		mode->type |= DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED,
+		drm_mode_probed_add(connector, mode);
+		num_modes++;
+	}
+
+	return num_modes;
+}
+
+static int imx_pd_connector_mode_valid(struct drm_connector *connector,
+			  struct drm_display_mode *mode)
+{
+	return 0;
+}
+
+static struct drm_encoder *imx_pd_connector_best_encoder(
+		struct drm_connector *connector)
+{
+	struct imx_parallel_display *imxpd = con_to_imxpd(connector);
+
+	return &imxpd->encoder;
+}
+
+static void imx_pd_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+}
+
+static bool imx_pd_encoder_mode_fixup(struct drm_encoder *encoder,
+			   const struct drm_display_mode *mode,
+			   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void imx_pd_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct imx_parallel_display *imxpd = enc_to_imxpd(encoder);
+
+	imx_drm_crtc_panel_format(encoder->crtc, DRM_MODE_ENCODER_NONE,
+			imxpd->interface_pix_fmt);
+}
+
+static void imx_pd_encoder_commit(struct drm_encoder *encoder)
+{
+}
+
+static void imx_pd_encoder_mode_set(struct drm_encoder *encoder,
+			 struct drm_display_mode *mode,
+			 struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void imx_pd_encoder_disable(struct drm_encoder *encoder)
+{
+}
+
+static void imx_pd_encoder_destroy(struct drm_encoder *encoder)
+{
+	/* do not free here */
+}
+
+static struct drm_connector_funcs imx_pd_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.detect = imx_pd_connector_detect,
+	.destroy = imx_pd_connector_destroy,
+};
+
+static struct drm_connector_helper_funcs imx_pd_connector_helper_funcs = {
+	.get_modes = imx_pd_connector_get_modes,
+	.best_encoder = imx_pd_connector_best_encoder,
+	.mode_valid = imx_pd_connector_mode_valid,
+};
+
+static struct drm_encoder_funcs imx_pd_encoder_funcs = {
+	.destroy = imx_pd_encoder_destroy,
+};
+
+static struct drm_encoder_helper_funcs imx_pd_encoder_helper_funcs = {
+	.dpms = imx_pd_encoder_dpms,
+	.mode_fixup = imx_pd_encoder_mode_fixup,
+	.prepare = imx_pd_encoder_prepare,
+	.commit = imx_pd_encoder_commit,
+	.mode_set = imx_pd_encoder_mode_set,
+	.disable = imx_pd_encoder_disable,
+};
+
+static int imx_pd_register(struct imx_parallel_display *imxpd)
+{
+	int ret;
+
+	drm_mode_connector_attach_encoder(&imxpd->connector, &imxpd->encoder);
+
+	imxpd->connector.funcs = &imx_pd_connector_funcs;
+	imxpd->encoder.funcs = &imx_pd_encoder_funcs;
+
+	imxpd->encoder.encoder_type = DRM_MODE_ENCODER_NONE;
+	imxpd->connector.connector_type = DRM_MODE_CONNECTOR_VGA;
+
+	drm_encoder_helper_add(&imxpd->encoder, &imx_pd_encoder_helper_funcs);
+	ret = imx_drm_add_encoder(&imxpd->encoder, &imxpd->imx_drm_encoder,
+			THIS_MODULE);
+	if (ret) {
+		dev_err(imxpd->dev, "adding encoder failed with %d\n", ret);
+		return ret;
+	}
+
+	drm_connector_helper_add(&imxpd->connector,
+			&imx_pd_connector_helper_funcs);
+
+	ret = imx_drm_add_connector(&imxpd->connector,
+			&imxpd->imx_drm_connector, THIS_MODULE);
+	if (ret) {
+		imx_drm_remove_encoder(imxpd->imx_drm_encoder);
+		dev_err(imxpd->dev, "adding connector failed with %d\n", ret);
+		return ret;
+	}
+
+	imxpd->connector.encoder = &imxpd->encoder;
+
+	return 0;
+}
+
+static int __devinit imx_pd_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const u8 *edidp;
+	struct imx_parallel_display *imxpd;
+	int ret;
+	const char *fmt;
+
+	imxpd = devm_kzalloc(&pdev->dev, sizeof(*imxpd), GFP_KERNEL);
+	if (!imxpd)
+		return -ENOMEM;
+
+	edidp = of_get_property(np, "edid", &imxpd->edid_len);
+	if (edidp)
+		imxpd->edid = kmemdup(edidp, imxpd->edid_len, GFP_KERNEL);
+
+	ret = of_property_read_string(np, "interface-pix-fmt", &fmt);
+	if (!ret) {
+		if (!strcmp(fmt, "rgb24"))
+			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB24;
+		else if (!strcmp(fmt, "rgb565"))
+			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB565;
+	}
+
+	imxpd->dev = &pdev->dev;
+
+	ret = imx_pd_register(imxpd);
+	if (ret)
+		return ret;
+
+	ret = imx_drm_encoder_add_possible_crtcs(imxpd->imx_drm_encoder, np);
+
+	platform_set_drvdata(pdev, imxpd);
+
+	return 0;
+}
+
+static int __devexit imx_pd_remove(struct platform_device *pdev)
+{
+	struct imx_parallel_display *imxpd = platform_get_drvdata(pdev);
+	struct drm_connector *connector = &imxpd->connector;
+	struct drm_encoder *encoder = &imxpd->encoder;
+
+	drm_mode_connector_detach_encoder(connector, encoder);
+
+	imx_drm_remove_connector(imxpd->imx_drm_connector);
+	imx_drm_remove_encoder(imxpd->imx_drm_encoder);
+
+	return 0;
+}
+
+static const struct of_device_id imx_pd_dt_ids[] = {
+	{ .compatible = "fsl,imx-parallel-display", },
+	{ /* sentinel */ }
+};
+
+static struct platform_driver imx_pd_driver = {
+	.probe		= imx_pd_probe,
+	.remove		= __devexit_p(imx_pd_remove),
+	.driver		= {
+		.of_match_table = imx_pd_dt_ids,
+		.name	= "imx-parallel-display",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(imx_pd_driver);
+
+MODULE_DESCRIPTION("i.MX parallel display driver");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/ipack/Kconfig b/drivers/staging/ipack/Kconfig
index af321789dddb..4cf47066140c 100644
--- a/drivers/staging/ipack/Kconfig
+++ b/drivers/staging/ipack/Kconfig
@@ -4,6 +4,7 @@
 
 menuconfig IPACK_BUS
 	tristate "IndustryPack bus support"
+	depends on HAS_IOMEM
 	---help---
 	  If you say Y here you get support for the IndustryPack Framework
 	  for drivers for many types of boards that support this industrial
diff --git a/drivers/staging/ipack/TODO b/drivers/staging/ipack/TODO
index b21d33ab144a..ffafe6911a77 100644
--- a/drivers/staging/ipack/TODO
+++ b/drivers/staging/ipack/TODO
@@ -12,29 +12,8 @@ operations between the two kind of boards.
 TODO
 ====
 
-TPCI-200
---------
-
-* It has a linked list with the tpci200 devices it is managing. Get rid of it
-  and use driver_for_each_device() instead.
-
-IP-OCTAL
---------
-
-* It has a linked list which saves the devices it is currently
-  managing. It should use the driver_for_each_device() function. It is not there
-  due to the impossibility of using container_of macro to recover the
-  corresponding "struct ipoctal" because the attribute "struct ipack_device" is
-  a pointer. This code should be refactored.
-
-Ipack
------
-
-* The structures and API exported can be improved a lot. For example, the
-  way to unregistering IP module devices, doing the IP module driver a call to
-  remove_device() to notify the carrier driver, or the opposite with the call to
-  the ipack_driver_ops' remove() function could be improved.
-
+checkpatch.pl warnings
+cleanup
 
 Contact
 =======
diff --git a/drivers/staging/ipack/bridges/tpci200.c b/drivers/staging/ipack/bridges/tpci200.c
index 2b83fa8e550a..bb8aa70281cd 100644
--- a/drivers/staging/ipack/bridges/tpci200.c
+++ b/drivers/staging/ipack/bridges/tpci200.c
@@ -14,38 +14,32 @@
 #include <linux/module.h>
 #include "tpci200.h"
 
-static struct ipack_bus_ops tpci200_bus_ops;
-
-/* TPCI200 controls registers */
-static int control_reg[] = {
-	TPCI200_CONTROL_A_REG,
-	TPCI200_CONTROL_B_REG,
-	TPCI200_CONTROL_C_REG,
-	TPCI200_CONTROL_D_REG
+static u16 tpci200_status_timeout[] = {
+	TPCI200_A_TIMEOUT,
+	TPCI200_B_TIMEOUT,
+	TPCI200_C_TIMEOUT,
+	TPCI200_D_TIMEOUT,
 };
 
-/* Linked list to save the registered devices */
-static LIST_HEAD(tpci200_list);
-
-static int tpci200_slot_unregister(struct ipack_device *dev);
+static u16 tpci200_status_error[] = {
+	TPCI200_A_ERROR,
+	TPCI200_B_ERROR,
+	TPCI200_C_ERROR,
+	TPCI200_D_ERROR,
+};
 
 static struct tpci200_board *check_slot(struct ipack_device *dev)
 {
 	struct tpci200_board *tpci200;
-	int found = 0;
 
 	if (dev == NULL)
 		return NULL;
 
-	list_for_each_entry(tpci200, &tpci200_list, list) {
-		if (tpci200->number == dev->bus_nr) {
-			found = 1;
-			break;
-		}
-	}
 
-	if (!found) {
-		dev_err(&dev->dev, "Carrier not found\n");
+	tpci200 = dev_get_drvdata(dev->bus->parent);
+
+	if (tpci200 == NULL) {
+		dev_info(&dev->dev, "carrier board not found\n");
 		return NULL;
 	}
 
@@ -59,296 +53,190 @@ static struct tpci200_board *check_slot(struct ipack_device *dev)
 	return tpci200;
 }
 
-static inline unsigned char __tpci200_read8(void __iomem *address,
-					    unsigned long offset)
-{
-	return ioread8(address + (offset^1));
-}
-
-static inline unsigned short __tpci200_read16(void __iomem *address,
-					      unsigned long offset)
+static void tpci200_clear_mask(struct tpci200_board *tpci200,
+			       __le16 __iomem *addr, u16 mask)
 {
-	return ioread16(address + offset);
+	unsigned long flags;
+	spin_lock_irqsave(&tpci200->regs_lock, flags);
+	iowrite16(ioread16(addr) & (~mask), addr);
+	spin_unlock_irqrestore(&tpci200->regs_lock, flags);
 }
 
-static inline unsigned int __tpci200_read32(void __iomem *address,
-					    unsigned long offset)
+static void tpci200_set_mask(struct tpci200_board *tpci200,
+			     __le16 __iomem *addr, u16 mask)
 {
-	return swahw32(ioread32(address + offset));
+	unsigned long flags;
+	spin_lock_irqsave(&tpci200->regs_lock, flags);
+	iowrite16(ioread16(addr) | mask, addr);
+	spin_unlock_irqrestore(&tpci200->regs_lock, flags);
 }
 
-static inline void __tpci200_write8(unsigned char value,
-				    void __iomem *address, unsigned long offset)
+static void tpci200_unregister(struct tpci200_board *tpci200)
 {
-	iowrite8(value, address+(offset^1));
-}
+	int i;
 
-static inline void __tpci200_write16(unsigned short value,
-				     void __iomem *address,
-				     unsigned long offset)
-{
-	iowrite16(value, address+offset);
-}
+	free_irq(tpci200->info->pdev->irq, (void *) tpci200);
 
-static inline void __tpci200_write32(unsigned int value,
-				     void __iomem *address,
-				     unsigned long offset)
-{
-	iowrite32(swahw32(value), address+offset);
-}
+	pci_iounmap(tpci200->info->pdev, tpci200->info->interface_regs);
+	pci_iounmap(tpci200->info->pdev, tpci200->info->ioidint_space);
+	pci_iounmap(tpci200->info->pdev, tpci200->info->mem8_space);
+	pci_iounmap(tpci200->info->pdev, tpci200->info->cfg_regs);
 
-static struct ipack_addr_space *get_slot_address_space(struct ipack_device *dev,
-						       int space)
-{
-	struct ipack_addr_space *addr;
+	pci_release_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR);
+	pci_release_region(tpci200->info->pdev, TPCI200_IO_ID_INT_SPACES_BAR);
+	pci_release_region(tpci200->info->pdev, TPCI200_MEM8_SPACE_BAR);
+	pci_release_region(tpci200->info->pdev, TPCI200_CFG_MEM_BAR);
 
-	switch (space) {
-	case IPACK_IO_SPACE:
-		addr = &dev->io_space;
-		break;
-	case IPACK_ID_SPACE:
-		addr = &dev->id_space;
-		break;
-	case IPACK_MEM_SPACE:
-		addr = &dev->mem_space;
-		break;
-	default:
-		dev_err(&dev->dev,
-			"Slot [%d:%d] space number %d doesn't exist !\n",
-			dev->bus_nr, dev->slot, space);
-		return NULL;
-		break;
-	}
+	pci_disable_device(tpci200->info->pdev);
+	pci_dev_put(tpci200->info->pdev);
 
-	if ((addr->size == 0) || (addr->address == NULL)) {
-		dev_err(&dev->dev, "Error, slot space not mapped !\n");
-		return NULL;
+	for (i = 0; i < TPCI200_NB_SLOT; i++) {
+		tpci200->slots[i].io_phys.address = NULL;
+		tpci200->slots[i].io_phys.size = 0;
+		tpci200->slots[i].id_phys.address = NULL;
+		tpci200->slots[i].id_phys.size = 0;
+		tpci200->slots[i].int_phys.address = NULL;
+		tpci200->slots[i].int_phys.size = 0;
+		tpci200->slots[i].mem_phys.address = NULL;
+		tpci200->slots[i].mem_phys.size = 0;
 	}
-
-	return addr;
 }
 
-static int tpci200_read8(struct ipack_device *dev, int space,
-			 unsigned long offset, unsigned char *value)
+static void tpci200_enable_irq(struct tpci200_board *tpci200,
+			       int islot)
 {
-	struct ipack_addr_space *addr;
-	struct tpci200_board *tpci200;
-
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
-
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
-
-	if (offset >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
-	}
-
-	*value = __tpci200_read8(addr->address, offset);
-
-	return 0;
+	tpci200_set_mask(tpci200,
+			&tpci200->info->interface_regs->control[islot],
+			TPCI200_INT0_EN | TPCI200_INT1_EN);
 }
 
-static int tpci200_read16(struct ipack_device *dev, int space,
-			  unsigned long offset, unsigned short *value)
+static void tpci200_disable_irq(struct tpci200_board *tpci200,
+				int islot)
 {
-	struct ipack_addr_space *addr;
-	struct tpci200_board *tpci200;
-
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
-
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
-
-	if ((offset+2) >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
-	}
-	*value = __tpci200_read16(addr->address, offset);
-
-	return 0;
+	tpci200_clear_mask(tpci200,
+			&tpci200->info->interface_regs->control[islot],
+			TPCI200_INT0_EN | TPCI200_INT1_EN);
 }
 
-static int tpci200_read32(struct ipack_device *dev, int space,
-			  unsigned long offset, unsigned int *value)
+static irqreturn_t tpci200_slot_irq(struct slot_irq *slot_irq)
 {
-	struct ipack_addr_space *addr;
-	struct tpci200_board *tpci200;
-
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
-
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
+	irqreturn_t ret;
 
-	if ((offset+4) >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
-	}
-
-	*value = __tpci200_read32(addr->address, offset);
+	if (!slot_irq)
+		return -ENODEV;
+	ret = slot_irq->handler(slot_irq->arg);
 
-	return 0;
+	return ret;
 }
 
-static int tpci200_write8(struct ipack_device *dev, int space,
-			  unsigned long offset, unsigned char value)
+static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
 {
-	struct ipack_addr_space *addr;
-	struct tpci200_board *tpci200;
+	struct tpci200_board *tpci200 = (struct tpci200_board *) dev_id;
+	struct slot_irq *slot_irq;
+	irqreturn_t ret;
+	u16 status_reg;
+	int i;
 
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
+	/* Read status register */
+	status_reg = ioread16(&tpci200->info->interface_regs->status);
 
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
+	/* Did we cause the interrupt? */
+	if (!(status_reg & TPCI200_SLOT_INT_MASK))
+		return IRQ_NONE;
 
-	if (offset >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
+	/* callback to the IRQ handler for the corresponding slot */
+	rcu_read_lock();
+	for (i = 0; i < TPCI200_NB_SLOT; i++) {
+		if (!(status_reg & ((TPCI200_A_INT0 | TPCI200_A_INT1) << (2 * i))))
+			continue;
+		slot_irq = rcu_dereference(tpci200->slots[i].irq);
+		ret = tpci200_slot_irq(slot_irq);
+		if (ret == -ENODEV) {
+			dev_info(&tpci200->info->pdev->dev,
+				 "No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n",
+				 tpci200->number, i);
+			tpci200_disable_irq(tpci200, i);
+		}
 	}
+	rcu_read_unlock();
 
-	__tpci200_write8(value, addr->address, offset);
-
-	return 0;
+	return IRQ_HANDLED;
 }
 
-static int tpci200_write16(struct ipack_device *dev, int space,
-			   unsigned long offset, unsigned short value)
+static int tpci200_free_irq(struct ipack_device *dev)
 {
-	struct ipack_addr_space *addr;
+	struct slot_irq *slot_irq;
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
 	if (tpci200 == NULL)
 		return -EINVAL;
 
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
-	if ((offset+2) >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
+	if (tpci200->slots[dev->slot].irq == NULL) {
+		mutex_unlock(&tpci200->mutex);
+		return -EINVAL;
 	}
 
-	__tpci200_write16(value, addr->address, offset);
-
+	tpci200_disable_irq(tpci200, dev->slot);
+	slot_irq = tpci200->slots[dev->slot].irq;
+	/* uninstall handler */
+	RCU_INIT_POINTER(tpci200->slots[dev->slot].irq, NULL);
+	synchronize_rcu();
+	kfree(slot_irq);
+	mutex_unlock(&tpci200->mutex);
 	return 0;
 }
 
-static int tpci200_write32(struct ipack_device *dev, int space,
-			   unsigned long offset, unsigned int value)
+static int tpci200_request_irq(struct ipack_device *dev,
+			       irqreturn_t (*handler)(void *), void *arg)
 {
-	struct ipack_addr_space *addr;
+	int res = 0;
+	struct slot_irq *slot_irq;
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
 	if (tpci200 == NULL)
 		return -EINVAL;
 
-	addr = get_slot_address_space(dev, space);
-	if (addr == NULL)
-		return -EINVAL;
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
-	if ((offset+4) >= addr->size) {
-		dev_err(&dev->dev, "Error, slot space offset error !\n");
-		return -EFAULT;
+	if (tpci200->slots[dev->slot].irq != NULL) {
+		dev_err(&dev->dev,
+			"Slot [%d:%d] IRQ already registered !\n", dev->bus_nr,
+			dev->slot);
+		res = -EINVAL;
+		goto out_unlock;
 	}
 
-	__tpci200_write32(value, addr->address, offset);
-
-	return 0;
-}
-
-static void tpci200_unregister(struct tpci200_board *tpci200)
-{
-	int i;
-
-	free_irq(tpci200->info->pdev->irq, (void *) tpci200);
-
-	pci_iounmap(tpci200->info->pdev, tpci200->info->interface_regs);
-	pci_iounmap(tpci200->info->pdev, tpci200->info->ioidint_space);
-	pci_iounmap(tpci200->info->pdev, tpci200->info->mem8_space);
-
-	pci_release_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR);
-	pci_release_region(tpci200->info->pdev, TPCI200_IO_ID_INT_SPACES_BAR);
-	pci_release_region(tpci200->info->pdev, TPCI200_MEM8_SPACE_BAR);
-
-	pci_disable_device(tpci200->info->pdev);
-	pci_dev_put(tpci200->info->pdev);
-
-	for (i = 0; i < TPCI200_NB_SLOT; i++) {
-		tpci200->slots[i].io_phys.address = NULL;
-		tpci200->slots[i].io_phys.size = 0;
-		tpci200->slots[i].id_phys.address = NULL;
-		tpci200->slots[i].id_phys.size = 0;
-		tpci200->slots[i].mem_phys.address = NULL;
-		tpci200->slots[i].mem_phys.size = 0;
+	slot_irq = kzalloc(sizeof(struct slot_irq), GFP_KERNEL);
+	if (slot_irq == NULL) {
+		dev_err(&dev->dev,
+			"Slot [%d:%d] unable to allocate memory for IRQ !\n",
+			dev->bus_nr, dev->slot);
+		res = -ENOMEM;
+		goto out_unlock;
 	}
-}
 
-static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
-{
-	struct tpci200_board *tpci200 = (struct tpci200_board *) dev_id;
-	int i;
-	unsigned short status_reg, reg_value;
-	unsigned short unhandled_ints = 0;
-	irqreturn_t ret = IRQ_NONE;
+	/*
+	 * WARNING: Setup Interrupt Vector in the IndustryPack device
+	 * before an IRQ request.
+	 * Read the User Manual of your IndustryPack device to know
+	 * where to write the vector in memory.
+	 */
+	slot_irq->handler = handler;
+	slot_irq->arg = arg;
+	slot_irq->holder = dev;
 
-	/* Read status register */
-	status_reg = readw(tpci200->info->interface_regs +
-			   TPCI200_STATUS_REG);
-
-	if (status_reg & TPCI200_SLOT_INT_MASK) {
-		unhandled_ints = status_reg & TPCI200_SLOT_INT_MASK;
-		/* callback to the IRQ handler for the corresponding slot */
-		for (i = 0; i < TPCI200_NB_SLOT; i++) {
-			if ((tpci200->slots[i].irq != NULL) &&
-			    (status_reg & ((TPCI200_A_INT0 | TPCI200_A_INT1) << (2*i)))) {
-
-				ret = tpci200->slots[i].irq->handler(tpci200->slots[i].irq->arg);
-
-				/* Dummy reads */
-				readw(tpci200->slots[i].dev->io_space.address +
-				      0xC0);
-				readw(tpci200->slots[i].dev->io_space.address +
-				      0xC2);
-
-				unhandled_ints &= ~(((TPCI200_A_INT0 | TPCI200_A_INT1) << (2*i)));
-			}
-		}
-	}
-	/* Interrupt not handled are disabled */
-	if (unhandled_ints) {
-		for (i = 0; i < TPCI200_NB_SLOT; i++) {
-			if (unhandled_ints & ((TPCI200_INT0_EN | TPCI200_INT1_EN) << (2*i))) {
-				dev_info(&tpci200->slots[i].dev->dev,
-					 "No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n",
-					 tpci200->number, i);
-				reg_value = readw(
-					tpci200->info->interface_regs +
-					control_reg[i]);
-				reg_value &=
-					~(TPCI200_INT0_EN | TPCI200_INT1_EN);
-				writew(reg_value,
-				       (tpci200->info->interface_regs +
-					control_reg[i]));
-			}
-		}
-	}
+	rcu_assign_pointer(tpci200->slots[dev->slot].irq, slot_irq);
+	tpci200_enable_irq(tpci200, dev->slot);
 
-	return ret;
+out_unlock:
+	mutex_unlock(&tpci200->mutex);
+	return res;
 }
 
 static int tpci200_register(struct tpci200_board *tpci200)
@@ -398,18 +286,21 @@ static int tpci200_register(struct tpci200_board *tpci200)
 
 	/* Map internal tpci200 driver user space */
 	tpci200->info->interface_regs =
-		ioremap(pci_resource_start(tpci200->info->pdev,
+		ioremap_nocache(pci_resource_start(tpci200->info->pdev,
 					   TPCI200_IP_INTERFACE_BAR),
 			TPCI200_IFACE_SIZE);
 	tpci200->info->ioidint_space =
-		ioremap(pci_resource_start(tpci200->info->pdev,
+		ioremap_nocache(pci_resource_start(tpci200->info->pdev,
 					   TPCI200_IO_ID_INT_SPACES_BAR),
 			TPCI200_IOIDINT_SIZE);
 	tpci200->info->mem8_space =
-		ioremap(pci_resource_start(tpci200->info->pdev,
+		ioremap_nocache(pci_resource_start(tpci200->info->pdev,
 					   TPCI200_MEM8_SPACE_BAR),
 			TPCI200_MEM8_SIZE);
 
+	/* Initialize lock that protects interface_regs */
+	spin_lock_init(&tpci200->regs_lock);
+
 	ioidint_base = pci_resource_start(tpci200->info->pdev,
 					  TPCI200_IO_ID_INT_SPACES_BAR);
 	mem_base = pci_resource_start(tpci200->info->pdev,
@@ -437,12 +328,16 @@ static int tpci200_register(struct tpci200_board *tpci200)
 			TPCI200_ID_SPACE_OFF + TPCI200_ID_SPACE_GAP*i;
 		tpci200->slots[i].id_phys.size = TPCI200_ID_SPACE_SIZE;
 
+		tpci200->slots[i].int_phys.address =
+			(void __iomem *)ioidint_base +
+			TPCI200_INT_SPACE_OFF + TPCI200_INT_SPACE_GAP * i;
+		tpci200->slots[i].int_phys.size = TPCI200_INT_SPACE_SIZE;
+
 		tpci200->slots[i].mem_phys.address =
 			(void __iomem *)mem_base + TPCI200_MEM8_GAP*i;
 		tpci200->slots[i].mem_phys.size = TPCI200_MEM8_SIZE;
 
-		writew(slot_ctrl, (tpci200->info->interface_regs +
-				   control_reg[i]));
+		writew(slot_ctrl, &tpci200->info->interface_regs->control[i]);
 	}
 
 	res = request_irq(tpci200->info->pdev->irq,
@@ -467,70 +362,6 @@ out_disable_pci:
 	return res;
 }
 
-static int __tpci200_request_irq(struct tpci200_board *tpci200,
-				 struct ipack_device *dev)
-{
-	unsigned short slot_ctrl;
-
-	/* Set the default parameters of the slot
-	 * INT0 enabled, level sensitive
-	 * INT1 enabled, level sensitive
-	 * error interrupt disabled
-	 * timeout interrupt disabled
-	 * recover time disabled
-	 * clock rate 8 MHz
-	 */
-	slot_ctrl = TPCI200_INT0_EN | TPCI200_INT1_EN;
-	writew(slot_ctrl, (tpci200->info->interface_regs +
-			   control_reg[dev->slot]));
-
-	return 0;
-}
-
-static void __tpci200_free_irq(struct tpci200_board *tpci200,
-			       struct ipack_device *dev)
-{
-	unsigned short slot_ctrl;
-
-	/* Set the default parameters of the slot
-	 * INT0 disabled, level sensitive
-	 * INT1 disabled, level sensitive
-	 * error interrupt disabled
-	 * timeout interrupt disabled
-	 * recover time disabled
-	 * clock rate 8 MHz
-	 */
-	slot_ctrl = 0;
-	writew(slot_ctrl, (tpci200->info->interface_regs +
-			   control_reg[dev->slot]));
-}
-
-static int tpci200_free_irq(struct ipack_device *dev)
-{
-	struct slot_irq *slot_irq;
-	struct tpci200_board *tpci200;
-
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
-
-	if (mutex_lock_interruptible(&tpci200->mutex))
-		return -ERESTARTSYS;
-
-	if (tpci200->slots[dev->slot].irq == NULL) {
-		mutex_unlock(&tpci200->mutex);
-		return -EINVAL;
-	}
-
-	__tpci200_free_irq(tpci200, dev);
-	slot_irq = tpci200->slots[dev->slot].irq;
-	tpci200->slots[dev->slot].irq = NULL;
-	kfree(slot_irq);
-
-	mutex_unlock(&tpci200->mutex);
-	return 0;
-}
-
 static int tpci200_slot_unmap_space(struct ipack_device *dev, int space)
 {
 	struct ipack_addr_space *virt_addr_space;
@@ -562,6 +393,15 @@ static int tpci200_slot_unmap_space(struct ipack_device *dev, int space)
 		}
 		virt_addr_space = &dev->id_space;
 		break;
+	case IPACK_INT_SPACE:
+		if (dev->int_space.address == NULL) {
+			dev_info(&dev->dev,
+				 "Slot [%d:%d] INT space not mapped !\n",
+				 dev->bus_nr, dev->slot);
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->int_space;
+		break;
 	case IPACK_MEM_SPACE:
 		if (dev->mem_space.address == NULL) {
 			dev_info(&dev->dev,
@@ -588,29 +428,6 @@ out_unlock:
 	return 0;
 }
 
-static int tpci200_slot_unregister(struct ipack_device *dev)
-{
-	struct tpci200_board *tpci200;
-
-	if (dev == NULL)
-		return -ENODEV;
-
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
-
-	tpci200_free_irq(dev);
-
-	if (mutex_lock_interruptible(&tpci200->mutex))
-		return -ERESTARTSYS;
-
-	ipack_device_unregister(dev);
-	tpci200->slots[dev->slot].dev = NULL;
-	mutex_unlock(&tpci200->mutex);
-
-	return 0;
-}
-
 static int tpci200_slot_map_space(struct ipack_device *dev,
 				  unsigned int memory_size, int space)
 {
@@ -654,6 +471,19 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 		phys_address = tpci200->slots[dev->slot].id_phys.address;
 		size_to_map = tpci200->slots[dev->slot].id_phys.size;
 		break;
+	case IPACK_INT_SPACE:
+		if (dev->int_space.address != NULL) {
+			dev_err(&dev->dev,
+				"Slot [%d:%d] INT space already mapped !\n",
+				tpci200->number, dev->slot);
+			res = -EINVAL;
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->int_space;
+
+		phys_address = tpci200->slots[dev->slot].int_phys.address;
+		size_to_map = tpci200->slots[dev->slot].int_phys.size;
+		break;
 	case IPACK_MEM_SPACE:
 		if (dev->mem_space.address != NULL) {
 			dev_err(&dev->dev,
@@ -685,85 +515,109 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 
 	virt_addr_space->size = size_to_map;
 	virt_addr_space->address =
-		ioremap((unsigned long)phys_address, size_to_map);
+		ioremap_nocache((unsigned long)phys_address, size_to_map);
 
 out_unlock:
 	mutex_unlock(&tpci200->mutex);
 	return res;
 }
 
-static int tpci200_request_irq(struct ipack_device *dev, int vector,
-			       int (*handler)(void *), void *arg)
+static int tpci200_get_clockrate(struct ipack_device *dev)
 {
-	int res;
-	struct slot_irq *slot_irq;
-	struct tpci200_board *tpci200;
+	struct tpci200_board *tpci200 = check_slot(dev);
+	__le16 __iomem *addr;
 
-	tpci200 = check_slot(dev);
-	if (tpci200 == NULL)
-		return -EINVAL;
+	if (!tpci200)
+		return -ENODEV;
 
-	if (mutex_lock_interruptible(&tpci200->mutex))
-		return -ERESTARTSYS;
+	addr = &tpci200->info->interface_regs->control[dev->slot];
+	return (ioread16(addr) & TPCI200_CLK32) ? 32 : 8;
+}
 
-	if (tpci200->slots[dev->slot].irq != NULL) {
-		dev_err(&dev->dev,
-			"Slot [%d:%d] IRQ already registered !\n", dev->bus_nr,
-			dev->slot);
-		res = -EINVAL;
-		goto out_unlock;
-	}
+static int tpci200_set_clockrate(struct ipack_device *dev, int mherz)
+{
+	struct tpci200_board *tpci200 = check_slot(dev);
+	__le16 __iomem *addr;
 
-	slot_irq = kzalloc(sizeof(struct slot_irq), GFP_KERNEL);
-	if (slot_irq == NULL) {
-		dev_err(&dev->dev,
-			"Slot [%d:%d] unable to allocate memory for IRQ !\n",
-			dev->bus_nr, dev->slot);
-		res = -ENOMEM;
-		goto out_unlock;
+	if (!tpci200)
+		return -ENODEV;
+
+	addr = &tpci200->info->interface_regs->control[dev->slot];
+
+	switch (mherz) {
+	case 8:
+		tpci200_clear_mask(tpci200, addr, TPCI200_CLK32);
+		break;
+	case 32:
+		tpci200_set_mask(tpci200, addr, TPCI200_CLK32);
+		break;
+	default:
+		return -EINVAL;
 	}
+	return 0;
+}
 
-	/*
-	 * WARNING: Setup Interrupt Vector in the IndustryPack device
-	 * before an IRQ request.
-	 * Read the User Manual of your IndustryPack device to know
-	 * where to write the vector in memory.
-	 */
-	slot_irq->vector = vector;
-	slot_irq->handler = handler;
-	slot_irq->arg = arg;
+static int tpci200_get_error(struct ipack_device *dev)
+{
+	struct tpci200_board *tpci200 = check_slot(dev);
+	__le16 __iomem *addr;
+	u16 mask;
 
-	tpci200->slots[dev->slot].irq = slot_irq;
-	res = __tpci200_request_irq(tpci200, dev);
+	if (!tpci200)
+		return -ENODEV;
 
-out_unlock:
-	mutex_unlock(&tpci200->mutex);
-	return res;
+	addr = &tpci200->info->interface_regs->status;
+	mask = tpci200_status_error[dev->slot];
+	return (ioread16(addr) & mask) ? 1 : 0;
 }
 
-static void tpci200_uninstall(struct tpci200_board *tpci200)
+static int tpci200_get_timeout(struct ipack_device *dev)
 {
-	int i;
+	struct tpci200_board *tpci200 = check_slot(dev);
+	__le16 __iomem *addr;
+	u16 mask;
 
-	for (i = 0; i < TPCI200_NB_SLOT; i++)
-		tpci200_slot_unregister(tpci200->slots[i].dev);
+	if (!tpci200)
+		return -ENODEV;
+
+	addr = &tpci200->info->interface_regs->status;
+	mask = tpci200_status_timeout[dev->slot];
+
+	return (ioread16(addr) & mask) ? 1 : 0;
+}
+
+static int tpci200_reset_timeout(struct ipack_device *dev)
+{
+	struct tpci200_board *tpci200 = check_slot(dev);
+	__le16 __iomem *addr;
+	u16 mask;
+
+	if (!tpci200)
+		return -ENODEV;
+
+	addr = &tpci200->info->interface_regs->status;
+	mask = tpci200_status_timeout[dev->slot];
 
+	iowrite16(mask, addr);
+	return 0;
+}
+
+static void tpci200_uninstall(struct tpci200_board *tpci200)
+{
 	tpci200_unregister(tpci200);
 	kfree(tpci200->slots);
 }
 
-static struct ipack_bus_ops tpci200_bus_ops = {
+static const struct ipack_bus_ops tpci200_bus_ops = {
 	.map_space = tpci200_slot_map_space,
 	.unmap_space = tpci200_slot_unmap_space,
 	.request_irq = tpci200_request_irq,
 	.free_irq = tpci200_free_irq,
-	.read8 = tpci200_read8,
-	.read16 = tpci200_read16,
-	.read32 = tpci200_read32,
-	.write8 = tpci200_write8,
-	.write16 = tpci200_write16,
-	.write32 = tpci200_write32,
-	.remove_device = tpci200_slot_unregister,
+	.get_clockrate = tpci200_get_clockrate,
+	.set_clockrate = tpci200_set_clockrate,
+	.get_error     = tpci200_get_error,
+	.get_timeout   = tpci200_get_timeout,
+	.reset_timeout = tpci200_reset_timeout,
 };
 
 static int tpci200_install(struct tpci200_board *tpci200)
@@ -786,11 +640,12 @@ static int tpci200_install(struct tpci200_board *tpci200)
 	return 0;
 }
 
-static int tpci200_pciprobe(struct pci_dev *pdev,
-			    const struct pci_device_id *id)
+static int tpci200_pci_probe(struct pci_dev *pdev,
+			     const struct pci_device_id *id)
 {
 	int ret, i;
 	struct tpci200_board *tpci200;
+	u32 reg32;
 
 	tpci200 = kzalloc(sizeof(struct tpci200_board), GFP_KERNEL);
 	if (!tpci200)
@@ -798,10 +653,40 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 
 	tpci200->info = kzalloc(sizeof(struct tpci200_infos), GFP_KERNEL);
 	if (!tpci200->info) {
-		kfree(tpci200);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto out_err_info;
 	}
 
+	pci_dev_get(pdev);
+
+	/* Obtain a mapping of the carrier's PCI configuration registers */
+	ret = pci_request_region(pdev, TPCI200_CFG_MEM_BAR,
+				 KBUILD_MODNAME " Configuration Memory");
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to allocate PCI Configuration Memory");
+		ret = -EBUSY;
+		goto out_err_pci_request;
+	}
+	tpci200->info->cfg_regs = ioremap_nocache(
+			pci_resource_start(pdev, TPCI200_CFG_MEM_BAR),
+			pci_resource_len(pdev, TPCI200_CFG_MEM_BAR));
+	if (!tpci200->info->cfg_regs) {
+		dev_err(&pdev->dev, "Failed to map PCI Configuration Memory");
+		ret = -EFAULT;
+		goto out_err_ioremap;
+	}
+
+	/* Disable byte swapping for 16 bit IP module access. This will ensure
+	 * that the Industrypack big endian byte order is preserved by the
+	 * carrier. */
+	reg32 = ioread32(tpci200->info->cfg_regs + LAS1_DESC);
+	reg32 |= 1 << LAS_BIT_BIGENDIAN;
+	iowrite32(reg32, tpci200->info->cfg_regs + LAS1_DESC);
+
+	reg32 = ioread32(tpci200->info->cfg_regs + LAS2_DESC);
+	reg32 |= 1 << LAS_BIT_BIGENDIAN;
+	iowrite32(reg32, tpci200->info->cfg_regs + LAS2_DESC);
+
 	/* Save struct pci_dev pointer */
 	tpci200->info->pdev = pdev;
 	tpci200->info->id_table = (struct pci_device_id *)id;
@@ -809,10 +694,9 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 	/* register the device and initialize it */
 	ret = tpci200_install(tpci200);
 	if (ret) {
-		dev_err(&pdev->dev, "Error during tpci200 install !\n");
-		kfree(tpci200->info);
-		kfree(tpci200);
-		return -ENODEV;
+		dev_err(&pdev->dev, "error during tpci200 install\n");
+		ret = -ENODEV;
+		goto out_err_install;
 	}
 
 	/* Register the carrier in the industry pack bus driver */
@@ -822,48 +706,46 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 	if (!tpci200->info->ipack_bus) {
 		dev_err(&pdev->dev,
 			"error registering the carrier on ipack driver\n");
-		tpci200_uninstall(tpci200);
-		kfree(tpci200->info);
-		kfree(tpci200);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto out_err_bus_register;
 	}
 
 	/* save the bus number given by ipack to logging purpose */
 	tpci200->number = tpci200->info->ipack_bus->bus_nr;
 	dev_set_drvdata(&pdev->dev, tpci200);
-	/* add the registered device in an internal linked list */
-	list_add_tail(&tpci200->list, &tpci200_list);
 
-	/*
-	 * Give the same IRQ number as the slot number.
-	 * The TPCI200 has assigned his own two IRQ by PCI bus driver
-	 */
 	for (i = 0; i < TPCI200_NB_SLOT; i++)
-		tpci200->slots[i].dev =
-			ipack_device_register(tpci200->info->ipack_bus, i, i);
+		ipack_device_register(tpci200->info->ipack_bus, i);
+	return 0;
+
+out_err_bus_register:
+	tpci200_uninstall(tpci200);
+out_err_install:
+	iounmap(tpci200->info->cfg_regs);
+out_err_ioremap:
+	pci_release_region(pdev, TPCI200_CFG_MEM_BAR);
+out_err_pci_request:
+	pci_dev_put(pdev);
+	kfree(tpci200->info);
+out_err_info:
+	kfree(tpci200);
 	return ret;
 }
 
 static void __tpci200_pci_remove(struct tpci200_board *tpci200)
 {
-	tpci200_uninstall(tpci200);
-	list_del(&tpci200->list);
 	ipack_bus_unregister(tpci200->info->ipack_bus);
+	tpci200_uninstall(tpci200);
+
 	kfree(tpci200->info);
 	kfree(tpci200);
 }
 
 static void __devexit tpci200_pci_remove(struct pci_dev *dev)
 {
-	struct tpci200_board *tpci200, *next;
+	struct tpci200_board *tpci200 = pci_get_drvdata(dev);
 
-	/* Search the registered device to uninstall it */
-	list_for_each_entry_safe(tpci200, next, &tpci200_list, list) {
-		if (tpci200->info->pdev == dev) {
-			__tpci200_pci_remove(tpci200);
-			break;
-		}
-	}
+	__tpci200_pci_remove(tpci200);
 }
 
 static DEFINE_PCI_DEVICE_TABLE(tpci200_idtable) = {
@@ -877,7 +759,7 @@ MODULE_DEVICE_TABLE(pci, tpci200_idtable);
 static struct pci_driver tpci200_pci_drv = {
 	.name = "tpci200",
 	.id_table = tpci200_idtable,
-	.probe = tpci200_pciprobe,
+	.probe = tpci200_pci_probe,
 	.remove = __devexit_p(tpci200_pci_remove),
 };
 
@@ -888,11 +770,6 @@ static int __init tpci200_drvr_init_module(void)
 
 static void __exit tpci200_drvr_exit_module(void)
 {
-	struct tpci200_board *tpci200, *next;
-
-	list_for_each_entry_safe(tpci200, next, &tpci200_list, list)
-		__tpci200_pci_remove(tpci200);
-
 	pci_unregister_driver(&tpci200_pci_drv);
 }
 
diff --git a/drivers/staging/ipack/bridges/tpci200.h b/drivers/staging/ipack/bridges/tpci200.h
index d04510a89be4..235d1fe4f48c 100644
--- a/drivers/staging/ipack/bridges/tpci200.h
+++ b/drivers/staging/ipack/bridges/tpci200.h
@@ -17,7 +17,6 @@
 #include <linux/limits.h>
 #include <linux/pci.h>
 #include <linux/spinlock.h>
-#include <linux/interrupt.h>
 #include <linux/swab.h>
 #include <linux/io.h>
 
@@ -31,18 +30,21 @@
 #define TPCI200_SUBVENDOR_ID          0x1498
 #define TPCI200_SUBDEVICE_ID          0x300A
 
+#define TPCI200_CFG_MEM_BAR           0
 #define TPCI200_IP_INTERFACE_BAR      2
 #define TPCI200_IO_ID_INT_SPACES_BAR  3
 #define TPCI200_MEM16_SPACE_BAR       4
 #define TPCI200_MEM8_SPACE_BAR        5
 
-#define TPCI200_REVISION_REG          0x00
-#define TPCI200_CONTROL_A_REG         0x02
-#define TPCI200_CONTROL_B_REG         0x04
-#define TPCI200_CONTROL_C_REG         0x06
-#define TPCI200_CONTROL_D_REG         0x08
-#define TPCI200_RESET_REG             0x0A
-#define TPCI200_STATUS_REG            0x0C
+struct tpci200_regs {
+	__le16	revision;
+	/* writes to control should occur with the mutex held to protect
+	 * read-modify-write operations */
+	__le16  control[4];
+	__le16	reset;
+	__le16	status;
+	u8	reserved[242];
+} __packed;
 
 #define TPCI200_IFACE_SIZE            0x100
 
@@ -62,6 +64,7 @@
 #define TPCI200_MEM16_GAP             0x00800000
 #define TPCI200_MEM16_SIZE            0x00800000
 
+/* control field in tpci200_regs */
 #define TPCI200_INT0_EN               0x0040
 #define TPCI200_INT1_EN               0x0080
 #define TPCI200_INT0_EDGE             0x0010
@@ -71,11 +74,13 @@
 #define TPCI200_RECOVER_EN            0x0002
 #define TPCI200_CLK32                 0x0001
 
+/* reset field in tpci200_regs */
 #define TPCI200_A_RESET               0x0001
 #define TPCI200_B_RESET               0x0002
 #define TPCI200_C_RESET               0x0004
 #define TPCI200_D_RESET               0x0008
 
+/* status field in tpci200_regs */
 #define TPCI200_A_TIMEOUT             0x1000
 #define TPCI200_B_TIMEOUT             0x2000
 #define TPCI200_C_TIMEOUT             0x4000
@@ -97,6 +102,13 @@
 
 #define TPCI200_SLOT_INT_MASK         0x00FF
 
+/* PCI Configuration registers. The PCI bridge is a PLX Technology PCI9030. */
+#define LAS1_DESC		      0x2C
+#define LAS2_DESC		      0x30
+
+/* Bits in the LAS?_DESC registers */
+#define LAS_BIT_BIGENDIAN	      24
+
 #define VME_IOID_SPACE  "IOID"
 #define VME_MEM_SPACE  "MEM"
 
@@ -108,8 +120,9 @@
  *
  */
 struct slot_irq {
+	struct ipack_device *holder;
 	int		vector;
-	int		(*handler)(void *);
+	irqreturn_t	(*handler)(void *);
 	void		*arg;
 };
 
@@ -119,14 +132,15 @@ struct slot_irq {
  * @irq		Slot IRQ infos
  * @io_phys	IO physical base address register of the slot
  * @id_phys	ID physical base address register of the slot
+ * @int_phys	INT physical base address register of the slot
  * @mem_phys	MEM physical base address register of the slot
  *
  */
 struct tpci200_slot {
-	struct ipack_device	*dev;
 	struct slot_irq		*irq;
 	struct ipack_addr_space io_phys;
 	struct ipack_addr_space id_phys;
+	struct ipack_addr_space int_phys;
 	struct ipack_addr_space mem_phys;
 };
 
@@ -141,15 +155,16 @@ struct tpci200_slot {
 struct tpci200_infos {
 	struct pci_dev			*pdev;
 	struct pci_device_id		*id_table;
-	void __iomem			*interface_regs;
+	struct tpci200_regs __iomem	*interface_regs;
 	void __iomem			*ioidint_space;
 	void __iomem			*mem8_space;
+	void __iomem			*cfg_regs;
 	struct ipack_bus_device		*ipack_bus;
 };
 struct tpci200_board {
-	struct list_head	list;
 	unsigned int		number;
 	struct mutex		mutex;
+	spinlock_t		regs_lock;
 	struct tpci200_slot	*slots;
 	struct tpci200_infos	*info;
 };
diff --git a/drivers/staging/ipack/devices/ipoctal.c b/drivers/staging/ipack/devices/ipoctal.c
index fd0e30132ca2..d751edfda839 100644
--- a/drivers/staging/ipack/devices/ipoctal.c
+++ b/drivers/staging/ipack/devices/ipoctal.c
@@ -20,127 +20,68 @@
 #include <linux/tty_flip.h>
 #include <linux/slab.h>
 #include <linux/atomic.h>
+#include <linux/io.h>
 #include "../ipack.h"
 #include "ipoctal.h"
 #include "scc2698.h"
 
-#define IP_OCTAL_MANUFACTURER_ID    0xF0
-#define IP_OCTAL_232_ID             0x22
-#define IP_OCTAL_422_ID             0x2A
-#define IP_OCTAL_485_ID             0x48
-
 #define IP_OCTAL_ID_SPACE_VECTOR    0x41
 #define IP_OCTAL_NB_BLOCKS          4
 
-static struct ipack_driver driver;
 static const struct tty_operations ipoctal_fops;
 
+struct ipoctal_channel {
+	struct ipoctal_stats		stats;
+	unsigned int			nb_bytes;
+	wait_queue_head_t		queue;
+	spinlock_t			lock;
+	unsigned int			pointer_read;
+	unsigned int			pointer_write;
+	atomic_t			open;
+	struct tty_port			tty_port;
+	union scc2698_channel __iomem	*regs;
+	union scc2698_block __iomem	*block_regs;
+	unsigned int			board_id;
+	unsigned char			*board_write;
+	u8				isr_rx_rdy_mask;
+	u8				isr_tx_rdy_mask;
+};
+
 struct ipoctal {
-	struct list_head		list;
 	struct ipack_device		*dev;
 	unsigned int			board_id;
-	struct scc2698_channel		*chan_regs;
-	struct scc2698_block		*block_regs;
-	struct ipoctal_stats		chan_stats[NR_CHANNELS];
-	unsigned int			nb_bytes[NR_CHANNELS];
-	unsigned int			count_wr[NR_CHANNELS];
-	wait_queue_head_t		queue[NR_CHANNELS];
-	spinlock_t			lock[NR_CHANNELS];
-	unsigned int			pointer_read[NR_CHANNELS];
-	unsigned int			pointer_write[NR_CHANNELS];
-	atomic_t			open[NR_CHANNELS];
+	struct ipoctal_channel		channel[NR_CHANNELS];
 	unsigned char			write;
-	struct tty_port			tty_port[NR_CHANNELS];
 	struct tty_driver		*tty_drv;
 };
 
-/* Linked list to save the registered devices */
-static LIST_HEAD(ipoctal_list);
-
-static inline void ipoctal_write_io_reg(struct ipoctal *ipoctal,
-					unsigned char *dest,
-					unsigned char value)
-{
-	unsigned long offset;
-
-	offset = ((void __iomem *) dest) - ipoctal->dev->io_space.address;
-	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_IO_SPACE, offset,
-				       value);
-}
-
-static inline void ipoctal_write_cr_cmd(struct ipoctal *ipoctal,
-					unsigned char *dest,
-					unsigned char value)
-{
-	ipoctal_write_io_reg(ipoctal, dest, value);
-}
-
-static inline unsigned char ipoctal_read_io_reg(struct ipoctal *ipoctal,
-						unsigned char *src)
-{
-	unsigned long offset;
-	unsigned char value;
-
-	offset = ((void __iomem *) src) - ipoctal->dev->io_space.address;
-	ipoctal->dev->bus->ops->read8(ipoctal->dev, IPACK_IO_SPACE, offset,
-				      &value);
-	return value;
-}
-
-static struct ipoctal *ipoctal_find_board(struct tty_struct *tty)
-{
-	struct ipoctal *p;
-
-	list_for_each_entry(p, &ipoctal_list, list) {
-		if (tty->driver->major == p->tty_drv->major)
-			return p;
-	}
-
-	return NULL;
-}
-
 static int ipoctal_port_activate(struct tty_port *port, struct tty_struct *tty)
 {
-	struct ipoctal *ipoctal;
-	int channel = tty->index;
+	struct ipoctal_channel *channel;
 
-	ipoctal = ipoctal_find_board(tty);
+	channel = dev_get_drvdata(tty->dev);
 
-	if (ipoctal == NULL) {
-		dev_err(tty->dev, "Device not found. Major %d\n",
-			tty->driver->major);
-		return -ENODEV;
-	}
-
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_ENABLE_RX);
+	iowrite8(CR_ENABLE_RX, &channel->regs->w.cr);
 	return 0;
 }
 
 static int ipoctal_open(struct tty_struct *tty, struct file *file)
 {
-	int channel = tty->index;
 	int res;
-	struct ipoctal *ipoctal;
+	struct ipoctal_channel *channel;
 
-	ipoctal = ipoctal_find_board(tty);
-
-	if (ipoctal == NULL) {
-		dev_err(tty->dev, "Device not found. Major %d\n",
-			tty->driver->major);
-		return -ENODEV;
-	}
+	channel = dev_get_drvdata(tty->dev);
 
-	if (atomic_read(&ipoctal->open[channel]))
+	if (atomic_read(&channel->open))
 		return -EBUSY;
 
-	tty->driver_data = ipoctal;
+	tty->driver_data = channel;
 
-	res = tty_port_open(&ipoctal->tty_port[channel], tty, file);
+	res = tty_port_open(&channel->tty_port, tty, file);
 	if (res)
 		return res;
 
-	atomic_inc(&ipoctal->open[channel]);
+	atomic_inc(&channel->open);
 	return 0;
 }
 
@@ -154,175 +95,166 @@ static void ipoctal_reset_stats(struct ipoctal_stats *stats)
 	stats->parity_err = 0;
 }
 
-static void ipoctal_free_channel(struct tty_struct *tty)
+static void ipoctal_free_channel(struct ipoctal_channel *channel)
 {
-	int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
-
-	if (ipoctal == NULL)
-		return;
-
-	ipoctal_reset_stats(&ipoctal->chan_stats[channel]);
-	ipoctal->pointer_read[channel] = 0;
-	ipoctal->pointer_write[channel] = 0;
-	ipoctal->nb_bytes[channel] = 0;
+	ipoctal_reset_stats(&channel->stats);
+	channel->pointer_read = 0;
+	channel->pointer_write = 0;
+	channel->nb_bytes = 0;
 }
 
 static void ipoctal_close(struct tty_struct *tty, struct file *filp)
 {
-	int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	struct ipoctal_channel *channel = tty->driver_data;
 
-	tty_port_close(&ipoctal->tty_port[channel], tty, filp);
+	tty_port_close(&channel->tty_port, tty, filp);
 
-	if (atomic_dec_and_test(&ipoctal->open[channel]))
-		ipoctal_free_channel(tty);
+	if (atomic_dec_and_test(&channel->open))
+		ipoctal_free_channel(channel);
 }
 
 static int ipoctal_get_icount(struct tty_struct *tty,
 			      struct serial_icounter_struct *icount)
 {
-	struct ipoctal *ipoctal = tty->driver_data;
-	int channel = tty->index;
+	struct ipoctal_channel *channel = tty->driver_data;
 
 	icount->cts = 0;
 	icount->dsr = 0;
 	icount->rng = 0;
 	icount->dcd = 0;
-	icount->rx = ipoctal->chan_stats[channel].rx;
-	icount->tx = ipoctal->chan_stats[channel].tx;
-	icount->frame = ipoctal->chan_stats[channel].framing_err;
-	icount->parity = ipoctal->chan_stats[channel].parity_err;
-	icount->brk = ipoctal->chan_stats[channel].rcv_break;
+	icount->rx = channel->stats.rx;
+	icount->tx = channel->stats.tx;
+	icount->frame = channel->stats.framing_err;
+	icount->parity = channel->stats.parity_err;
+	icount->brk = channel->stats.rcv_break;
 	return 0;
 }
 
-static int ipoctal_irq_handler(void *arg)
+static void ipoctal_irq_rx(struct ipoctal_channel *channel,
+			   struct tty_struct *tty, u8 sr)
 {
-	unsigned int channel;
-	unsigned int block;
-	unsigned char isr;
-	unsigned char sr;
-	unsigned char isr_tx_rdy, isr_rx_rdy;
 	unsigned char value;
-	unsigned char flag;
-	struct tty_struct *tty;
-	struct ipoctal *ipoctal = (struct ipoctal *) arg;
-
-	/* Check all channels */
-	for (channel = 0; channel < NR_CHANNELS; channel++) {
-		/* If there is no client, skip the check */
-		if (!atomic_read(&ipoctal->open[channel]))
-			continue;
-
-		tty = tty_port_tty_get(&ipoctal->tty_port[channel]);
-		if (!tty)
-			continue;
-
-		/*
-		 * The HW is organized in pair of channels.
-		 * See which register we need to read from
-		 */
-		block = channel / 2;
-		isr = ipoctal_read_io_reg(ipoctal,
-					  &ipoctal->block_regs[block].u.r.isr);
-		sr = ipoctal_read_io_reg(ipoctal,
-					 &ipoctal->chan_regs[channel].u.r.sr);
-
-		if ((channel % 2) == 1) {
-			isr_tx_rdy = isr & ISR_TxRDY_B;
-			isr_rx_rdy = isr & ISR_RxRDY_FFULL_B;
-		} else {
-			isr_tx_rdy = isr & ISR_TxRDY_A;
-			isr_rx_rdy = isr & ISR_RxRDY_FFULL_A;
+	unsigned char flag = TTY_NORMAL;
+	u8 isr;
+
+	do {
+		value = ioread8(&channel->regs->r.rhr);
+		/* Error: count statistics */
+		if (sr & SR_ERROR) {
+			iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
+
+			if (sr & SR_OVERRUN_ERROR) {
+				channel->stats.overrun_err++;
+				/* Overrun doesn't affect the current character*/
+				tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+			}
+			if (sr & SR_PARITY_ERROR) {
+				channel->stats.parity_err++;
+				flag = TTY_PARITY;
+			}
+			if (sr & SR_FRAMING_ERROR) {
+				channel->stats.framing_err++;
+				flag = TTY_FRAME;
+			}
+			if (sr & SR_RECEIVED_BREAK) {
+				iowrite8(CR_CMD_RESET_BREAK_CHANGE, &channel->regs->w.cr);
+				channel->stats.rcv_break++;
+				flag = TTY_BREAK;
+			}
 		}
+		tty_insert_flip_char(tty, value, flag);
 
-		/* In case of RS-485, change from TX to RX when finishing TX.
-		 * Half-duplex.
+		/* Check if there are more characters in RX FIFO
+		 * If there are more, the isr register for this channel
+		 * has enabled the RxRDY|FFULL bit.
 		 */
-		if ((ipoctal->board_id == IP_OCTAL_485_ID) &&
-		    (sr & SR_TX_EMPTY) &&
-		    (ipoctal->nb_bytes[channel] == 0)) {
-			ipoctal_write_io_reg(ipoctal,
-					     &ipoctal->chan_regs[channel].u.w.cr,
-					     CR_DISABLE_TX);
-			ipoctal_write_cr_cmd(ipoctal,
-					     &ipoctal->chan_regs[channel].u.w.cr,
-					     CR_CMD_NEGATE_RTSN);
-			ipoctal_write_io_reg(ipoctal,
-					     &ipoctal->chan_regs[channel].u.w.cr,
-					     CR_ENABLE_RX);
-			ipoctal->write = 1;
-			wake_up_interruptible(&ipoctal->queue[channel]);
-		}
+		isr = ioread8(&channel->block_regs->r.isr);
+		sr = ioread8(&channel->regs->r.sr);
+	} while (isr & channel->isr_rx_rdy_mask);
 
-		/* RX data */
-		if (isr_rx_rdy && (sr & SR_RX_READY)) {
-			value = ipoctal_read_io_reg(ipoctal,
-						    &ipoctal->chan_regs[channel].u.r.rhr);
-			flag = TTY_NORMAL;
-
-			/* Error: count statistics */
-			if (sr & SR_ERROR) {
-				ipoctal_write_cr_cmd(ipoctal,
-						     &ipoctal->chan_regs[channel].u.w.cr,
-						     CR_CMD_RESET_ERR_STATUS);
-
-				if (sr & SR_OVERRUN_ERROR) {
-					ipoctal->chan_stats[channel].overrun_err++;
-					/* Overrun doesn't affect the current character*/
-					tty_insert_flip_char(tty, 0, TTY_OVERRUN);
-				}
-				if (sr & SR_PARITY_ERROR) {
-					ipoctal->chan_stats[channel].parity_err++;
-					flag = TTY_PARITY;
-				}
-				if (sr & SR_FRAMING_ERROR) {
-					ipoctal->chan_stats[channel].framing_err++;
-					flag = TTY_FRAME;
-				}
-				if (sr & SR_RECEIVED_BREAK) {
-					ipoctal->chan_stats[channel].rcv_break++;
-					flag = TTY_BREAK;
-				}
-			}
+	tty_flip_buffer_push(tty);
+}
 
-			tty_insert_flip_char(tty, value, flag);
-		}
+static void ipoctal_irq_tx(struct ipoctal_channel *channel)
+{
+	unsigned char value;
+	unsigned int *pointer_write = &channel->pointer_write;
 
-		/* TX of each character */
-		if (isr_tx_rdy && (sr & SR_TX_READY)) {
-			unsigned int *pointer_write =
-				&ipoctal->pointer_write[channel];
+	if (channel->nb_bytes <= 0) {
+		channel->nb_bytes = 0;
+		return;
+	}
 
-			if (ipoctal->nb_bytes[channel] <= 0) {
-				ipoctal->nb_bytes[channel] = 0;
-				continue;
-			}
+	value = channel->tty_port.xmit_buf[*pointer_write];
+	iowrite8(value, &channel->regs->w.thr);
+	channel->stats.tx++;
+	(*pointer_write)++;
+	*pointer_write = *pointer_write % PAGE_SIZE;
+	channel->nb_bytes--;
 
-			value = ipoctal->tty_port[channel].xmit_buf[*pointer_write];
-			ipoctal_write_io_reg(ipoctal,
-					     &ipoctal->chan_regs[channel].u.w.thr,
-					     value);
-			ipoctal->chan_stats[channel].tx++;
-			ipoctal->count_wr[channel]++;
-			(*pointer_write)++;
-			*pointer_write = *pointer_write % PAGE_SIZE;
-			ipoctal->nb_bytes[channel]--;
-
-			if ((ipoctal->nb_bytes[channel] == 0) &&
-			    (waitqueue_active(&ipoctal->queue[channel]))) {
-
-				if (ipoctal->board_id != IP_OCTAL_485_ID) {
-					ipoctal->write = 1;
-					wake_up_interruptible(&ipoctal->queue[channel]);
-				}
-			}
+	if ((channel->nb_bytes == 0) &&
+	    (waitqueue_active(&channel->queue))) {
+
+		if (channel->board_id != IPACK1_DEVICE_ID_SBS_OCTAL_485) {
+			*channel->board_write = 1;
+			wake_up_interruptible(&channel->queue);
 		}
+	}
+}
 
-		tty_flip_buffer_push(tty);
-		tty_kref_put(tty);
+static void ipoctal_irq_channel(struct ipoctal_channel *channel)
+{
+	u8 isr, sr;
+	struct tty_struct *tty;
+
+	/* If there is no client, skip the check */
+	if (!atomic_read(&channel->open))
+		return;
+
+	tty = tty_port_tty_get(&channel->tty_port);
+	if (!tty)
+		return;
+	/* The HW is organized in pair of channels.  See which register we need
+	 * to read from */
+	isr = ioread8(&channel->block_regs->r.isr);
+	sr = ioread8(&channel->regs->r.sr);
+
+	/* In case of RS-485, change from TX to RX when finishing TX.
+	 * Half-duplex. */
+	if ((channel->board_id == IPACK1_DEVICE_ID_SBS_OCTAL_485) &&
+	    (sr & SR_TX_EMPTY) && (channel->nb_bytes == 0)) {
+		iowrite8(CR_DISABLE_TX, &channel->regs->w.cr);
+		iowrite8(CR_CMD_NEGATE_RTSN, &channel->regs->w.cr);
+		iowrite8(CR_ENABLE_RX, &channel->regs->w.cr);
+		*channel->board_write = 1;
+		wake_up_interruptible(&channel->queue);
 	}
+
+	/* RX data */
+	if ((isr & channel->isr_rx_rdy_mask) && (sr & SR_RX_READY))
+		ipoctal_irq_rx(channel, tty, sr);
+
+	/* TX of each character */
+	if ((isr & channel->isr_tx_rdy_mask) && (sr & SR_TX_READY))
+		ipoctal_irq_tx(channel);
+
+	tty_flip_buffer_push(tty);
+	tty_kref_put(tty);
+}
+
+static irqreturn_t ipoctal_irq_handler(void *arg)
+{
+	unsigned int i;
+	struct ipoctal *ipoctal = (struct ipoctal *) arg;
+
+	/* Check all channels */
+	for (i = 0; i < NR_CHANNELS; i++)
+		ipoctal_irq_channel(&ipoctal->channel[i]);
+
+	/* Clear the IPack device interrupt */
+	readw(ipoctal->dev->int_space.address + ACK_INT_REQ0);
+	readw(ipoctal->dev->int_space.address + ACK_INT_REQ1);
+
 	return IRQ_HANDLED;
 }
 
@@ -331,18 +263,15 @@ static int ipoctal_check_model(struct ipack_device *dev, unsigned char *id)
 	unsigned char manufacturerID;
 	unsigned char board_id;
 
-	dev->bus->ops->read8(dev, IPACK_ID_SPACE,
-			IPACK_IDPROM_OFFSET_MANUFACTURER_ID, &manufacturerID);
-	if (manufacturerID != IP_OCTAL_MANUFACTURER_ID)
-		return -ENODEV;
-
-	dev->bus->ops->read8(dev, IPACK_ID_SPACE,
-			IPACK_IDPROM_OFFSET_MODEL, (unsigned char *)&board_id);
 
+	manufacturerID = ioread8(dev->id_space.address + IPACK_IDPROM_OFFSET_MANUFACTURER_ID);
+	if (manufacturerID != IPACK1_VENDOR_ID_SBS)
+		return -ENODEV;
+	board_id = ioread8(dev->id_space.address + IPACK_IDPROM_OFFSET_MODEL);
 	switch (board_id) {
-	case IP_OCTAL_232_ID:
-	case IP_OCTAL_422_ID:
-	case IP_OCTAL_485_ID:
+	case IPACK1_DEVICE_ID_SBS_OCTAL_232:
+	case IPACK1_DEVICE_ID_SBS_OCTAL_422:
+	case IPACK1_DEVICE_ID_SBS_OCTAL_485:
 		*id = board_id;
 		break;
 	default:
@@ -358,13 +287,16 @@ static const struct tty_port_operations ipoctal_tty_port_ops = {
 };
 
 static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
-			     unsigned int slot, unsigned int vector)
+			     unsigned int slot)
 {
 	int res = 0;
 	int i;
 	struct tty_driver *tty;
 	char name[20];
 	unsigned char board_id;
+	struct ipoctal_channel *channel;
+	union scc2698_channel __iomem *chan_regs;
+	union scc2698_block __iomem *block_regs;
 
 	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
 						IPACK_ID_SPACE);
@@ -392,54 +324,61 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 		goto out_unregister_id_space;
 	}
 
+	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
+						IPACK_INT_SPACE);
+	if (res) {
+		dev_err(&ipoctal->dev->dev,
+			"Unable to map slot [%d:%d] INT space!\n",
+			bus_nr, slot);
+		goto out_unregister_io_space;
+	}
+
 	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev,
 					   0x8000, IPACK_MEM_SPACE);
 	if (res) {
 		dev_err(&ipoctal->dev->dev,
 			"Unable to map slot [%d:%d] MEM space!\n",
 			bus_nr, slot);
-		goto out_unregister_io_space;
+		goto out_unregister_int_space;
 	}
 
 	/* Save the virtual address to access the registers easily */
-	ipoctal->chan_regs =
-		(struct scc2698_channel *) ipoctal->dev->io_space.address;
-	ipoctal->block_regs =
-		(struct scc2698_block *) ipoctal->dev->io_space.address;
+	chan_regs =
+		(union scc2698_channel __iomem *) ipoctal->dev->io_space.address;
+	block_regs =
+		(union scc2698_block __iomem *) ipoctal->dev->io_space.address;
 
 	/* Disable RX and TX before touching anything */
 	for (i = 0; i < NR_CHANNELS ; i++) {
-		ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
-				     CR_DISABLE_RX | CR_DISABLE_TX);
-		ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
-				     CR_CMD_RESET_RX);
-		ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
-				     CR_CMD_RESET_TX);
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->chan_regs[i].u.w.mr,
-				     MR1_CHRL_8_BITS | MR1_ERROR_CHAR |
-				     MR1_RxINT_RxRDY); /* mr1 */
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->chan_regs[i].u.w.mr,
-				     0); /* mr2 */
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->chan_regs[i].u.w.csr,
-				     TX_CLK_9600  | RX_CLK_9600);
+		struct ipoctal_channel *channel = &ipoctal->channel[i];
+		channel->regs = chan_regs + i;
+		channel->block_regs = block_regs + (i >> 1);
+		channel->board_write = &ipoctal->write;
+		channel->board_id = ipoctal->board_id;
+		if (i & 1) {
+			channel->isr_tx_rdy_mask = ISR_TxRDY_B;
+			channel->isr_rx_rdy_mask = ISR_RxRDY_FFULL_B;
+		} else {
+			channel->isr_tx_rdy_mask = ISR_TxRDY_A;
+			channel->isr_rx_rdy_mask = ISR_RxRDY_FFULL_A;
+		}
+
+		iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
+		iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
+		iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
+		iowrite8(MR1_CHRL_8_BITS | MR1_ERROR_CHAR | MR1_RxINT_RxRDY,
+			 &channel->regs->w.mr); /* mr1 */
+		iowrite8(0, &channel->regs->w.mr); /* mr2 */
+		iowrite8(TX_CLK_9600  | RX_CLK_9600, &channel->regs->w.csr);
 	}
 
 	for (i = 0; i < IP_OCTAL_NB_BLOCKS; i++) {
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->block_regs[i].u.w.acr,
-				     ACR_BRG_SET2);
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->block_regs[i].u.w.opcr,
-				     OPCR_MPP_OUTPUT | OPCR_MPOa_RTSN |
-				     OPCR_MPOb_RTSN);
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->block_regs[i].u.w.imr,
-				     IMR_TxRDY_A | IMR_RxRDY_FFULL_A |
-				     IMR_DELTA_BREAK_A | IMR_TxRDY_B |
-				     IMR_RxRDY_FFULL_B | IMR_DELTA_BREAK_B);
+		iowrite8(ACR_BRG_SET2, &block_regs[i].w.acr);
+		iowrite8(OPCR_MPP_OUTPUT | OPCR_MPOa_RTSN | OPCR_MPOb_RTSN,
+			 &block_regs[i].w.opcr);
+		iowrite8(IMR_TxRDY_A | IMR_RxRDY_FFULL_A | IMR_DELTA_BREAK_A |
+			 IMR_TxRDY_B | IMR_RxRDY_FFULL_B | IMR_DELTA_BREAK_B,
+			 &block_regs[i].w.imr);
 	}
 
 	/*
@@ -447,10 +386,10 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	 * Depending of the carrier these addresses are accesible or not.
 	 * More info in the datasheet.
 	 */
-	ipoctal->dev->bus->ops->request_irq(ipoctal->dev, vector,
+	ipoctal->dev->bus->ops->request_irq(ipoctal->dev,
 				       ipoctal_irq_handler, ipoctal);
-	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_MEM_SPACE, 0,
-				       vector);
+	/* Dummy write */
+	iowrite8(1, ipoctal->dev->mem_space.address + 1);
 
 	/* Register the TTY device */
 
@@ -464,8 +403,8 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 
 	/* Fill struct tty_driver with ipoctal data */
 	tty->owner = THIS_MODULE;
-	tty->driver_name = "ipoctal";
-	sprintf(name, "ipoctal.%d.%d.", bus_nr, slot);
+	tty->driver_name = KBUILD_MODNAME;
+	sprintf(name, KBUILD_MODNAME ".%d.%d.", bus_nr, slot);
 	tty->name = name;
 	tty->major = 0;
 
@@ -490,32 +429,40 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	ipoctal->tty_drv = tty;
 
 	for (i = 0; i < NR_CHANNELS; i++) {
-		tty_port_init(&ipoctal->tty_port[i]);
-		tty_port_alloc_xmit_buf(&ipoctal->tty_port[i]);
-		ipoctal->tty_port[i].ops = &ipoctal_tty_port_ops;
-
-		ipoctal_reset_stats(&ipoctal->chan_stats[i]);
-		ipoctal->nb_bytes[i] = 0;
-		init_waitqueue_head(&ipoctal->queue[i]);
-
-		spin_lock_init(&ipoctal->lock[i]);
-		ipoctal->pointer_read[i] = 0;
-		ipoctal->pointer_write[i] = 0;
-		ipoctal->nb_bytes[i] = 0;
-		tty_register_device(tty, i, NULL);
+		struct device *tty_dev;
+
+		channel = &ipoctal->channel[i];
+		tty_port_init(&channel->tty_port);
+		tty_port_alloc_xmit_buf(&channel->tty_port);
+		channel->tty_port.ops = &ipoctal_tty_port_ops;
+
+		ipoctal_reset_stats(&channel->stats);
+		channel->nb_bytes = 0;
+		init_waitqueue_head(&channel->queue);
+
+		spin_lock_init(&channel->lock);
+		channel->pointer_read = 0;
+		channel->pointer_write = 0;
+		tty_dev = tty_port_register_device(&channel->tty_port, tty, i, NULL);
+		if (IS_ERR(tty_dev)) {
+			dev_err(&ipoctal->dev->dev, "Failed to register tty device.\n");
+			continue;
+		}
+		dev_set_drvdata(tty_dev, channel);
 
 		/*
 		 * Enable again the RX. TX will be enabled when
 		 * there is something to send
 		 */
-		ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
-				     CR_ENABLE_RX);
+		iowrite8(CR_ENABLE_RX, &channel->regs->w.cr);
 	}
 
 	return 0;
 
 out_unregister_slot_unmap:
 	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_ID_SPACE);
+out_unregister_int_space:
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_INT_SPACE);
 out_unregister_io_space:
 	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_IO_SPACE);
 out_unregister_id_space:
@@ -523,23 +470,22 @@ out_unregister_id_space:
 	return res;
 }
 
-static inline int ipoctal_copy_write_buffer(struct ipoctal *ipoctal,
-					    unsigned int channel,
+static inline int ipoctal_copy_write_buffer(struct ipoctal_channel *channel,
 					    const unsigned char *buf,
 					    int count)
 {
 	unsigned long flags;
 	int i;
-	unsigned int *pointer_read = &ipoctal->pointer_read[channel];
+	unsigned int *pointer_read = &channel->pointer_read;
 
 	/* Copy the bytes from the user buffer to the internal one */
 	for (i = 0; i < count; i++) {
-		if (i <= (PAGE_SIZE - ipoctal->nb_bytes[channel])) {
-			spin_lock_irqsave(&ipoctal->lock[channel], flags);
-			ipoctal->tty_port[channel].xmit_buf[*pointer_read] = buf[i];
+		if (i <= (PAGE_SIZE - channel->nb_bytes)) {
+			spin_lock_irqsave(&channel->lock, flags);
+			channel->tty_port.xmit_buf[*pointer_read] = buf[i];
 			*pointer_read = (*pointer_read + 1) % PAGE_SIZE;
-			ipoctal->nb_bytes[channel]++;
-			spin_unlock_irqrestore(&ipoctal->lock[channel], flags);
+			channel->nb_bytes++;
+			spin_unlock_irqrestore(&channel->lock, flags);
 		} else {
 			break;
 		}
@@ -547,63 +493,44 @@ static inline int ipoctal_copy_write_buffer(struct ipoctal *ipoctal,
 	return i;
 }
 
-static int ipoctal_write(struct ipoctal *ipoctal, unsigned int channel,
-			 const unsigned char *buf, int count)
+static int ipoctal_write_tty(struct tty_struct *tty,
+			     const unsigned char *buf, int count)
 {
-	ipoctal->nb_bytes[channel] = 0;
-	ipoctal->count_wr[channel] = 0;
+	struct ipoctal_channel *channel = tty->driver_data;
+	unsigned int char_copied;
 
-	ipoctal_copy_write_buffer(ipoctal, channel, buf, count);
+	char_copied = ipoctal_copy_write_buffer(channel, buf, count);
 
 	/* As the IP-OCTAL 485 only supports half duplex, do it manually */
-	if (ipoctal->board_id == IP_OCTAL_485_ID) {
-		ipoctal_write_io_reg(ipoctal,
-				     &ipoctal->chan_regs[channel].u.w.cr,
-				     CR_DISABLE_RX);
-		ipoctal_write_cr_cmd(ipoctal,
-				     &ipoctal->chan_regs[channel].u.w.cr,
-				     CR_CMD_ASSERT_RTSN);
+	if (channel->board_id == IPACK1_DEVICE_ID_SBS_OCTAL_485) {
+		iowrite8(CR_DISABLE_RX, &channel->regs->w.cr);
+		iowrite8(CR_CMD_ASSERT_RTSN, &channel->regs->w.cr);
 	}
 
 	/*
 	 * Send a packet and then disable TX to avoid failure after several send
 	 * operations
 	 */
-	ipoctal_write_io_reg(ipoctal,
-			     &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_ENABLE_TX);
-	wait_event_interruptible(ipoctal->queue[channel], ipoctal->write);
-	ipoctal_write_io_reg(ipoctal,
-			     &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_DISABLE_TX);
-
-	ipoctal->write = 0;
-	return ipoctal->count_wr[channel];
-}
-
-static int ipoctal_write_tty(struct tty_struct *tty,
-			     const unsigned char *buf, int count)
-{
-	unsigned int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	iowrite8(CR_ENABLE_TX, &channel->regs->w.cr);
+	wait_event_interruptible(channel->queue, *channel->board_write);
+	iowrite8(CR_DISABLE_TX, &channel->regs->w.cr);
 
-	return ipoctal_write(ipoctal, channel, buf, count);
+	*channel->board_write = 0;
+	return char_copied;
 }
 
 static int ipoctal_write_room(struct tty_struct *tty)
 {
-	int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	struct ipoctal_channel *channel = tty->driver_data;
 
-	return PAGE_SIZE - ipoctal->nb_bytes[channel];
+	return PAGE_SIZE - channel->nb_bytes;
 }
 
 static int ipoctal_chars_in_buffer(struct tty_struct *tty)
 {
-	int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	struct ipoctal_channel *channel = tty->driver_data;
 
-	return ipoctal->nb_bytes[channel];
+	return channel->nb_bytes;
 }
 
 static void ipoctal_set_termios(struct tty_struct *tty,
@@ -613,23 +540,17 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	unsigned char mr1 = 0;
 	unsigned char mr2 = 0;
 	unsigned char csr = 0;
-	unsigned int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	struct ipoctal_channel *channel = tty->driver_data;
 	speed_t baud;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	/* Disable and reset everything before change the setup */
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_DISABLE_RX | CR_DISABLE_TX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_RX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_TX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_ERR_STATUS);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_MR);
+	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
 
 	/* Set Bits per chars */
 	switch (cflag & CSIZE) {
@@ -643,7 +564,7 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	default:
 		mr1 |= MR1_CHRL_8_BITS;
 		/* By default, select CS8 */
-		tty->termios->c_cflag = (cflag & ~CSIZE) | CS8;
+		tty->termios.c_cflag = (cflag & ~CSIZE) | CS8;
 		break;
 	}
 
@@ -657,7 +578,7 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 		mr1 |= MR1_PARITY_OFF;
 
 	/* Mark or space parity is not supported */
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	/* Set stop bits */
 	if (cflag & CSTOPB)
@@ -666,8 +587,8 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 		mr2 |= MR2_STOP_BITS_LENGTH_1;
 
 	/* Set the flow control */
-	switch (ipoctal->board_id) {
-	case IP_OCTAL_232_ID:
+	switch (channel->board_id) {
+	case IPACK1_DEVICE_ID_SBS_OCTAL_232:
 		if (cflag & CRTSCTS) {
 			mr1 |= MR1_RxRTS_CONTROL_ON;
 			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_ON;
@@ -676,11 +597,11 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
 		}
 		break;
-	case IP_OCTAL_422_ID:
+	case IPACK1_DEVICE_ID_SBS_OCTAL_422:
 		mr1 |= MR1_RxRTS_CONTROL_OFF;
 		mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
 		break;
-	case IP_OCTAL_485_ID:
+	case IPACK1_DEVICE_ID_SBS_OCTAL_485:
 		mr1 |= MR1_RxRTS_CONTROL_OFF;
 		mr2 |= MR2_TxRTS_CONTROL_ON | MR2_CTS_ENABLE_TX_OFF;
 		break;
@@ -690,10 +611,10 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	}
 
 	baud = tty_get_baud_rate(tty);
-	tty_termios_encode_baud_rate(tty->termios, baud, baud);
+	tty_termios_encode_baud_rate(&tty->termios, baud, baud);
 
 	/* Set baud rate */
-	switch (tty->termios->c_ospeed) {
+	switch (baud) {
 	case 75:
 		csr |= TX_CLK_75 | RX_CLK_75;
 		break;
@@ -734,7 +655,7 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	default:
 		csr |= TX_CLK_38400 | RX_CLK_38400;
 		/* In case of default, we establish 38400 bps */
-		tty_termios_encode_baud_rate(tty->termios, 38400, 38400);
+		tty_termios_encode_baud_rate(&tty->termios, 38400, 38400);
 		break;
 	}
 
@@ -742,45 +663,38 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	mr1 |= MR1_RxINT_RxRDY;
 
 	/* Write the control registers */
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.mr, mr1);
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.mr, mr2);
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.csr, csr);
+	iowrite8(mr1, &channel->regs->w.mr);
+	iowrite8(mr2, &channel->regs->w.mr);
+	iowrite8(csr, &channel->regs->w.csr);
 
 	/* Enable again the RX */
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_ENABLE_RX);
+	iowrite8(CR_ENABLE_RX, &channel->regs->w.cr);
 }
 
 static void ipoctal_hangup(struct tty_struct *tty)
 {
 	unsigned long flags;
-	int channel = tty->index;
-	struct ipoctal *ipoctal = tty->driver_data;
+	struct ipoctal_channel *channel = tty->driver_data;
 
-	if (ipoctal == NULL)
+	if (channel == NULL)
 		return;
 
-	spin_lock_irqsave(&ipoctal->lock[channel], flags);
-	ipoctal->nb_bytes[channel] = 0;
-	ipoctal->pointer_read[channel] = 0;
-	ipoctal->pointer_write[channel] = 0;
-	spin_unlock_irqrestore(&ipoctal->lock[channel], flags);
-
-	tty_port_hangup(&ipoctal->tty_port[channel]);
-
-	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_DISABLE_RX | CR_DISABLE_TX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_RX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_TX);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_ERR_STATUS);
-	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
-			     CR_CMD_RESET_MR);
-
-	clear_bit(ASYNCB_INITIALIZED, &ipoctal->tty_port[channel].flags);
-	wake_up_interruptible(&ipoctal->tty_port[channel].open_wait);
+	spin_lock_irqsave(&channel->lock, flags);
+	channel->nb_bytes = 0;
+	channel->pointer_read = 0;
+	channel->pointer_write = 0;
+	spin_unlock_irqrestore(&channel->lock, flags);
+
+	tty_port_hangup(&channel->tty_port);
+
+	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
+
+	clear_bit(ASYNCB_INITIALIZED, &channel->tty_port.flags);
+	wake_up_interruptible(&channel->tty_port.open_wait);
 }
 
 static const struct tty_operations ipoctal_fops = {
@@ -795,27 +709,6 @@ static const struct tty_operations ipoctal_fops = {
 	.hangup =		ipoctal_hangup,
 };
 
-static int ipoctal_match(struct ipack_device *dev)
-{
-	int res;
-	unsigned char board_id;
-
-	if ((!dev->bus->ops) || (!dev->bus->ops->map_space) ||
-	    (!dev->bus->ops->unmap_space))
-		return 0;
-
-	res = dev->bus->ops->map_space(dev, 0, IPACK_ID_SPACE);
-	if (res)
-		return 0;
-
-	res = ipoctal_check_model(dev, &board_id);
-	dev->bus->ops->unmap_space(dev, IPACK_ID_SPACE);
-	if (!res)
-		return 1;
-
-	return 0;
-}
-
 static int ipoctal_probe(struct ipack_device *dev)
 {
 	int res;
@@ -826,11 +719,11 @@ static int ipoctal_probe(struct ipack_device *dev)
 		return -ENOMEM;
 
 	ipoctal->dev = dev;
-	res = ipoctal_inst_slot(ipoctal, dev->bus_nr, dev->slot, dev->irq);
+	res = ipoctal_inst_slot(ipoctal, dev->bus_nr, dev->slot);
 	if (res)
 		goto out_uninst;
 
-	list_add_tail(&ipoctal->list, &ipoctal_list);
+	dev_set_drvdata(&dev->dev, ipoctal);
 	return 0;
 
 out_uninst:
@@ -842,46 +735,57 @@ static void __ipoctal_remove(struct ipoctal *ipoctal)
 {
 	int i;
 
+	ipoctal->dev->bus->ops->free_irq(ipoctal->dev);
+
 	for (i = 0; i < NR_CHANNELS; i++) {
+		struct ipoctal_channel *channel = &ipoctal->channel[i];
 		tty_unregister_device(ipoctal->tty_drv, i);
-		tty_port_free_xmit_buf(&ipoctal->tty_port[i]);
+		tty_port_free_xmit_buf(&channel->tty_port);
 	}
 
 	tty_unregister_driver(ipoctal->tty_drv);
 	put_tty_driver(ipoctal->tty_drv);
-	list_del(&ipoctal->list);
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_MEM_SPACE);
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_INT_SPACE);
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_IO_SPACE);
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_ID_SPACE);
 	kfree(ipoctal);
 }
 
-static void ipoctal_remove(struct ipack_device *device)
+static void ipoctal_remove(struct ipack_device *idev)
 {
-	struct ipoctal *ipoctal, *next;
-
-	list_for_each_entry_safe(ipoctal, next, &ipoctal_list, list) {
-		if (ipoctal->dev == device)
-			__ipoctal_remove(ipoctal);
-	}
+	__ipoctal_remove(dev_get_drvdata(&idev->dev));
 }
 
-static struct ipack_driver_ops ipoctal_drv_ops = {
-	.match = ipoctal_match,
-	.probe = ipoctal_probe,
+static DEFINE_IPACK_DEVICE_TABLE(ipoctal_ids) = {
+	{ IPACK_DEVICE(IPACK_ID_VERSION_1, IPACK1_VENDOR_ID_SBS,
+			IPACK1_DEVICE_ID_SBS_OCTAL_232) },
+	{ IPACK_DEVICE(IPACK_ID_VERSION_1, IPACK1_VENDOR_ID_SBS,
+			IPACK1_DEVICE_ID_SBS_OCTAL_422) },
+	{ IPACK_DEVICE(IPACK_ID_VERSION_1, IPACK1_VENDOR_ID_SBS,
+			IPACK1_DEVICE_ID_SBS_OCTAL_485) },
+	{ 0, },
+};
+
+MODULE_DEVICE_TABLE(ipack, ipoctal_ids);
+
+static const struct ipack_driver_ops ipoctal_drv_ops = {
+	.probe  = ipoctal_probe,
 	.remove = ipoctal_remove,
 };
 
+static struct ipack_driver driver = {
+	.ops      = &ipoctal_drv_ops,
+	.id_table = ipoctal_ids,
+};
+
 static int __init ipoctal_init(void)
 {
-	driver.ops = &ipoctal_drv_ops;
 	return ipack_driver_register(&driver, THIS_MODULE, KBUILD_MODNAME);
 }
 
 static void __exit ipoctal_exit(void)
 {
-	struct ipoctal *p, *next;
-
-	list_for_each_entry_safe(p, next, &ipoctal_list, list)
-		p->dev->bus->ops->remove_device(p->dev);
-
 	ipack_driver_unregister(&driver);
 }
 
diff --git a/drivers/staging/ipack/devices/scc2698.h b/drivers/staging/ipack/devices/scc2698.h
index 47f6269985fd..96e8d8c30e14 100644
--- a/drivers/staging/ipack/devices/scc2698.h
+++ b/drivers/staging/ipack/devices/scc2698.h
@@ -15,78 +15,74 @@
 #define SCC2698_H_
 
 /*
- * struct scc2698_channel - Channel access to scc2698 IO
+ * union scc2698_channel - Channel access to scc2698 IO
  *
  * dn value are only spacer.
  *
  */
-struct scc2698_channel {
-	union {
-		struct {
-			unsigned char d0, mr;  /* Mode register 1/2*/
-			unsigned char d1, sr;  /* Status register */
-			unsigned char d2, r1;  /* reserved */
-			unsigned char d3, rhr; /* Receive holding register (R) */
-			unsigned char junk[8]; /* other crap for block control */
-		} r; /* Read access */
-		struct {
-			unsigned char d0, mr;  /* Mode register 1/2 */
-			unsigned char d1, csr; /* Clock select register */
-			unsigned char d2, cr;  /* Command register */
-			unsigned char d3, thr; /* Transmit holding register */
-			unsigned char junk[8]; /* other crap for block control */
-		} w; /* Write access */
-	} u;
+union scc2698_channel {
+	struct {
+		u8 d0, mr;  /* Mode register 1/2*/
+		u8 d1, sr;  /* Status register */
+		u8 d2, r1;  /* reserved */
+		u8 d3, rhr; /* Receive holding register (R) */
+		u8 junk[8]; /* other crap for block control */
+	} __packed r; /* Read access */
+	struct {
+		u8 d0, mr;  /* Mode register 1/2 */
+		u8 d1, csr; /* Clock select register */
+		u8 d2, cr;  /* Command register */
+		u8 d3, thr; /* Transmit holding register */
+		u8 junk[8]; /* other crap for block control */
+	} __packed w; /* Write access */
 };
 
 /*
- * struct scc2698_block - Block access to scc2698 IO
+ * union scc2698_block - Block access to scc2698 IO
  *
  * The scc2698 contain 4 block.
  * Each block containt two channel a and b.
  * dn value are only spacer.
  *
  */
-struct scc2698_block {
-	union {
-		struct {
-			unsigned char d0, mra;  /* Mode register 1/2 (a) */
-			unsigned char d1, sra;  /* Status register (a) */
-			unsigned char d2, r1;   /* reserved */
-			unsigned char d3, rhra; /* Receive holding register (a) */
-			unsigned char d4, ipcr; /* Input port change register of block */
-			unsigned char d5, isr;  /* Interrupt status register of block */
-			unsigned char d6, ctur; /* Counter timer upper register of block */
-			unsigned char d7, ctlr; /* Counter timer lower register of block */
-			unsigned char d8, mrb;  /* Mode register 1/2 (b) */
-			unsigned char d9, srb;  /* Status register (b) */
-			unsigned char da, r2;   /* reserved */
-			unsigned char db, rhrb; /* Receive holding register (b) */
-			unsigned char dc, r3;   /* reserved */
-			unsigned char dd, ip;   /* Input port register of block */
-			unsigned char de, ctg;  /* Start counter timer of block */
-			unsigned char df, cts;  /* Stop counter timer of block */
-		} r; /* Read access */
-		struct {
-			unsigned char d0, mra;  /* Mode register 1/2 (a) */
-			unsigned char d1, csra; /* Clock select register (a) */
-			unsigned char d2, cra;  /* Command register (a) */
-			unsigned char d3, thra; /* Transmit holding register (a) */
-			unsigned char d4, acr;  /* Auxiliary control register of block */
-			unsigned char d5, imr;  /* Interrupt mask register of block  */
-			unsigned char d6, ctu;  /* Counter timer upper register of block */
-			unsigned char d7, ctl;  /* Counter timer lower register of block */
-			unsigned char d8, mrb;  /* Mode register 1/2 (b) */
-			unsigned char d9, csrb; /* Clock select register (a) */
-			unsigned char da, crb;  /* Command register (b) */
-			unsigned char db, thrb; /* Transmit holding register (b) */
-			unsigned char dc, r1;   /* reserved */
-			unsigned char dd, opcr; /* Output port configuration register of block */
-			unsigned char de, r2;   /* reserved */
-			unsigned char df, r3;   /* reserved */
-		} w; /* Write access */
-	} u;
-} ;
+union scc2698_block {
+	struct {
+		u8 d0, mra;  /* Mode register 1/2 (a) */
+		u8 d1, sra;  /* Status register (a) */
+		u8 d2, r1;   /* reserved */
+		u8 d3, rhra; /* Receive holding register (a) */
+		u8 d4, ipcr; /* Input port change register of block */
+		u8 d5, isr;  /* Interrupt status register of block */
+		u8 d6, ctur; /* Counter timer upper register of block */
+		u8 d7, ctlr; /* Counter timer lower register of block */
+		u8 d8, mrb;  /* Mode register 1/2 (b) */
+		u8 d9, srb;  /* Status register (b) */
+		u8 da, r2;   /* reserved */
+		u8 db, rhrb; /* Receive holding register (b) */
+		u8 dc, r3;   /* reserved */
+		u8 dd, ip;   /* Input port register of block */
+		u8 de, ctg;  /* Start counter timer of block */
+		u8 df, cts;  /* Stop counter timer of block */
+	} __packed r; /* Read access */
+	struct {
+		u8 d0, mra;  /* Mode register 1/2 (a) */
+		u8 d1, csra; /* Clock select register (a) */
+		u8 d2, cra;  /* Command register (a) */
+		u8 d3, thra; /* Transmit holding register (a) */
+		u8 d4, acr;  /* Auxiliary control register of block */
+		u8 d5, imr;  /* Interrupt mask register of block  */
+		u8 d6, ctu;  /* Counter timer upper register of block */
+		u8 d7, ctl;  /* Counter timer lower register of block */
+		u8 d8, mrb;  /* Mode register 1/2 (b) */
+		u8 d9, csrb; /* Clock select register (a) */
+		u8 da, crb;  /* Command register (b) */
+		u8 db, thrb; /* Transmit holding register (b) */
+		u8 dc, r1;   /* reserved */
+		u8 dd, opcr; /* Output port configuration register of block */
+		u8 de, r2;   /* reserved */
+		u8 df, r3;   /* reserved */
+	} __packed w; /* Write access */
+};
 
 #define MR1_CHRL_5_BITS             (0x0 << 0)
 #define MR1_CHRL_6_BITS             (0x1 << 0)
@@ -225,4 +221,7 @@ struct scc2698_block {
 #define ISR_DELTA_BREAK_B           (0x1 << 6)
 #define ISR_INPUT_PORT_CHANGE       (0x1 << 7)
 
+#define ACK_INT_REQ0			0
+#define ACK_INT_REQ1			2
+
 #endif /* SCC2698_H_ */
diff --git a/drivers/staging/ipack/ipack.c b/drivers/staging/ipack/ipack.c
index c1cd97a4d9ce..d1e0651592a2 100644
--- a/drivers/staging/ipack/ipack.c
+++ b/drivers/staging/ipack/ipack.c
@@ -12,6 +12,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/idr.h>
+#include <asm/io.h>
 #include "ipack.h"
 
 #define to_ipack_dev(device) container_of(device, struct ipack_device, dev)
@@ -22,55 +23,190 @@ static DEFINE_IDA(ipack_ida);
 static void ipack_device_release(struct device *dev)
 {
 	struct ipack_device *device = to_ipack_dev(dev);
+	kfree(device->id);
 	kfree(device);
 }
 
-static int ipack_bus_match(struct device *device, struct device_driver *driver)
+static inline const struct ipack_device_id *
+ipack_match_one_device(const struct ipack_device_id *id,
+		       const struct ipack_device *device)
 {
-	int ret;
-	struct ipack_device *dev = to_ipack_dev(device);
-	struct ipack_driver *drv = to_ipack_driver(driver);
+	if ((id->format == IPACK_ANY_FORMAT ||
+				id->format == device->id_format) &&
+	    (id->vendor == IPACK_ANY_ID || id->vendor == device->id_vendor) &&
+	    (id->device == IPACK_ANY_ID || id->device == device->id_device))
+		return id;
+	return NULL;
+}
 
-	if ((!drv->ops) || (!drv->ops->match))
-		return -EINVAL;
+static const struct ipack_device_id *
+ipack_match_id(const struct ipack_device_id *ids, struct ipack_device *idev)
+{
+	if (ids) {
+		while (ids->vendor || ids->device) {
+			if (ipack_match_one_device(ids, idev))
+				return ids;
+			ids++;
+		}
+	}
+	return NULL;
+}
 
-	ret = drv->ops->match(dev);
-	if (ret)
-		dev->driver = drv;
+static int ipack_bus_match(struct device *dev, struct device_driver *drv)
+{
+	struct ipack_device *idev = to_ipack_dev(dev);
+	struct ipack_driver *idrv = to_ipack_driver(drv);
+	const struct ipack_device_id *found_id;
 
-	return ret;
+	found_id = ipack_match_id(idrv->id_table, idev);
+	return found_id ? 1 : 0;
 }
 
 static int ipack_bus_probe(struct device *device)
 {
 	struct ipack_device *dev = to_ipack_dev(device);
+	struct ipack_driver *drv = to_ipack_driver(device->driver);
 
-	if (!dev->driver->ops->probe)
+	if (!drv->ops->probe)
 		return -EINVAL;
 
-	return dev->driver->ops->probe(dev);
+	return drv->ops->probe(dev);
 }
 
 static int ipack_bus_remove(struct device *device)
 {
 	struct ipack_device *dev = to_ipack_dev(device);
+	struct ipack_driver *drv = to_ipack_driver(device->driver);
 
-	if (!dev->driver->ops->remove)
+	if (!drv->ops->remove)
 		return -EINVAL;
 
-	dev->driver->ops->remove(dev);
+	drv->ops->remove(dev);
+	return 0;
+}
+
+#ifdef CONFIG_HOTPLUG
+
+static int ipack_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	struct ipack_device *idev;
+
+	if (!dev)
+		return -ENODEV;
+
+	idev = to_ipack_dev(dev);
+
+	if (add_uevent_var(env,
+			   "MODALIAS=ipack:f%02Xv%08Xd%08X", idev->id_format,
+			   idev->id_vendor, idev->id_device))
+		return -ENOMEM;
+
 	return 0;
 }
 
+#else /* !CONFIG_HOTPLUG */
+
+#define ipack_uevent NULL
+
+#endif /* !CONFIG_HOTPLUG */
+
+#define ipack_device_attr(field, format_string)				\
+static ssize_t								\
+field##_show(struct device *dev, struct device_attribute *attr,		\
+		char *buf)						\
+{									\
+	struct ipack_device *idev = to_ipack_dev(dev);			\
+	return sprintf(buf, format_string, idev->field);		\
+}
+
+static ssize_t id_show(struct device *dev,
+		       struct device_attribute *attr, char *buf)
+{
+	unsigned int i, c, l, s;
+	struct ipack_device *idev = to_ipack_dev(dev);
+
+
+	switch (idev->id_format) {
+	case IPACK_ID_VERSION_1:
+		l = 0x7; s = 1; break;
+	case IPACK_ID_VERSION_2:
+		l = 0xf; s = 2; break;
+	default:
+		return -EIO;
+	}
+	c = 0;
+	for (i = 0; i < idev->id_avail; i++) {
+		if (i > 0) {
+			if ((i & l) == 0)
+				buf[c++] = '\n';
+			else if ((i & s) == 0)
+				buf[c++] = ' ';
+		}
+		sprintf(&buf[c], "%02x", idev->id[i]);
+		c += 2;
+	}
+	buf[c++] = '\n';
+	return c;
+}
+
+static ssize_t
+id_vendor_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct ipack_device *idev = to_ipack_dev(dev);
+	switch (idev->id_format) {
+	case IPACK_ID_VERSION_1:
+		return sprintf(buf, "0x%02x\n", idev->id_vendor);
+	case IPACK_ID_VERSION_2:
+		return sprintf(buf, "0x%06x\n", idev->id_vendor);
+	default:
+		return -EIO;
+	}
+}
+
+static ssize_t
+id_device_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct ipack_device *idev = to_ipack_dev(dev);
+	switch (idev->id_format) {
+	case IPACK_ID_VERSION_1:
+		return sprintf(buf, "0x%02x\n", idev->id_device);
+	case IPACK_ID_VERSION_2:
+		return sprintf(buf, "0x%04x\n", idev->id_device);
+	default:
+		return -EIO;
+	}
+}
+
+static ssize_t modalias_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct ipack_device *idev = to_ipack_dev(dev);
+
+	return sprintf(buf, "ipac:f%02Xv%08Xd%08X", idev->id_format,
+		       idev->id_vendor, idev->id_device);
+}
+
+ipack_device_attr(id_format, "0x%hhu\n");
+
+static struct device_attribute ipack_dev_attrs[] = {
+	__ATTR_RO(id),
+	__ATTR_RO(id_device),
+	__ATTR_RO(id_format),
+	__ATTR_RO(id_vendor),
+	__ATTR_RO(modalias),
+};
+
 static struct bus_type ipack_bus_type = {
-	.name  = "ipack",
-	.probe = ipack_bus_probe,
-	.match = ipack_bus_match,
-	.remove = ipack_bus_remove,
+	.name      = "ipack",
+	.probe     = ipack_bus_probe,
+	.match     = ipack_bus_match,
+	.remove    = ipack_bus_remove,
+	.dev_attrs = ipack_dev_attrs,
+	.uevent	   = ipack_uevent,
 };
 
 struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
-					    struct ipack_bus_ops *ops)
+					    const struct ipack_bus_ops *ops)
 {
 	int bus_nr;
 	struct ipack_bus_device *bus;
@@ -93,8 +229,20 @@ struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
 }
 EXPORT_SYMBOL_GPL(ipack_bus_register);
 
+static int ipack_unregister_bus_member(struct device *dev, void *data)
+{
+	struct ipack_device *idev = to_ipack_dev(dev);
+	struct ipack_bus_device *bus = data;
+
+	if (idev->bus_nr == bus->bus_nr)
+		ipack_device_unregister(idev);
+
+	return 1;
+}
+
 int ipack_bus_unregister(struct ipack_bus_device *bus)
 {
+	bus_for_each_dev(&ipack_bus_type, NULL, bus, ipack_unregister_bus_member);
 	ida_simple_remove(&ipack_ida, bus->bus_nr);
 	kfree(bus);
 	return 0;
@@ -102,7 +250,7 @@ int ipack_bus_unregister(struct ipack_bus_device *bus)
 EXPORT_SYMBOL_GPL(ipack_bus_unregister);
 
 int ipack_driver_register(struct ipack_driver *edrv, struct module *owner,
-			  char *name)
+			  const char *name)
 {
 	edrv->driver.owner = owner;
 	edrv->driver.name = name;
@@ -117,8 +265,169 @@ void ipack_driver_unregister(struct ipack_driver *edrv)
 }
 EXPORT_SYMBOL_GPL(ipack_driver_unregister);
 
+static u16 ipack_crc_byte(u16 crc, u8 c)
+{
+	int i;
+
+	crc ^= c << 8;
+	for (i = 0; i < 8; i++)
+		crc = (crc << 1) ^ ((crc & 0x8000) ? 0x1021 : 0);
+	return crc;
+}
+
+/*
+ * The algorithm in lib/crc-ccitt.c does not seem to apply since it uses the
+ * opposite bit ordering.
+ */
+static u8 ipack_calc_crc1(struct ipack_device *dev)
+{
+	u8 c;
+	u16 crc;
+	unsigned int i;
+
+	crc = 0xffff;
+	for (i = 0; i < dev->id_avail; i++) {
+		c = (i != 11) ? dev->id[i] : 0;
+		crc = ipack_crc_byte(crc, c);
+	}
+	crc = ~crc;
+	return crc & 0xff;
+}
+
+static u16 ipack_calc_crc2(struct ipack_device *dev)
+{
+	u8 c;
+	u16 crc;
+	unsigned int i;
+
+	crc = 0xffff;
+	for (i = 0; i < dev->id_avail; i++) {
+		c = ((i != 0x18) && (i != 0x19)) ? dev->id[i] : 0;
+		crc = ipack_crc_byte(crc, c);
+	}
+	crc = ~crc;
+	return crc;
+}
+
+static void ipack_parse_id1(struct ipack_device *dev)
+{
+	u8 *id = dev->id;
+	u8 crc;
+
+	dev->id_vendor = id[4];
+	dev->id_device = id[5];
+	dev->speed_8mhz = 1;
+	dev->speed_32mhz = (id[7] == 'H');
+	crc = ipack_calc_crc1(dev);
+	dev->id_crc_correct = (crc == id[11]);
+	if (!dev->id_crc_correct) {
+		dev_warn(&dev->dev, "ID CRC invalid found 0x%x, expected 0x%x.\n",
+				id[11], crc);
+	}
+}
+
+static void ipack_parse_id2(struct ipack_device *dev)
+{
+	__be16 *id = (__be16 *) dev->id;
+	u16 flags, crc;
+
+	dev->id_vendor = ((be16_to_cpu(id[3]) & 0xff) << 16)
+			 + be16_to_cpu(id[4]);
+	dev->id_device = be16_to_cpu(id[5]);
+	flags = be16_to_cpu(id[10]);
+	dev->speed_8mhz = !!(flags & 2);
+	dev->speed_32mhz = !!(flags & 4);
+	crc = ipack_calc_crc2(dev);
+	dev->id_crc_correct = (crc == be16_to_cpu(id[12]));
+	if (!dev->id_crc_correct) {
+		dev_warn(&dev->dev, "ID CRC invalid found 0x%x, expected 0x%x.\n",
+				id[11], crc);
+	}
+}
+
+static int ipack_device_read_id(struct ipack_device *dev)
+{
+	u8 __iomem *idmem;
+	int i;
+	int ret = 0;
+
+	ret = dev->bus->ops->map_space(dev, 0, IPACK_ID_SPACE);
+	if (ret) {
+		dev_err(&dev->dev, "error mapping memory\n");
+		return ret;
+	}
+	idmem = dev->id_space.address;
+
+	/* Determine ID PROM Data Format.  If we find the ids "IPAC" or "IPAH"
+	 * we are dealing with a IndustryPack  format 1 device.  If we detect
+	 * "VITA4 " (16 bit big endian formatted) we are dealing with a
+	 * IndustryPack format 2 device */
+	if ((ioread8(idmem + 1) == 'I') &&
+			(ioread8(idmem + 3) == 'P') &&
+			(ioread8(idmem + 5) == 'A') &&
+			((ioread8(idmem + 7) == 'C') ||
+			 (ioread8(idmem + 7) == 'H'))) {
+		dev->id_format = IPACK_ID_VERSION_1;
+		dev->id_avail = ioread8(idmem + 0x15);
+		if ((dev->id_avail < 0x0c) || (dev->id_avail > 0x40)) {
+			dev_warn(&dev->dev, "invalid id size");
+			dev->id_avail = 0x0c;
+		}
+	} else if ((ioread8(idmem + 0) == 'I') &&
+			(ioread8(idmem + 1) == 'V') &&
+			(ioread8(idmem + 2) == 'A') &&
+			(ioread8(idmem + 3) == 'T') &&
+			(ioread8(idmem + 4) == ' ') &&
+			(ioread8(idmem + 5) == '4')) {
+		dev->id_format = IPACK_ID_VERSION_2;
+		dev->id_avail = ioread16be(idmem + 0x16);
+		if ((dev->id_avail < 0x1a) || (dev->id_avail > 0x40)) {
+			dev_warn(&dev->dev, "invalid id size");
+			dev->id_avail = 0x1a;
+		}
+	} else {
+		dev->id_format = IPACK_ID_VERSION_INVALID;
+		dev->id_avail = 0;
+	}
+
+	if (!dev->id_avail) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	/* Obtain the amount of memory required to store a copy of the complete
+	 * ID ROM contents */
+	dev->id = kmalloc(dev->id_avail, GFP_KERNEL);
+	if (!dev->id) {
+		dev_err(&dev->dev, "dev->id alloc failed.\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+	for (i = 0; i < dev->id_avail; i++) {
+		if (dev->id_format == IPACK_ID_VERSION_1)
+			dev->id[i] = ioread8(idmem + (i << 1) + 1);
+		else
+			dev->id[i] = ioread8(idmem + i);
+	}
+
+	/* now we can finally work with the copy */
+	switch (dev->id_format) {
+	case IPACK_ID_VERSION_1:
+		ipack_parse_id1(dev);
+		break;
+	case IPACK_ID_VERSION_2:
+		ipack_parse_id2(dev);
+		break;
+	}
+
+out:
+	dev->bus->ops->unmap_space(dev, IPACK_ID_SPACE);
+
+	return ret;
+}
+
 struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
-					   int slot, int irqv)
+					   int slot)
 {
 	int ret;
 	struct ipack_device *dev;
@@ -132,13 +441,32 @@ struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
 	dev->dev.parent = bus->parent;
 	dev->slot = slot;
 	dev->bus_nr = bus->bus_nr;
-	dev->irq = irqv;
 	dev->bus = bus;
 	dev_set_name(&dev->dev,
 		     "ipack-dev.%u.%u", dev->bus_nr, dev->slot);
 
+	if (bus->ops->set_clockrate(dev, 8))
+		dev_warn(&dev->dev, "failed to switch to 8 MHz operation for reading of device ID.\n");
+	if (bus->ops->reset_timeout(dev))
+		dev_warn(&dev->dev, "failed to reset potential timeout.");
+
+	ret = ipack_device_read_id(dev);
+	if (ret < 0) {
+		dev_err(&dev->dev, "error reading device id section.\n");
+		kfree(dev);
+		return NULL;
+	}
+
+	/* if the device supports 32 MHz operation, use it. */
+	if (dev->speed_32mhz) {
+		ret = bus->ops->set_clockrate(dev, 32);
+		if (ret < 0)
+			dev_err(&dev->dev, "failed to switch to 32 MHz operation.\n");
+	}
+
 	ret = device_register(&dev->dev);
 	if (ret < 0) {
+		kfree(dev->id);
 		kfree(dev);
 		return NULL;
 	}
diff --git a/drivers/staging/ipack/ipack.h b/drivers/staging/ipack/ipack.h
index 8bc001e3ca4e..d8e3bb6feac8 100644
--- a/drivers/staging/ipack/ipack.h
+++ b/drivers/staging/ipack/ipack.h
@@ -9,7 +9,11 @@
  * Software Foundation; version 2 of the License.
  */
 
+#include <linux/mod_devicetable.h>
 #include <linux/device.h>
+#include <linux/interrupt.h>
+
+#include "ipack_ids.h"
 
 #define IPACK_IDPROM_OFFSET_I			0x01
 #define IPACK_IDPROM_OFFSET_P			0x03
@@ -31,6 +35,7 @@ enum ipack_space {
 	IPACK_IO_SPACE    = 0,
 	IPACK_ID_SPACE    = 1,
 	IPACK_MEM_SPACE   = 2,
+	IPACK_INT_SPACE,
 };
 
 /**
@@ -49,8 +54,6 @@ struct ipack_addr_space {
  *
  *	@bus_nr: IP bus number where the device is plugged
  *	@slot: Slot where the device is plugged in the carrier board
- *	@irq: IRQ vector
- *	@driver: Pointer to the ipack_driver that manages the device
  *	@bus: ipack_bus_device where the device is plugged to.
  *	@id_space: Virtual address to ID space.
  *	@io_space: Virtual address to IO space.
@@ -64,25 +67,30 @@ struct ipack_addr_space {
 struct ipack_device {
 	unsigned int bus_nr;
 	unsigned int slot;
-	unsigned int irq;
-	struct ipack_driver *driver;
 	struct ipack_bus_device *bus;
 	struct ipack_addr_space id_space;
 	struct ipack_addr_space io_space;
+	struct ipack_addr_space int_space;
 	struct ipack_addr_space mem_space;
 	struct device dev;
+	u8                      *id;
+	size_t			 id_avail;
+	u32			 id_vendor;
+	u32			 id_device;
+	u8			 id_format;
+	unsigned int		 id_crc_correct:1;
+	unsigned int		 speed_8mhz:1;
+	unsigned int		 speed_32mhz:1;
 };
 
 /**
  *	struct ipack_driver_ops -- callbacks to mezzanine driver for installing/removing one device
  *
- *	@match: Match function
  *	@probe: Probe function
  *	@remove: tell the driver that the carrier board wants to remove one device
  */
 
 struct ipack_driver_ops {
-	int (*match) (struct ipack_device *dev);
 	int (*probe) (struct ipack_device *dev);
 	void (*remove) (struct ipack_device *dev);
 };
@@ -95,7 +103,8 @@ struct ipack_driver_ops {
  */
 struct ipack_driver {
 	struct device_driver driver;
-	struct ipack_driver_ops *ops;
+	const struct ipack_device_id *id_table;
+	const struct ipack_driver_ops *ops;
 };
 
 /**
@@ -105,26 +114,27 @@ struct ipack_driver {
  *	@unmap_space: unmap IP address space
  *	@request_irq: request IRQ
  *	@free_irq: free IRQ
- *	@read8: read unsigned char
- *	@read16: read unsigned short
- *	@read32: read unsigned int
- *	@write8: read unsigned char
- *	@write16: read unsigned short
- *	@write32: read unsigned int
- *	@remove_device: tell the bridge module that the device has been removed
+ *	@get_clockrate: Returns the clockrate the carrier is currently
+ *		communicating with the device at.
+ *	@set_clockrate: Sets the clock-rate for carrier / module communication.
+ *		Should return -EINVAL if the requested speed is not supported.
+ *	@get_error: Returns the error state for the slot the device is attached
+ *		to.
+ *	@get_timeout: Returns 1 if the communication with the device has
+ *		previously timed out.
+ *	@reset_timeout: Resets the state returned by get_timeout.
  */
 struct ipack_bus_ops {
 	int (*map_space) (struct ipack_device *dev, unsigned int memory_size, int space);
 	int (*unmap_space) (struct ipack_device *dev, int space);
-	int (*request_irq) (struct ipack_device *dev, int vector, int (*handler)(void *), void *arg);
+	int (*request_irq) (struct ipack_device *dev,
+			    irqreturn_t (*handler)(void *), void *arg);
 	int (*free_irq) (struct ipack_device *dev);
-	int (*read8) (struct ipack_device *dev, int space, unsigned long offset, unsigned char *value);
-	int (*read16) (struct ipack_device *dev, int space, unsigned long offset, unsigned short *value);
-	int (*read32) (struct ipack_device *dev, int space, unsigned long offset, unsigned int *value);
-	int (*write8) (struct ipack_device *dev, int space, unsigned long offset, unsigned char value);
-	int (*write16) (struct ipack_device *dev, int space, unsigned long offset, unsigned short value);
-	int (*write32) (struct ipack_device *dev, int space, unsigned long offset, unsigned int value);
-	int (*remove_device) (struct ipack_device *dev);
+	int (*get_clockrate) (struct ipack_device *dev);
+	int (*set_clockrate) (struct ipack_device *dev, int mherz);
+	int (*get_error) (struct ipack_device *dev);
+	int (*get_timeout) (struct ipack_device *dev);
+	int (*reset_timeout) (struct ipack_device *dev);
 };
 
 /**
@@ -139,7 +149,7 @@ struct ipack_bus_device {
 	struct device *parent;
 	int slots;
 	int bus_nr;
-	struct ipack_bus_ops *ops;
+	const struct ipack_bus_ops *ops;
 };
 
 /**
@@ -153,7 +163,7 @@ struct ipack_bus_device {
  * available bus device in ipack.
  */
 struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
-					    struct ipack_bus_ops *ops);
+					    const struct ipack_bus_ops *ops);
 
 /**
  *	ipack_bus_unregister -- unregister an ipack bus
@@ -166,7 +176,8 @@ int ipack_bus_unregister(struct ipack_bus_device *bus);
  * Called by a ipack driver to register itself as a driver
  * that can manage ipack devices.
  */
-int ipack_driver_register(struct ipack_driver *edrv, struct module *owner, char *name);
+int ipack_driver_register(struct ipack_driver *edrv, struct module *owner,
+			  const char *name);
 void ipack_driver_unregister(struct ipack_driver *edrv);
 
 /**
@@ -174,10 +185,33 @@ void ipack_driver_unregister(struct ipack_driver *edrv);
  *
  * @bus: ipack bus device it is plugged to.
  * @slot: slot position in the bus device.
- * @irqv: IRQ vector for the mezzanine.
  *
  * Register a new ipack device (mezzanine device). The call is done by
  * the carrier device driver.
  */
-struct ipack_device *ipack_device_register(struct ipack_bus_device *bus, int slot, int irqv);
+struct ipack_device *ipack_device_register(struct ipack_bus_device *bus, int slot);
 void ipack_device_unregister(struct ipack_device *dev);
+
+/**
+ * DEFINE_IPACK_DEVICE_TABLE - macro used to describe a IndustryPack table
+ * @_table: device table name
+ *
+ * This macro is used to create a struct ipack_device_id array (a device table)
+ * in a generic manner.
+ */
+#define DEFINE_IPACK_DEVICE_TABLE(_table) \
+	const struct ipack_device_id _table[] __devinitconst
+
+/**
+ * IPACK_DEVICE - macro used to describe a specific IndustryPack device
+ * @_format: the format version (currently either 1 or 2, 8 bit value)
+ * @vend:    the 8 or 24 bit IndustryPack Vendor ID
+ * @dev:     the 8 or 16  bit IndustryPack Device ID
+ *
+ * This macro is used to create a struct ipack_device_id that matches a specific
+ * device.
+ */
+#define IPACK_DEVICE(_format, vend, dev) \
+	 .format = (_format), \
+	 .vendor = (vend), \
+	 .device = (dev)
diff --git a/drivers/staging/ipack/ipack_ids.h b/drivers/staging/ipack/ipack_ids.h
new file mode 100644
index 000000000000..8153fee3f2f7
--- /dev/null
+++ b/drivers/staging/ipack/ipack_ids.h
@@ -0,0 +1,32 @@
+/*
+ * IndustryPack Fromat, Vendor and Device IDs.
+ */
+
+/* ID section format versions */
+#define IPACK_ID_VERSION_INVALID	0x00
+#define IPACK_ID_VERSION_1		0x01
+#define IPACK_ID_VERSION_2		0x02
+
+/* Vendors and devices. Sort key: vendor first, device next. */
+#define IPACK1_VENDOR_ID_RESERVED1	0x00
+#define IPACK1_VENDOR_ID_RESERVED2	0xFF
+#define IPACK1_VENDOR_ID_UNREGISTRED01	0x01
+#define IPACK1_VENDOR_ID_UNREGISTRED02	0x02
+#define IPACK1_VENDOR_ID_UNREGISTRED03	0x03
+#define IPACK1_VENDOR_ID_UNREGISTRED04	0x04
+#define IPACK1_VENDOR_ID_UNREGISTRED05	0x05
+#define IPACK1_VENDOR_ID_UNREGISTRED06	0x06
+#define IPACK1_VENDOR_ID_UNREGISTRED07	0x07
+#define IPACK1_VENDOR_ID_UNREGISTRED08	0x08
+#define IPACK1_VENDOR_ID_UNREGISTRED09	0x09
+#define IPACK1_VENDOR_ID_UNREGISTRED10	0x0A
+#define IPACK1_VENDOR_ID_UNREGISTRED11	0x0B
+#define IPACK1_VENDOR_ID_UNREGISTRED12	0x0C
+#define IPACK1_VENDOR_ID_UNREGISTRED13	0x0D
+#define IPACK1_VENDOR_ID_UNREGISTRED14	0x0E
+#define IPACK1_VENDOR_ID_UNREGISTRED15	0x0F
+
+#define IPACK1_VENDOR_ID_SBS            0xF0
+#define IPACK1_DEVICE_ID_SBS_OCTAL_232  0x22
+#define IPACK1_DEVICE_ID_SBS_OCTAL_422  0x2A
+#define IPACK1_DEVICE_ID_SBS_OCTAL_485  0x48
diff --git a/drivers/staging/keucr/smcommon.h b/drivers/staging/keucr/smcommon.h
index 278bdb871129..4d57203b64d8 100644
--- a/drivers/staging/keucr/smcommon.h
+++ b/drivers/staging/keucr/smcommon.h
@@ -25,7 +25,5 @@ Define Difinetion
 #define ERR_NoSmartMedia    0x003A /* Medium Not Present */
 
 /***************************************************************************/
-void StringCopy(char *, char *, int);
-int  StringCmp(char *, char *, int);
 
 #endif
diff --git a/drivers/staging/line6/pcm.c b/drivers/staging/line6/pcm.c
index 5e319e3ce685..7fe44a6fd0ed 100644
--- a/drivers/staging/line6/pcm.c
+++ b/drivers/staging/line6/pcm.c
@@ -48,7 +48,13 @@ static ssize_t pcm_set_impulse_volume(struct device *dev,
 				      const char *buf, size_t count)
 {
 	struct snd_line6_pcm *line6pcm = dev2pcm(dev);
-	int value = simple_strtoul(buf, NULL, 10);
+	int value;
+	int rv;
+
+	rv = kstrtoint(buf, 10, &value);
+	if (rv < 0)
+		return rv;
+
 	line6pcm->impulse_volume = value;
 
 	if (value > 0)
diff --git a/drivers/staging/line6/variax.c b/drivers/staging/line6/variax.c
index bb99ee4919e7..f97416b1de54 100644
--- a/drivers/staging/line6/variax.c
+++ b/drivers/staging/line6/variax.c
@@ -353,10 +353,10 @@ static ssize_t variax_set_model(struct device *dev,
 {
 	struct usb_line6_variax *variax =
 	    usb_get_intfdata(to_usb_interface(dev));
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -387,10 +387,10 @@ static ssize_t variax_set_active(struct device *dev,
 {
 	struct usb_line6_variax *variax =
 	    usb_get_intfdata(to_usb_interface(dev));
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/nvec/Kconfig b/drivers/staging/nvec/Kconfig
index 43048e9ba0d4..1235a7897d04 100644
--- a/drivers/staging/nvec/Kconfig
+++ b/drivers/staging/nvec/Kconfig
@@ -28,7 +28,7 @@ config NVEC_POWER
 
 config NVEC_PAZ00
 	bool "Support for OEM specific functions on Compal PAZ00 based devices"
-	depends on MFD_NVEC && LEDS_CLASS && MACH_PAZ00
+	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.
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index d0a7e408efe9..24d8eebc1d10 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -264,7 +264,7 @@ int nvec_write_async(struct nvec_chip *nvec, const unsigned char *data,
 	list_add_tail(&msg->node, &nvec->tx_data);
 	spin_unlock_irqrestore(&nvec->tx_lock, flags);
 
-	queue_work(nvec->wq, &nvec->tx_work);
+	queue_work(system_nrt_wq, &nvec->tx_work);
 
 	return 0;
 }
@@ -294,8 +294,10 @@ struct nvec_msg *nvec_write_sync(struct nvec_chip *nvec,
 
 	nvec->sync_write_pending = (data[1] << 8) + data[0];
 
-	if (nvec_write_async(nvec, data, size) < 0)
+	if (nvec_write_async(nvec, data, size) < 0) {
+		mutex_unlock(&nvec->sync_write_mutex);
 		return NULL;
+	}
 
 	dev_dbg(nvec->dev, "nvec_sync_write: 0x%04x\n",
 					nvec->sync_write_pending);
@@ -366,8 +368,7 @@ static void nvec_request_master(struct work_struct *work)
 static int parse_msg(struct nvec_chip *nvec, struct nvec_msg *msg)
 {
 	if ((msg->data[0] & 1 << 7) == 0 && msg->data[3]) {
-		dev_err(nvec->dev, "ec responded %02x %02x %02x %02x\n",
-			msg->data[0], msg->data[1], msg->data[2], msg->data[3]);
+		dev_err(nvec->dev, "ec responded %*ph\n", 4, msg->data);
 		return -EINVAL;
 	}
 
@@ -470,7 +471,7 @@ static void nvec_rx_completed(struct nvec_chip *nvec)
 	if (!nvec_msg_is_event(nvec->rx))
 		complete(&nvec->ec_transfer);
 
-	queue_work(nvec->wq, &nvec->rx_work);
+	queue_work(system_nrt_wq, &nvec->rx_work);
 }
 
 /**
@@ -737,12 +738,14 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 		nvec->gpio = pdata->gpio;
 		nvec->i2c_addr = pdata->i2c_addr;
 	} else if (nvec->dev->of_node) {
-		nvec->gpio = of_get_named_gpio(nvec->dev->of_node, "request-gpios", 0);
+		nvec->gpio = of_get_named_gpio(nvec->dev->of_node,
+					"request-gpios", 0);
 		if (nvec->gpio < 0) {
 			dev_err(&pdev->dev, "no gpio specified");
 			return -ENODEV;
 		}
-		if (of_property_read_u32(nvec->dev->of_node, "slave-addr", &nvec->i2c_addr)) {
+		if (of_property_read_u32(nvec->dev->of_node,
+					"slave-addr", &nvec->i2c_addr)) {
 			dev_err(&pdev->dev, "no i2c address specified");
 			return -ENODEV;
 		}
@@ -769,7 +772,7 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	i2c_clk = clk_get_sys("tegra-i2c.2", NULL);
+	i2c_clk = clk_get_sys("tegra-i2c.2", "div-clk");
 	if (IS_ERR(i2c_clk)) {
 		dev_err(nvec->dev, "failed to get controller clock\n");
 		return -ENODEV;
@@ -791,13 +794,11 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&nvec->tx_data);
 	INIT_WORK(&nvec->rx_work, nvec_dispatch);
 	INIT_WORK(&nvec->tx_work, nvec_request_master);
-	nvec->wq = alloc_workqueue("nvec", WQ_NON_REENTRANT, 2);
 
 	err = devm_gpio_request_one(&pdev->dev, nvec->gpio, GPIOF_OUT_INIT_HIGH,
 					"nvec gpio");
 	if (err < 0) {
 		dev_err(nvec->dev, "couldn't request gpio\n");
-		destroy_workqueue(nvec->wq);
 		return -ENODEV;
 	}
 
@@ -805,7 +806,6 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 				"nvec", nvec);
 	if (err) {
 		dev_err(nvec->dev, "couldn't request irq\n");
-		destroy_workqueue(nvec->wq);
 		return -ENODEV;
 	}
 	disable_irq(nvec->irq);
@@ -859,7 +859,8 @@ static int __devexit tegra_nvec_remove(struct platform_device *pdev)
 
 	nvec_write_async(nvec, EC_DISABLE_EVENT_REPORTING, 3);
 	mfd_remove_devices(nvec->dev);
-	destroy_workqueue(nvec->wq);
+	cancel_work_sync(&nvec->rx_work);
+	cancel_work_sync(&nvec->tx_work);
 
 	return 0;
 }
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 2c4bd746715a..d49c32a95690 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -11,6 +11,7 @@
  * License as published by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
 #include <linux/fb.h>
@@ -72,18 +73,16 @@ static int dcon_hw_init(struct dcon_priv *dcon, int is_init)
 
 	ver = dcon_read(dcon, DCON_REG_ID);
 	if ((ver >> 8) != 0xDC) {
-		printk(KERN_ERR "olpc-dcon:  DCON ID not 0xDCxx: 0x%04x instead.\n",
-			ver);
+		pr_err("DCON ID not 0xDCxx: 0x%04x instead.\n", ver);
 		rc = -ENXIO;
 		goto err;
 	}
 
 	if (is_init) {
-		printk(KERN_INFO "olpc-dcon:  Discovered DCON version %x\n",
-				ver & 0xFF);
+		pr_info("Discovered DCON version %x\n", ver & 0xFF);
 		rc = pdata->init(dcon);
 		if (rc != 0) {
-			printk(KERN_ERR "olpc-dcon:  Unable to init.\n");
+			pr_err("Unable to init.\n");
 			goto err;
 		}
 	}
@@ -137,8 +136,7 @@ power_up:
 		x = 1;
 		x = olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
 		if (x) {
-			printk(KERN_WARNING "olpc-dcon:  unable to force dcon to power up: %d!\n",
-				x);
+			pr_warn("unable to force dcon to power up: %d!\n", x);
 			return x;
 		}
 		msleep(10); /* we'll be conservative */
@@ -151,7 +149,7 @@ power_up:
 		x = dcon_read(dcon, DCON_REG_ID);
 	}
 	if (x < 0) {
-		printk(KERN_ERR "olpc-dcon:  unable to stabilize dcon's smbus, reasserting power and praying.\n");
+		pr_err("unable to stabilize dcon's smbus, reasserting power and praying.\n");
 		BUG_ON(olpc_board_at_least(olpc_board(0xc2)));
 		x = 0;
 		olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
@@ -222,8 +220,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 		x = 0;
 		x = olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
 		if (x)
-			printk(KERN_WARNING "olpc-dcon:  unable to force dcon to power down: %d!\n",
-				x);
+			pr_warn("unable to force dcon to power down: %d!\n", x);
 		else
 			dcon->asleep = sleep;
 	} else {
@@ -232,8 +229,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 			dcon->disp_mode |= MODE_BL_ENABLE;
 		x = dcon_bus_stabilize(dcon, 1);
 		if (x)
-			printk(KERN_WARNING "olpc-dcon:  unable to reinit dcon hardware: %d!\n",
-				x);
+			pr_warn("unable to reinit dcon hardware: %d!\n", x);
 		else
 			dcon->asleep = sleep;
 
@@ -304,12 +300,11 @@ static void dcon_source_switch(struct work_struct *work)
 
 	switch (source) {
 	case DCON_SOURCE_CPU:
-		printk(KERN_INFO "dcon_source_switch to CPU\n");
+		pr_info("dcon_source_switch to CPU\n");
 		/* Enable the scanline interrupt bit */
 		if (dcon_write(dcon, DCON_REG_MODE,
 				dcon->disp_mode | MODE_SCAN_INT))
-			printk(KERN_ERR
-			       "olpc-dcon:  couldn't enable scanline interrupt!\n");
+			pr_err("couldn't enable scanline interrupt!\n");
 		else {
 			/* Wait up to one second for the scanline interrupt */
 			wait_event_timeout(dcon_wait_queue,
@@ -317,11 +312,11 @@ static void dcon_source_switch(struct work_struct *work)
 		}
 
 		if (!dcon->switched)
-			printk(KERN_ERR "olpc-dcon:  Timeout entering CPU mode; expect a screen glitch.\n");
+			pr_err("Timeout entering CPU mode; expect a screen glitch.\n");
 
 		/* Turn off the scanline interrupt */
 		if (dcon_write(dcon, DCON_REG_MODE, dcon->disp_mode))
-			printk(KERN_ERR "olpc-dcon:  couldn't disable scanline interrupt!\n");
+			pr_err("couldn't disable scanline interrupt!\n");
 
 		/*
 		 * Ideally we'd like to disable interrupts here so that the
@@ -332,7 +327,7 @@ static void dcon_source_switch(struct work_struct *work)
 		 * For now, we just hope..
 		 */
 		if (!dcon_blank_fb(dcon, false)) {
-			printk(KERN_ERR "olpc-dcon:  Failed to enter CPU mode\n");
+			pr_err("Failed to enter CPU mode\n");
 			dcon->pending_src = DCON_SOURCE_DCON;
 			return;
 		}
@@ -341,14 +336,14 @@ static void dcon_source_switch(struct work_struct *work)
 		pdata->set_dconload(1);
 		getnstimeofday(&dcon->load_time);
 
-		printk(KERN_INFO "olpc-dcon: The CPU has control\n");
+		pr_info("The CPU has control\n");
 		break;
 	case DCON_SOURCE_DCON:
 	{
 		int t;
 		struct timespec delta_t;
 
-		printk(KERN_INFO "dcon_source_switch to DCON\n");
+		pr_info("dcon_source_switch to DCON\n");
 
 		add_wait_queue(&dcon_wait_queue, &wait);
 		set_current_state(TASK_UNINTERRUPTIBLE);
@@ -362,7 +357,7 @@ static void dcon_source_switch(struct work_struct *work)
 		set_current_state(TASK_RUNNING);
 
 		if (!dcon->switched) {
-			printk(KERN_ERR "olpc-dcon: Timeout entering DCON mode; expect a screen glitch.\n");
+			pr_err("Timeout entering DCON mode; expect a screen glitch.\n");
 		} else {
 			/* sometimes the DCON doesn't follow its own rules,
 			 * and doesn't wait for two vsync pulses before
@@ -378,7 +373,7 @@ static void dcon_source_switch(struct work_struct *work)
 			delta_t = timespec_sub(dcon->irq_time, dcon->load_time);
 			if (dcon->switched && delta_t.tv_sec == 0 &&
 					delta_t.tv_nsec < NSEC_PER_MSEC * 20) {
-				printk(KERN_ERR "olpc-dcon: missed loading, retrying\n");
+				pr_err("missed loading, retrying\n");
 				pdata->set_dconload(1);
 				mdelay(41);
 				pdata->set_dconload(0);
@@ -388,7 +383,7 @@ static void dcon_source_switch(struct work_struct *work)
 		}
 
 		dcon_blank_fb(dcon, true);
-		printk(KERN_INFO "olpc-dcon: The DCON has control\n");
+		pr_info("The DCON has control\n");
 		break;
 	}
 	default:
@@ -476,7 +471,7 @@ static ssize_t dcon_freeze_store(struct device *dev,
 	if (ret)
 		return ret;
 
-	printk(KERN_INFO "dcon_freeze_store: %lu\n", output);
+	pr_info("dcon_freeze_store: %lu\n", output);
 
 	switch (output) {
 	case 0:
@@ -650,7 +645,7 @@ static int dcon_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	dcon_device = platform_device_alloc("dcon", -1);
 
 	if (dcon_device == NULL) {
-		printk(KERN_ERR "dcon:  Unable to create the DCON device\n");
+		pr_err("Unable to create the DCON device\n");
 		rc = -ENOMEM;
 		goto eirq;
 	}
@@ -658,7 +653,7 @@ static int dcon_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	platform_set_drvdata(dcon_device, dcon);
 
 	if (rc) {
-		printk(KERN_ERR "dcon:  Unable to add the DCON device\n");
+		pr_err("Unable to add the DCON device\n");
 		goto edev;
 	}
 
@@ -762,7 +757,7 @@ irqreturn_t dcon_interrupt(int irq, void *id)
 
 	switch (status & 3) {
 	case 3:
-		printk(KERN_DEBUG "olpc-dcon: DCONLOAD_MISSED interrupt\n");
+		pr_debug("DCONLOAD_MISSED interrupt\n");
 		break;
 
 	case 2:	/* switch to DCON mode */
@@ -784,9 +779,9 @@ irqreturn_t dcon_interrupt(int irq, void *id)
 			dcon->switched = true;
 			getnstimeofday(&dcon->irq_time);
 			wake_up(&dcon_wait_queue);
-			printk(KERN_DEBUG "olpc-dcon: switching w/ status 0/0\n");
+			pr_debug("switching w/ status 0/0\n");
 		} else {
-			printk(KERN_DEBUG "olpc-dcon: scanline interrupt w/CPU\n");
+			pr_debug("scanline interrupt w/CPU\n");
 		}
 	}
 
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
index c87fdfac4855..77e8eb5a5abd 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
@@ -10,6 +10,9 @@
  * modify it under the terms of version 2 of the GNU General Public
  * License as published by the Free Software Foundation.
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/cs5535.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
@@ -22,23 +25,23 @@ static int dcon_init_xo_1(struct dcon_priv *dcon)
 	unsigned char lob;
 
 	if (gpio_request(OLPC_GPIO_DCON_STAT0, "OLPC-DCON")) {
-		printk(KERN_ERR "olpc-dcon: failed to request STAT0 GPIO\n");
+		pr_err("failed to request STAT0 GPIO\n");
 		return -EIO;
 	}
 	if (gpio_request(OLPC_GPIO_DCON_STAT1, "OLPC-DCON")) {
-		printk(KERN_ERR "olpc-dcon: failed to request STAT1 GPIO\n");
+		pr_err("failed to request STAT1 GPIO\n");
 		goto err_gp_stat1;
 	}
 	if (gpio_request(OLPC_GPIO_DCON_IRQ, "OLPC-DCON")) {
-		printk(KERN_ERR "olpc-dcon: failed to request IRQ GPIO\n");
+		pr_err("failed to request IRQ GPIO\n");
 		goto err_gp_irq;
 	}
 	if (gpio_request(OLPC_GPIO_DCON_LOAD, "OLPC-DCON")) {
-		printk(KERN_ERR "olpc-dcon: failed to request LOAD GPIO\n");
+		pr_err("failed to request LOAD GPIO\n");
 		goto err_gp_load;
 	}
 	if (gpio_request(OLPC_GPIO_DCON_BLANK, "OLPC-DCON")) {
-		printk(KERN_ERR "olpc-dcon: failed to request BLANK GPIO\n");
+		pr_err("failed to request BLANK GPIO\n");
 		goto err_gp_blank;
 	}
 
@@ -83,7 +86,7 @@ static int dcon_init_xo_1(struct dcon_priv *dcon)
 
 	/* Register the interrupt handler */
 	if (request_irq(DCON_IRQ, &dcon_interrupt, 0, "DCON", dcon)) {
-		printk(KERN_ERR "olpc-dcon: failed to request DCON's irq\n");
+		pr_err("failed to request DCON's irq\n");
 		goto err_req_irq;
 	}
 
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
index 69415eec425c..352dd3db0132 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
@@ -6,6 +6,8 @@
  * License as published by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/pci.h>
@@ -66,7 +68,7 @@ static int dcon_init_xo_1_5(struct dcon_priv *dcon)
 	pdev = pci_get_device(PCI_VENDOR_ID_VIA,
 			      PCI_DEVICE_ID_VIA_VX855, NULL);
 	if (!pdev) {
-		printk(KERN_ERR "cannot find VX855 PCI ID\n");
+		pr_err("cannot find VX855 PCI ID\n");
 		return 1;
 	}
 
@@ -104,7 +106,7 @@ static int dcon_init_xo_1_5(struct dcon_priv *dcon)
 	/* we're sharing the IRQ with ACPI */
 	irq = acpi_gbl_FADT.sci_interrupt;
 	if (request_irq(irq, &dcon_interrupt, IRQF_SHARED, "DCON", dcon)) {
-		printk(KERN_ERR PREFIX "DCON (IRQ%d) allocation failed\n", irq);
+		pr_err("DCON (IRQ%d) allocation failed\n", irq);
 		return 1;
 	}
 
diff --git a/drivers/staging/omap-thermal/omap-bandgap.c b/drivers/staging/omap-thermal/omap-bandgap.c
index c556abb63a17..368a2e19b2d4 100644
--- a/drivers/staging/omap-thermal/omap-bandgap.c
+++ b/drivers/staging/omap-thermal/omap-bandgap.c
@@ -157,7 +157,7 @@ static int temp_to_adc_conversion(long temp, struct omap_bandgap *bg_ptr, int i,
 	high = ts_data->adc_end_val - ts_data->adc_start_val;
 	mid = (high + low) / 2;
 
-	if (temp < bg_ptr->conv_table[high] || temp > bg_ptr->conv_table[high])
+	if (temp < bg_ptr->conv_table[low] || temp > bg_ptr->conv_table[high])
 		return -EINVAL;
 
 	while (low < high) {
@@ -953,12 +953,12 @@ int __devinit omap_bandgap_probe(struct platform_device *pdev)
 	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
 		char *domain;
 
+		if (bg_ptr->conf->sensors[i].register_cooling)
+			bg_ptr->conf->sensors[i].register_cooling(bg_ptr, i);
+
 		domain = bg_ptr->conf->sensors[i].domain;
 		if (bg_ptr->conf->expose_sensor)
 			bg_ptr->conf->expose_sensor(bg_ptr, i, domain);
-
-		if (bg_ptr->conf->sensors[i].register_cooling)
-			bg_ptr->conf->sensors[i].register_cooling(bg_ptr, i);
 	}
 
 	/*
@@ -1037,20 +1037,20 @@ static int omap_bandgap_save_ctxt(struct omap_bandgap *bg_ptr)
 
 		if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
 			rval->bg_mode_ctrl = omap_bandgap_readl(bg_ptr,
-								tsr->bgap_mode_ctrl);
+							tsr->bgap_mode_ctrl);
 		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
 			rval->bg_counter = omap_bandgap_readl(bg_ptr,
-							      tsr->bgap_counter);
+							tsr->bgap_counter);
 		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
 			rval->bg_threshold = omap_bandgap_readl(bg_ptr,
-								tsr->bgap_threshold);
+							tsr->bgap_threshold);
 			rval->bg_ctrl = omap_bandgap_readl(bg_ptr,
-							   tsr->bgap_mask_ctrl);
+						   tsr->bgap_mask_ctrl);
 		}
 
 		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
 			rval->tshut_threshold = omap_bandgap_readl(bg_ptr,
-								   tsr->tshut_threshold);
+						   tsr->tshut_threshold);
 	}
 
 	return 0;
@@ -1074,8 +1074,9 @@ static int omap_bandgap_restore_ctxt(struct omap_bandgap *bg_ptr)
 
 		if (val == 0) {
 			if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
-				omap_bandgap_writel(bg_ptr, rval->tshut_threshold,
-							   tsr->tshut_threshold);
+				omap_bandgap_writel(bg_ptr,
+					rval->tshut_threshold,
+						   tsr->tshut_threshold);
 			/* Force immediate temperature measurement and update
 			 * of the DTEMP field
 			 */
diff --git a/drivers/staging/omap-thermal/omap-thermal-common.c b/drivers/staging/omap-thermal/omap-thermal-common.c
index 0675a5e2f7c8..46ee0a9f49d9 100644
--- a/drivers/staging/omap-thermal/omap-thermal-common.c
+++ b/drivers/staging/omap-thermal/omap-thermal-common.c
@@ -77,10 +77,16 @@ static inline int omap_thermal_get_temp(struct thermal_zone_device *thermal,
 					 unsigned long *temp)
 {
 	struct omap_thermal_data *data = thermal->devdata;
-	struct omap_bandgap *bg_ptr = data->bg_ptr;
-	struct omap_temp_sensor *s = &bg_ptr->conf->sensors[data->sensor_id];
+	struct omap_bandgap *bg_ptr;
+	struct omap_temp_sensor *s;
 	int ret, tmp, pcb_temp, slope, constant;
 
+	if (!data)
+		return 0;
+
+	bg_ptr = data->bg_ptr;
+	s = &bg_ptr->conf->sensors[data->sensor_id];
+
 	ret = omap_bandgap_read_temperature(bg_ptr, data->sensor_id, &tmp);
 	if (ret)
 		return ret;
@@ -227,26 +233,44 @@ static struct thermal_zone_device_ops omap_thermal_ops = {
 	.get_crit_temp = omap_thermal_get_crit_temp,
 };
 
-int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
-			       char *domain)
+static struct omap_thermal_data
+*omap_thermal_build_data(struct omap_bandgap *bg_ptr, int id)
 {
 	struct omap_thermal_data *data;
 
 	data = devm_kzalloc(bg_ptr->dev, sizeof(*data), GFP_KERNEL);
 	if (!data) {
 		dev_err(bg_ptr->dev, "kzalloc fail\n");
-		return -ENOMEM;
+		return NULL;
 	}
 	data->sensor_id = id;
 	data->bg_ptr = bg_ptr;
 	data->mode = THERMAL_DEVICE_ENABLED;
 	INIT_WORK(&data->thermal_wq, omap_thermal_work);
 
+	return data;
+}
+
+int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
+			       char *domain)
+{
+	struct omap_thermal_pdata pdata;
+
+	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+
+	if (!data)
+		data = omap_thermal_build_pdata(bg_ptr, id);
+
+	if (!data)
+		return -EINVAL;
+
 	/* TODO: remove TC1 TC2 */
 	/* Create thermal zone */
 	data->omap_thermal = thermal_zone_device_register(domain,
 				OMAP_TRIP_NUMBER, 0, data, &omap_thermal_ops,
-				0, FAST_TEMP_MONITORING_RATE, 0, 0);
+				1, 2, /*TODO: remove this when FW allows */
+				FAST_TEMP_MONITORING_RATE,
+				FAST_TEMP_MONITORING_RATE);
 	if (IS_ERR_OR_NULL(data->omap_thermal)) {
 		dev_err(bg_ptr->dev, "thermal zone device is NULL\n");
 		return PTR_ERR(data->omap_thermal);
@@ -333,6 +357,11 @@ int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
 	int tab_size, ret;
 
 	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+	if (!data)
+		data = omap_thermal_build_pdata(bg_ptr, id);
+
+	if (!data)
+		return -EINVAL;
 
 	ret = omap_thermal_build_cpufreq_clip(bg_ptr, &tab_ptr, &tab_size);
 	if (ret < 0) {
@@ -349,6 +378,7 @@ int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
 		return PTR_ERR(data->cool_dev);
 	}
 	bg_ptr->conf->sensors[id].cooling_data.freq_clip_count = tab_size;
+	omap_bandgap_set_sensor_data(bg_ptr, id, data);
 
 	return 0;
 }
diff --git a/drivers/staging/omap-thermal/omap4-thermal.c b/drivers/staging/omap-thermal/omap4-thermal.c
index fa9dbcd71830..04c02b6c0077 100644
--- a/drivers/staging/omap-thermal/omap4-thermal.c
+++ b/drivers/staging/omap-thermal/omap4-thermal.c
@@ -77,15 +77,15 @@ const struct omap_bandgap_data omap4430_data = {
 	.remove_sensor = omap_thermal_remove_sensor,
 	.sensors = {
 		{
-			.registers = &omap4430_mpu_temp_sensor_registers,
-			.ts_data = &omap4430_mpu_temp_sensor_data,
-			.domain = "cpu",
-			.slope = 0,
-			.constant = 20000,
-			.slope_pcb = 0,
-			.constant_pcb = 20000,
-			.register_cooling = omap_thermal_register_cpu_cooling,
-			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.registers = &omap4430_mpu_temp_sensor_registers,
+		.ts_data = &omap4430_mpu_temp_sensor_data,
+		.domain = "cpu",
+		.slope = 0,
+		.constant = 20000,
+		.slope_pcb = 0,
+		.constant_pcb = 20000,
+		.register_cooling = omap_thermal_register_cpu_cooling,
+		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
@@ -215,15 +215,15 @@ const struct omap_bandgap_data omap4460_data = {
 	.remove_sensor = omap_thermal_remove_sensor,
 	.sensors = {
 		{
-			.registers = &omap4460_mpu_temp_sensor_registers,
-			.ts_data = &omap4460_mpu_temp_sensor_data,
-			.domain = "cpu",
-			.slope = OMAP_GRADIENT_SLOPE_4460,
-			.constant = OMAP_GRADIENT_CONST_4460,
-			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4460,
-			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4460,
-			.register_cooling = omap_thermal_register_cpu_cooling,
-			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.registers = &omap4460_mpu_temp_sensor_registers,
+		.ts_data = &omap4460_mpu_temp_sensor_data,
+		.domain = "cpu",
+		.slope = OMAP_GRADIENT_SLOPE_4460,
+		.constant = OMAP_GRADIENT_CONST_4460,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4460,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4460,
+		.register_cooling = omap_thermal_register_cpu_cooling,
+		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
@@ -244,15 +244,15 @@ const struct omap_bandgap_data omap4470_data = {
 	.remove_sensor = omap_thermal_remove_sensor,
 	.sensors = {
 		{
-			.registers = &omap4460_mpu_temp_sensor_registers,
-			.ts_data = &omap4460_mpu_temp_sensor_data,
-			.domain = "cpu",
-			.slope = OMAP_GRADIENT_SLOPE_4470,
-			.constant = OMAP_GRADIENT_CONST_4470,
-			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4470,
-			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4470,
-			.register_cooling = omap_thermal_register_cpu_cooling,
-			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.registers = &omap4460_mpu_temp_sensor_registers,
+		.ts_data = &omap4460_mpu_temp_sensor_data,
+		.domain = "cpu",
+		.slope = OMAP_GRADIENT_SLOPE_4470,
+		.constant = OMAP_GRADIENT_CONST_4470,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4470,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4470,
+		.register_cooling = omap_thermal_register_cpu_cooling,
+		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
 		},
 	},
 	.sensor_count = 1,
diff --git a/drivers/staging/omap-thermal/omap5-thermal.c b/drivers/staging/omap-thermal/omap5-thermal.c
index 0658af24a5c7..2f3a498dacd1 100644
--- a/drivers/staging/omap-thermal/omap5-thermal.c
+++ b/drivers/staging/omap-thermal/omap5-thermal.c
@@ -268,29 +268,29 @@ const struct omap_bandgap_data omap5430_data = {
 	.remove_sensor = omap_thermal_remove_sensor,
 	.sensors = {
 		{
-			.registers = &omap5430_mpu_temp_sensor_registers,
-			.ts_data = &omap5430_mpu_temp_sensor_data,
-			.domain = "cpu",
-			.register_cooling = omap_thermal_register_cpu_cooling,
-			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
-			.slope = OMAP_GRADIENT_SLOPE_5430_CPU,
-			.constant = OMAP_GRADIENT_CONST_5430_CPU,
-			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU,
-			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_CPU,
+		.registers = &omap5430_mpu_temp_sensor_registers,
+		.ts_data = &omap5430_mpu_temp_sensor_data,
+		.domain = "cpu",
+		.register_cooling = omap_thermal_register_cpu_cooling,
+		.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		.slope = OMAP_GRADIENT_SLOPE_5430_CPU,
+		.constant = OMAP_GRADIENT_CONST_5430_CPU,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_CPU,
 		},
 		{
-			.registers = &omap5430_gpu_temp_sensor_registers,
-			.ts_data = &omap5430_gpu_temp_sensor_data,
-			.domain = "gpu",
-			.slope = OMAP_GRADIENT_SLOPE_5430_GPU,
-			.constant = OMAP_GRADIENT_CONST_5430_GPU,
-			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU,
-			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_GPU,
+		.registers = &omap5430_gpu_temp_sensor_registers,
+		.ts_data = &omap5430_gpu_temp_sensor_data,
+		.domain = "gpu",
+		.slope = OMAP_GRADIENT_SLOPE_5430_GPU,
+		.constant = OMAP_GRADIENT_CONST_5430_GPU,
+		.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU,
+		.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_GPU,
 		},
 		{
-			.registers = &omap5430_core_temp_sensor_registers,
-			.ts_data = &omap5430_core_temp_sensor_data,
-			.domain = "core",
+		.registers = &omap5430_core_temp_sensor_registers,
+		.ts_data = &omap5430_core_temp_sensor_data,
+		.domain = "core",
 		},
 	},
 	.sensor_count = 3,
diff --git a/drivers/staging/omapdrm/omap_crtc.c b/drivers/staging/omapdrm/omap_crtc.c
index 62e0022561bc..732f2ad34036 100644
--- a/drivers/staging/omapdrm/omap_crtc.c
+++ b/drivers/staging/omapdrm/omap_crtc.c
@@ -155,6 +155,7 @@ static void page_flip_cb(void *arg)
 	struct drm_crtc *crtc = arg;
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
 	struct drm_framebuffer *old_fb = omap_crtc->old_fb;
+	struct drm_gem_object *bo;
 
 	omap_crtc->old_fb = NULL;
 
@@ -165,6 +166,9 @@ static void page_flip_cb(void *arg)
 	 * cycle.. for now go for correctness and later figure out speed..
 	 */
 	omap_plane_on_endwin(omap_crtc->plane, vblank_cb, crtc);
+
+	bo = omap_framebuffer_bo(crtc->fb, 0);
+	drm_gem_object_unreference_unlocked(bo);
 }
 
 static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
@@ -173,6 +177,7 @@ static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
 {
 	struct drm_device *dev = crtc->dev;
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct drm_gem_object *bo;
 
 	DBG("%d -> %d", crtc->fb ? crtc->fb->base.id : -1, fb->base.id);
 
@@ -185,16 +190,38 @@ static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
 	omap_crtc->event = event;
 	crtc->fb = fb;
 
-	omap_gem_op_async(omap_framebuffer_bo(fb, 0), OMAP_GEM_READ,
-			page_flip_cb, crtc);
+	/*
+	 * Hold a reference temporarily until the crtc is updated
+	 * and takes the reference to the bo.  This avoids it
+	 * getting freed from under us:
+	 */
+	bo = omap_framebuffer_bo(fb, 0);
+	drm_gem_object_reference(bo);
+
+	omap_gem_op_async(bo, OMAP_GEM_READ, page_flip_cb, crtc);
 
 	return 0;
 }
 
+static int omap_crtc_set_property(struct drm_crtc *crtc,
+		struct drm_property *property, uint64_t val)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct omap_drm_private *priv = crtc->dev->dev_private;
+
+	if (property == priv->rotation_prop) {
+		crtc->invert_dimensions =
+				!!(val & ((1LL << DRM_ROTATE_90) | (1LL << DRM_ROTATE_270)));
+	}
+
+	return omap_plane_set_property(omap_crtc->plane, property, val);
+}
+
 static const struct drm_crtc_funcs omap_crtc_funcs = {
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = omap_crtc_destroy,
 	.page_flip = omap_crtc_page_flip_locked,
+	.set_property = omap_crtc_set_property,
 };
 
 static const struct drm_crtc_helper_funcs omap_crtc_helper_funcs = {
@@ -231,6 +258,8 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 	drm_crtc_init(dev, crtc, &omap_crtc_funcs);
 	drm_crtc_helper_add(crtc, &omap_crtc_helper_funcs);
 
+	omap_plane_install_properties(omap_crtc->plane, &crtc->base);
+
 	return crtc;
 
 fail:
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.c b/drivers/staging/omapdrm/omap_dmm_tiler.c
index 86197831f63e..3ae39554df18 100644
--- a/drivers/staging/omapdrm/omap_dmm_tiler.c
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.c
@@ -120,7 +120,7 @@ static int wait_status(struct refill_engine *engine, uint32_t wait_mask)
 	return 0;
 }
 
-irqreturn_t omap_dmm_irq_handler(int irq, void *arg)
+static irqreturn_t omap_dmm_irq_handler(int irq, void *arg)
 {
 	struct dmm *dmm = arg;
 	uint32_t status = readl(dmm->base + DMM_PAT_IRQSTATUS);
@@ -367,7 +367,7 @@ struct tiler_block *tiler_reserve_1d(size_t size)
 	int num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 
 	if (!block)
-		return 0;
+		return ERR_PTR(-ENOMEM);
 
 	block->fmt = TILFMT_PAGE;
 
@@ -404,8 +404,26 @@ int tiler_release(struct tiler_block *block)
  * Utils
  */
 
-/* calculate the tiler space address of a pixel in a view orientation */
-static u32 tiler_get_address(u32 orient, enum tiler_fmt fmt, u32 x, u32 y)
+/* calculate the tiler space address of a pixel in a view orientation...
+ * below description copied from the display subsystem section of TRM:
+ *
+ * When the TILER is addressed, the bits:
+ *   [28:27] = 0x0 for 8-bit tiled
+ *             0x1 for 16-bit tiled
+ *             0x2 for 32-bit tiled
+ *             0x3 for page mode
+ *   [31:29] = 0x0 for 0-degree view
+ *             0x1 for 180-degree view + mirroring
+ *             0x2 for 0-degree view + mirroring
+ *             0x3 for 180-degree view
+ *             0x4 for 270-degree view + mirroring
+ *             0x5 for 270-degree view
+ *             0x6 for 90-degree view
+ *             0x7 for 90-degree view + mirroring
+ * Otherwise the bits indicated the corresponding bit address to access
+ * the SDRAM.
+ */
+static u32 tiler_get_address(enum tiler_fmt fmt, u32 orient, u32 x, u32 y)
 {
 	u32 x_bits, y_bits, tmp, x_mask, y_mask, alignment;
 
@@ -417,8 +435,11 @@ static u32 tiler_get_address(u32 orient, enum tiler_fmt fmt, u32 x, u32 y)
 	x_mask = MASK(x_bits);
 	y_mask = MASK(y_bits);
 
-	if (x < 0 || x > x_mask || y < 0 || y > y_mask)
+	if (x < 0 || x > x_mask || y < 0 || y > y_mask) {
+		DBG("invalid coords: %u < 0 || %u > %u || %u < 0 || %u > %u",
+				x, x, x_mask, y, y, y_mask);
 		return 0;
+	}
 
 	/* account for mirroring */
 	if (orient & MASK_X_INVERT)
@@ -439,11 +460,22 @@ dma_addr_t tiler_ssptr(struct tiler_block *block)
 {
 	BUG_ON(!validfmt(block->fmt));
 
-	return TILVIEW_8BIT + tiler_get_address(0, block->fmt,
+	return TILVIEW_8BIT + tiler_get_address(block->fmt, 0,
 			block->area.p0.x * geom[block->fmt].slot_w,
 			block->area.p0.y * geom[block->fmt].slot_h);
 }
 
+dma_addr_t tiler_tsptr(struct tiler_block *block, uint32_t orient,
+		uint32_t x, uint32_t y)
+{
+	struct tcm_pt *p = &block->area.p0;
+	BUG_ON(!validfmt(block->fmt));
+
+	return tiler_get_address(block->fmt, orient,
+			(p->x * geom[block->fmt].slot_w) + x,
+			(p->y * geom[block->fmt].slot_h) + y);
+}
+
 void tiler_align(enum tiler_fmt fmt, uint16_t *w, uint16_t *h)
 {
 	BUG_ON(!validfmt(fmt));
@@ -451,11 +483,14 @@ void tiler_align(enum tiler_fmt fmt, uint16_t *w, uint16_t *h)
 	*h = round_up(*h, geom[fmt].slot_h);
 }
 
-uint32_t tiler_stride(enum tiler_fmt fmt)
+uint32_t tiler_stride(enum tiler_fmt fmt, uint32_t orient)
 {
 	BUG_ON(!validfmt(fmt));
 
-	return 1 << (CONT_WIDTH_BITS + geom[fmt].y_shft);
+	if (orient & MASK_XY_FLIP)
+		return 1 << (CONT_HEIGHT_BITS + geom[fmt].x_shft);
+	else
+		return 1 << (CONT_WIDTH_BITS + geom[fmt].y_shft);
 }
 
 size_t tiler_size(enum tiler_fmt fmt, uint16_t w, uint16_t h)
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.h b/drivers/staging/omapdrm/omap_dmm_tiler.h
index 7b1052a329e4..740911df5fc3 100644
--- a/drivers/staging/omapdrm/omap_dmm_tiler.h
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.h
@@ -54,7 +54,18 @@ struct tiler_block {
 #define TILER_WIDTH             (1 << (CONT_WIDTH_BITS - SLOT_WIDTH_BITS))
 #define TILER_HEIGHT            (1 << (CONT_HEIGHT_BITS - SLOT_HEIGHT_BITS))
 
-/* tiler space addressing bitfields */
+/*
+Table 15-11. Coding and Description of TILER Orientations
+S Y X	Description				Alternate description
+0 0 0	0-degree view				Natural view
+0 0 1	0-degree view with vertical mirror 	180-degree view with horizontal mirror
+0 1 0	0-degree view with horizontal mirror 	180-degree view with vertical mirror
+0 1 1	180-degree view
+1 0 0	90-degree view with vertical mirror	270-degree view with horizontal mirror
+1 0 1	270-degree view
+1 1 0	90-degree view
+1 1 1	90-degree view with horizontal mirror	270-degree view with vertical mirror
+ */
 #define MASK_XY_FLIP		(1 << 31)
 #define MASK_Y_INVERT		(1 << 30)
 #define MASK_X_INVERT		(1 << 29)
@@ -90,7 +101,9 @@ int tiler_release(struct tiler_block *block);
 
 /* utilities */
 dma_addr_t tiler_ssptr(struct tiler_block *block);
-uint32_t tiler_stride(enum tiler_fmt fmt);
+dma_addr_t tiler_tsptr(struct tiler_block *block, uint32_t orient,
+		uint32_t x, uint32_t y);
+uint32_t tiler_stride(enum tiler_fmt fmt, uint32_t orient);
 size_t tiler_size(enum tiler_fmt fmt, uint16_t w, uint16_t h);
 size_t tiler_vsize(enum tiler_fmt fmt, uint16_t w, uint16_t h);
 void tiler_align(enum tiler_fmt fmt, uint16_t *w, uint16_t *h);
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
index 4beab9447ceb..2ec5264dd002 100644
--- a/drivers/staging/omapdrm/omap_drv.c
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -571,8 +571,7 @@ static int dev_load(struct drm_device *dev, unsigned long flags)
 
 	dev->dev_private = priv;
 
-	priv->wq = alloc_workqueue("omapdrm",
-			WQ_UNBOUND | WQ_NON_REENTRANT, 1);
+	priv->wq = alloc_ordered_workqueue("omapdrm", 0);
 
 	INIT_LIST_HEAD(&priv->obj_list);
 
@@ -649,6 +648,8 @@ static int dev_firstopen(struct drm_device *dev)
  */
 static void dev_lastclose(struct drm_device *dev)
 {
+	int i;
+
 	/* we don't support vga-switcheroo.. so just make sure the fbdev
 	 * mode is active
 	 */
@@ -657,6 +658,21 @@ static void dev_lastclose(struct drm_device *dev)
 
 	DBG("lastclose: dev=%p", dev);
 
+	/* need to restore default rotation state.. not sure if there is
+	 * a cleaner way to restore properties to default state?  Maybe
+	 * a flag that properties should automatically be restored to
+	 * default state on lastclose?
+	 */
+	for (i = 0; i < priv->num_crtcs; i++) {
+		drm_object_property_set_value(&priv->crtcs[i]->base,
+				priv->rotation_prop, 0);
+	}
+
+	for (i = 0; i < priv->num_planes; i++) {
+		drm_object_property_set_value(&priv->planes[i]->base,
+				priv->rotation_prop, 0);
+	}
+
 	ret = drm_fb_helper_restore_fbdev_mode(priv->fbdev);
 	if (ret)
 		DBG("failed to restore crtc mode");
@@ -761,7 +777,6 @@ static struct drm_driver omap_drm_driver = {
 		.irq_postinstall = dev_irq_postinstall,
 		.irq_uninstall = dev_irq_uninstall,
 		.irq_handler = dev_irq_handler,
-		.reclaim_buffers = drm_core_reclaim_buffers,
 #ifdef CONFIG_DEBUG_FS
 		.debugfs_init = omap_debugfs_init,
 		.debugfs_cleanup = omap_debugfs_cleanup,
diff --git a/drivers/staging/omapdrm/omap_drv.h b/drivers/staging/omapdrm/omap_drv.h
index 2092a9167d29..9dc72d143ff3 100644
--- a/drivers/staging/omapdrm/omap_drv.h
+++ b/drivers/staging/omapdrm/omap_drv.h
@@ -59,6 +59,27 @@ struct omap_drm_private {
 	struct list_head obj_list;
 
 	bool has_dmm;
+
+	/* properties: */
+	struct drm_property *rotation_prop;
+	struct drm_property *zorder_prop;
+};
+
+/* this should probably be in drm-core to standardize amongst drivers */
+#define DRM_ROTATE_0	0
+#define DRM_ROTATE_90	1
+#define DRM_ROTATE_180	2
+#define DRM_ROTATE_270	3
+#define DRM_REFLECT_X	4
+#define DRM_REFLECT_Y	5
+
+/* parameters which describe (unrotated) coordinates of scanout within a fb: */
+struct omap_drm_window {
+	uint32_t rotation;
+	int32_t  crtc_x, crtc_y;		/* signed because can be offscreen */
+	uint32_t crtc_w, crtc_h;
+	uint32_t src_x, src_y;
+	uint32_t src_w, src_h;
 };
 
 #ifdef CONFIG_DEBUG_FS
@@ -87,6 +108,10 @@ int omap_plane_mode_set(struct drm_plane *plane,
 		uint32_t src_w, uint32_t src_h);
 void omap_plane_on_endwin(struct drm_plane *plane,
 		void (*fxn)(void *), void *arg);
+void omap_plane_install_properties(struct drm_plane *plane,
+		struct drm_mode_object *obj);
+int omap_plane_set_property(struct drm_plane *plane,
+		struct drm_property *property, uint64_t val);
 
 struct drm_encoder *omap_encoder_init(struct drm_device *dev,
 		struct omap_overlay_manager *mgr);
@@ -114,8 +139,8 @@ struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb, int p);
 int omap_framebuffer_replace(struct drm_framebuffer *a,
 		struct drm_framebuffer *b, void *arg,
 		void (*unpin)(void *arg, struct drm_gem_object *bo));
-void omap_framebuffer_update_scanout(struct drm_framebuffer *fb, int x, int y,
-		struct omap_overlay_info *info);
+void omap_framebuffer_update_scanout(struct drm_framebuffer *fb,
+		struct omap_drm_window *win, struct omap_overlay_info *info);
 struct drm_connector *omap_framebuffer_get_next_connector(
 		struct drm_framebuffer *fb, struct drm_connector *from);
 void omap_framebuffer_flush(struct drm_framebuffer *fb,
@@ -157,8 +182,12 @@ int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages,
 		bool remap);
 int omap_gem_put_pages(struct drm_gem_object *obj);
 uint32_t omap_gem_flags(struct drm_gem_object *obj);
+int omap_gem_rotated_paddr(struct drm_gem_object *obj, uint32_t orient,
+		int x, int y, dma_addr_t *paddr);
 uint64_t omap_gem_mmap_offset(struct drm_gem_object *obj);
 size_t omap_gem_mmap_size(struct drm_gem_object *obj);
+int omap_gem_tiled_size(struct drm_gem_object *obj, uint16_t *w, uint16_t *h);
+int omap_gem_tiled_stride(struct drm_gem_object *obj, uint32_t orient);
 
 struct dma_buf * omap_gem_prime_export(struct drm_device *dev,
 		struct drm_gem_object *obj, int flags);
diff --git a/drivers/staging/omapdrm/omap_fb.c b/drivers/staging/omapdrm/omap_fb.c
index 74260f043ab1..446801d63007 100644
--- a/drivers/staging/omapdrm/omap_fb.c
+++ b/drivers/staging/omapdrm/omap_fb.c
@@ -18,6 +18,7 @@
  */
 
 #include "omap_drv.h"
+#include "omap_dmm_tiler.h"
 
 #include "drm_crtc.h"
 #include "drm_crtc_helper.h"
@@ -137,30 +138,100 @@ static const struct drm_framebuffer_funcs omap_framebuffer_funcs = {
 	.dirty = omap_framebuffer_dirty,
 };
 
+static uint32_t get_linear_addr(struct plane *plane,
+		const struct format *format, int n, int x, int y)
+{
+	uint32_t offset;
+
+	offset = plane->offset +
+			(x * format->planes[n].stride_bpp) +
+			(y * plane->pitch / format->planes[n].sub_y);
+
+	return plane->paddr + offset;
+}
+
 /* update ovl info for scanout, handles cases of multi-planar fb's, etc.
  */
-void omap_framebuffer_update_scanout(struct drm_framebuffer *fb, int x, int y,
-		struct omap_overlay_info *info)
+void omap_framebuffer_update_scanout(struct drm_framebuffer *fb,
+		struct omap_drm_window *win, struct omap_overlay_info *info)
 {
 	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
 	const struct format *format = omap_fb->format;
 	struct plane *plane = &omap_fb->planes[0];
-	unsigned int offset;
+	uint32_t x, y, orient = 0;
+
+	info->color_mode = format->dss_format;
+
+	info->pos_x      = win->crtc_x;
+	info->pos_y      = win->crtc_y;
+	info->out_width  = win->crtc_w;
+	info->out_height = win->crtc_h;
+	info->width      = win->src_w;
+	info->height     = win->src_h;
+
+	x = win->src_x;
+	y = win->src_y;
+
+	if (omap_gem_flags(plane->bo) & OMAP_BO_TILED) {
+		uint32_t w = win->src_w;
+		uint32_t h = win->src_h;
+
+		switch (win->rotation & 0xf) {
+		default:
+			dev_err(fb->dev->dev, "invalid rotation: %02x",
+					(uint32_t)win->rotation);
+			/* fallthru to default to no rotation */
+		case 0:
+		case BIT(DRM_ROTATE_0):
+			orient = 0;
+			break;
+		case BIT(DRM_ROTATE_90):
+			orient = MASK_XY_FLIP | MASK_X_INVERT;
+			break;
+		case BIT(DRM_ROTATE_180):
+			orient = MASK_X_INVERT | MASK_Y_INVERT;
+			break;
+		case BIT(DRM_ROTATE_270):
+			orient = MASK_XY_FLIP | MASK_Y_INVERT;
+			break;
+		}
 
-	offset = plane->offset +
-			(x * format->planes[0].stride_bpp) +
-			(y * plane->pitch / format->planes[0].sub_y);
+		if (win->rotation & BIT(DRM_REFLECT_X))
+			orient ^= MASK_X_INVERT;
+
+		if (win->rotation & BIT(DRM_REFLECT_Y))
+			orient ^= MASK_Y_INVERT;
+
+		/* adjust x,y offset for flip/invert: */
+		if (orient & MASK_XY_FLIP)
+			swap(w, h);
+		if (orient & MASK_Y_INVERT)
+			y += h - 1;
+		if (orient & MASK_X_INVERT)
+			x += w - 1;
 
-	info->color_mode   = format->dss_format;
-	info->paddr        = plane->paddr + offset;
-	info->screen_width = plane->pitch / format->planes[0].stride_bpp;
+		omap_gem_rotated_paddr(plane->bo, orient, x, y, &info->paddr);
+		info->rotation_type = OMAP_DSS_ROT_TILER;
+		info->screen_width  = omap_gem_tiled_stride(plane->bo, orient);
+	} else {
+		info->paddr         = get_linear_addr(plane, format, 0, x, y);
+		info->rotation_type = OMAP_DSS_ROT_DMA;
+		info->screen_width  = plane->pitch;
+	}
+
+	/* convert to pixels: */
+	info->screen_width /= format->planes[0].stride_bpp;
 
 	if (format->dss_format == OMAP_DSS_COLOR_NV12) {
 		plane = &omap_fb->planes[1];
-		offset = plane->offset +
-				(x * format->planes[1].stride_bpp) +
-				(y * plane->pitch / format->planes[1].sub_y);
-		info->p_uv_addr = plane->paddr + offset;
+
+		if (info->rotation_type == OMAP_DSS_ROT_TILER) {
+			WARN_ON(!(omap_gem_flags(plane->bo) & OMAP_BO_TILED));
+			omap_gem_rotated_paddr(plane->bo, orient,
+					x/2, y/2, &info->p_uv_addr);
+		} else {
+			info->p_uv_addr = get_linear_addr(plane, format, 1, x, y);
+		}
 	} else {
 		info->p_uv_addr = 0;
 	}
@@ -377,7 +448,7 @@ struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
 
 		size = pitch * mode_cmd->height / format->planes[i].sub_y;
 
-		if (size > (bos[i]->size - mode_cmd->offsets[i])) {
+		if (size > (omap_gem_mmap_size(bos[i]) - mode_cmd->offsets[i])) {
 			dev_err(dev->dev, "provided buffer object is too small! %d < %d\n",
 					bos[i]->size - mode_cmd->offsets[i], size);
 			ret = -EINVAL;
diff --git a/drivers/staging/omapdrm/omap_gem.c b/drivers/staging/omapdrm/omap_gem.c
index 3a0d035a9e03..c8287438e0dc 100644
--- a/drivers/staging/omapdrm/omap_gem.c
+++ b/drivers/staging/omapdrm/omap_gem.c
@@ -226,7 +226,8 @@ static int omap_gem_attach_pages(struct drm_gem_object *obj)
 	struct drm_device *dev = obj->dev;
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
 	struct page **pages;
-	int i, npages = obj->size >> PAGE_SHIFT;
+	int npages = obj->size >> PAGE_SHIFT;
+	int i, ret;
 	dma_addr_t *addrs;
 
 	WARN_ON(omap_obj->pages);
@@ -246,18 +247,32 @@ static int omap_gem_attach_pages(struct drm_gem_object *obj)
 	 */
 	if (omap_obj->flags & (OMAP_BO_WC|OMAP_BO_UNCACHED)) {
 		addrs = kmalloc(npages * sizeof(addrs), GFP_KERNEL);
+		if (!addrs) {
+			ret = -ENOMEM;
+			goto free_pages;
+		}
+
 		for (i = 0; i < npages; i++) {
 			addrs[i] = dma_map_page(dev->dev, pages[i],
 					0, PAGE_SIZE, DMA_BIDIRECTIONAL);
 		}
 	} else {
 		addrs = kzalloc(npages * sizeof(addrs), GFP_KERNEL);
+		if (!addrs) {
+			ret = -ENOMEM;
+			goto free_pages;
+		}
 	}
 
 	omap_obj->addrs = addrs;
 	omap_obj->pages = pages;
 
 	return 0;
+
+free_pages:
+	_drm_gem_put_pages(obj, pages, true, false);
+
+	return ret;
 }
 
 /** release backing pages */
@@ -339,6 +354,17 @@ size_t omap_gem_mmap_size(struct drm_gem_object *obj)
 	return size;
 }
 
+/* get tiled size, returns -EINVAL if not tiled buffer */
+int omap_gem_tiled_size(struct drm_gem_object *obj, uint16_t *w, uint16_t *h)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	if (omap_obj->flags & OMAP_BO_TILED) {
+		*w = omap_obj->width;
+		*h = omap_obj->height;
+		return 0;
+	}
+	return -EINVAL;
+}
 
 /* Normal handling for the case of faulting in non-tiled buffers */
 static int fault_1d(struct drm_gem_object *obj,
@@ -832,6 +858,36 @@ fail:
 	return ret;
 }
 
+/* Get rotated scanout address (only valid if already pinned), at the
+ * specified orientation and x,y offset from top-left corner of buffer
+ * (only valid for tiled 2d buffers)
+ */
+int omap_gem_rotated_paddr(struct drm_gem_object *obj, uint32_t orient,
+		int x, int y, dma_addr_t *paddr)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = -EINVAL;
+
+	mutex_lock(&obj->dev->struct_mutex);
+	if ((omap_obj->paddr_cnt > 0) && omap_obj->block &&
+			(omap_obj->flags & OMAP_BO_TILED)) {
+		*paddr = tiler_tsptr(omap_obj->block, orient, x, y);
+		ret = 0;
+	}
+	mutex_unlock(&obj->dev->struct_mutex);
+	return ret;
+}
+
+/* Get tiler stride for the buffer (only valid for 2d tiled buffers) */
+int omap_gem_tiled_stride(struct drm_gem_object *obj, uint32_t orient)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = -EINVAL;
+	if (omap_obj->flags & OMAP_BO_TILED)
+		ret = tiler_stride(gem2fmt(omap_obj->flags), orient);
+	return ret;
+}
+
 /* acquire pages when needed (for example, for DMA where physically
  * contiguous buffer is not required
  */
@@ -1402,7 +1458,7 @@ void omap_gem_init(struct drm_device *dev)
 		 */
 		usergart[i].height = h;
 		usergart[i].height_shift = ilog2(h);
-		usergart[i].stride_pfn = tiler_stride(fmts[i]) >> PAGE_SHIFT;
+		usergart[i].stride_pfn = tiler_stride(fmts[i], 0) >> PAGE_SHIFT;
 		usergart[i].slot_shift = ilog2((PAGE_SIZE / h) >> i);
 		for (j = 0; j < NUM_USERGART_ENTRIES; j++) {
 			struct usergart_entry *entry = &usergart[i].entry[j];
diff --git a/drivers/staging/omapdrm/omap_plane.c b/drivers/staging/omapdrm/omap_plane.c
index 7997be74010d..4bde639dd02c 100644
--- a/drivers/staging/omapdrm/omap_plane.c
+++ b/drivers/staging/omapdrm/omap_plane.c
@@ -20,6 +20,7 @@
 #include <linux/kfifo.h>
 
 #include "omap_drv.h"
+#include "omap_dmm_tiler.h"
 
 /* some hackery because omapdss has an 'enum omap_plane' (which would be
  * better named omap_plane_id).. and compiler seems unhappy about having
@@ -43,10 +44,9 @@ struct omap_plane {
 	struct omap_overlay *ovl;
 	struct omap_overlay_info info;
 
-	/* Source values, converted to integers because we don't support
-	 * fractional positions:
-	 */
-	unsigned int src_x, src_y;
+	/* position/orientation of scanout within the fb: */
+	struct omap_drm_window win;
+
 
 	/* last fb that we pinned: */
 	struct drm_framebuffer *pinned_fb;
@@ -289,6 +289,7 @@ static void update_scanout(struct drm_plane *plane)
 {
 	struct omap_plane *omap_plane = to_omap_plane(plane);
 	struct omap_overlay_info *info = &omap_plane->info;
+	struct omap_drm_window *win = &omap_plane->win;
 	int ret;
 
 	ret = update_pin(plane, plane->fb);
@@ -299,11 +300,10 @@ static void update_scanout(struct drm_plane *plane)
 		return;
 	}
 
-	omap_framebuffer_update_scanout(plane->fb,
-			omap_plane->src_x, omap_plane->src_y, info);
+	omap_framebuffer_update_scanout(plane->fb, win, info);
 
 	DBG("%s: %d,%d: %08x %08x (%d)", omap_plane->ovl->name,
-			omap_plane->src_x, omap_plane->src_y,
+			win->src_x, win->src_y,
 			(u32)info->paddr, (u32)info->p_uv_addr,
 			info->screen_width);
 }
@@ -316,21 +316,18 @@ int omap_plane_mode_set(struct drm_plane *plane,
 		uint32_t src_w, uint32_t src_h)
 {
 	struct omap_plane *omap_plane = to_omap_plane(plane);
+	struct omap_drm_window *win = &omap_plane->win;
+
+	win->crtc_x = crtc_x;
+	win->crtc_y = crtc_y;
+	win->crtc_w = crtc_w;
+	win->crtc_h = crtc_h;
 
 	/* src values are in Q16 fixed point, convert to integer: */
-	src_x = src_x >> 16;
-	src_y = src_y >> 16;
-	src_w = src_w >> 16;
-	src_h = src_h >> 16;
-
-	omap_plane->info.pos_x = crtc_x;
-	omap_plane->info.pos_y = crtc_y;
-	omap_plane->info.out_width = crtc_w;
-	omap_plane->info.out_height = crtc_h;
-	omap_plane->info.width = src_w;
-	omap_plane->info.height = src_h;
-	omap_plane->src_x = src_x;
-	omap_plane->src_y = src_y;
+	win->src_x = src_x >> 16;
+	win->src_y = src_y >> 16;
+	win->src_w = src_w >> 16;
+	win->src_h = src_h >> 16;
 
 	/* note: this is done after this fxn returns.. but if we need
 	 * to do a commit/update_scanout, etc before this returns we
@@ -359,6 +356,8 @@ static int omap_plane_update(struct drm_plane *plane,
 
 static int omap_plane_disable(struct drm_plane *plane)
 {
+	struct omap_plane *omap_plane = to_omap_plane(plane);
+	omap_plane->win.rotation = BIT(DRM_ROTATE_0);
 	return omap_plane_dpms(plane, DRM_MODE_DPMS_OFF);
 }
 
@@ -409,10 +408,79 @@ void omap_plane_on_endwin(struct drm_plane *plane,
 	install_irq(plane);
 }
 
+/* helper to install properties which are common to planes and crtcs */
+void omap_plane_install_properties(struct drm_plane *plane,
+		struct drm_mode_object *obj)
+{
+	struct drm_device *dev = plane->dev;
+	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_property *prop;
+
+	prop = priv->rotation_prop;
+	if (!prop) {
+		const struct drm_prop_enum_list props[] = {
+				{ DRM_ROTATE_0,   "rotate-0" },
+				{ DRM_ROTATE_90,  "rotate-90" },
+				{ DRM_ROTATE_180, "rotate-180" },
+				{ DRM_ROTATE_270, "rotate-270" },
+				{ DRM_REFLECT_X,  "reflect-x" },
+				{ DRM_REFLECT_Y,  "reflect-y" },
+		};
+		prop = drm_property_create_bitmask(dev, 0, "rotation",
+				props, ARRAY_SIZE(props));
+		if (prop == NULL)
+			return;
+		priv->rotation_prop = prop;
+	}
+	drm_object_attach_property(obj, prop, 0);
+
+        prop = priv->zorder_prop;
+        if (!prop) {
+		prop = drm_property_create_range(dev, 0, "zorder", 0, 3);
+		if (prop == NULL)
+			return;
+		priv->zorder_prop = prop;
+	}
+	drm_object_attach_property(obj, prop, 0);
+}
+
+int omap_plane_set_property(struct drm_plane *plane,
+		struct drm_property *property, uint64_t val)
+{
+	struct omap_plane *omap_plane = to_omap_plane(plane);
+	struct omap_drm_private *priv = plane->dev->dev_private;
+	int ret = -EINVAL;
+
+	if (property == priv->rotation_prop) {
+		struct omap_overlay *ovl = omap_plane->ovl;
+
+		DBG("%s: rotation: %02x", ovl->name, (uint32_t)val);
+		omap_plane->win.rotation = val;
+
+		if (ovl->is_enabled(ovl))
+			ret = omap_plane_dpms(plane, DRM_MODE_DPMS_ON);
+		else
+			ret = 0;
+	} else if (property == priv->zorder_prop) {
+		struct omap_overlay *ovl = omap_plane->ovl;
+
+		DBG("%s: zorder: %d", ovl->name, (uint32_t)val);
+		omap_plane->info.zorder = val;
+
+		if (ovl->is_enabled(ovl))
+			ret = omap_plane_dpms(plane, DRM_MODE_DPMS_ON);
+		else
+			ret = 0;
+	}
+
+	return ret;
+}
+
 static const struct drm_plane_funcs omap_plane_funcs = {
 		.update_plane = omap_plane_update,
 		.disable_plane = omap_plane_disable,
 		.destroy = omap_plane_destroy,
+		.set_property = omap_plane_set_property,
 };
 
 /* initialize plane */
@@ -455,6 +523,8 @@ struct drm_plane *omap_plane_init(struct drm_device *dev,
 	drm_plane_init(dev, plane, possible_crtcs, &omap_plane_funcs,
 			omap_plane->formats, omap_plane->nformats, priv);
 
+	omap_plane_install_properties(plane, &plane->base);
+
 	/* get our starting configuration, set defaults for parameters
 	 * we don't currently use, etc:
 	 */
@@ -463,7 +533,6 @@ struct drm_plane *omap_plane_init(struct drm_device *dev,
 	omap_plane->info.rotation = OMAP_DSS_ROT_0;
 	omap_plane->info.global_alpha = 0xff;
 	omap_plane->info.mirror = 0;
-	omap_plane->info.mirror = 0;
 
 	/* Set defaults depending on whether we are a CRTC or overlay
 	 * layer.
diff --git a/drivers/staging/ozwpan/ozcdev.c b/drivers/staging/ozwpan/ozcdev.c
index 758ce0a8d82e..64913aeb0bac 100644
--- a/drivers/staging/ozwpan/ozcdev.c
+++ b/drivers/staging/ozwpan/ozcdev.c
@@ -104,10 +104,10 @@ ssize_t oz_cdev_read(struct file *filp, char __user *buf, size_t count,
 	if (pd)
 		oz_pd_get(pd);
 	spin_unlock_bh(&g_cdev.lock);
-	if (pd == 0)
+	if (pd == NULL)
 		return -1;
 	ctx = oz_cdev_claim_ctx(pd);
-	if (ctx == 0)
+	if (ctx == NULL)
 		goto out2;
 	n = ctx->rd_in - ctx->rd_out;
 	if (n < 0)
@@ -157,11 +157,11 @@ ssize_t oz_cdev_write(struct file *filp, const char __user *buf, size_t count,
 	if (pd)
 		oz_pd_get(pd);
 	spin_unlock_bh(&g_cdev.lock);
-	if (pd == 0)
+	if (pd == NULL)
 		return -1;
 	eb = &pd->elt_buff;
 	ei = oz_elt_info_alloc(eb);
-	if (ei == 0) {
+	if (ei == NULL) {
 		count = 0;
 		goto out;
 	}
@@ -410,7 +410,7 @@ int oz_cdev_start(struct oz_pd *pd, int resume)
 		return 0;
 	}
 	ctx = kzalloc(sizeof(struct oz_serial_ctx), GFP_ATOMIC);
-	if (ctx == 0)
+	if (ctx == NULL)
 		return -ENOMEM;
 	atomic_set(&ctx->ref_count, 1);
 	ctx->tx_seq_num = 1;
@@ -424,7 +424,7 @@ int oz_cdev_start(struct oz_pd *pd, int resume)
 		spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
 	}
 	spin_lock(&g_cdev.lock);
-	if ((g_cdev.active_pd == 0) &&
+	if ((g_cdev.active_pd == NULL) &&
 		(memcmp(pd->mac_addr, g_cdev.active_addr, ETH_ALEN) == 0)) {
 		oz_pd_get(pd);
 		g_cdev.active_pd = pd;
@@ -477,7 +477,7 @@ void oz_cdev_rx(struct oz_pd *pd, struct oz_elt *elt)
 	int ix;
 
 	ctx = oz_cdev_claim_ctx(pd);
-	if (ctx == 0) {
+	if (ctx == NULL) {
 		oz_trace("Cannot claim serial context.\n");
 		return;
 	}
diff --git a/drivers/staging/ozwpan/ozevent.c b/drivers/staging/ozwpan/ozevent.c
index 7f66b4f19b01..a48498bd9b5f 100644
--- a/drivers/staging/ozwpan/ozevent.c
+++ b/drivers/staging/ozwpan/ozevent.c
@@ -14,7 +14,7 @@
 #include "ozappif.h"
 /*------------------------------------------------------------------------------
  * Although the event mask is logically part of the oz_evtdev structure, it is
- * needed outside of this file so define it seperately to avoid the need to
+ * needed outside of this file so define it separately to avoid the need to
  * export definition of struct oz_evtdev.
  */
 u32 g_evt_mask;
@@ -39,8 +39,8 @@ static struct oz_evtdev g_evtdev;
  */
 void oz_event_init(void)
 {
-	/* Because g_evtdev is static external all fields initally zero so no
-	 * need to reinitialised those.
+	/* Because g_evtdev is static external all fields initially zero so no
+	 * need to reinitialized those.
 	 */
 	oz_trace("Event tracing initialized\n");
 	spin_lock_init(&g_evtdev.lock);
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index 251f07c39a6b..2e087acf1578 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -417,6 +417,44 @@ static void oz_ep_free(struct oz_port *port, struct oz_endpoint *ep)
 /*------------------------------------------------------------------------------
  * Context: softirq
  */
+void oz_complete_buffered_urb(struct oz_port *port, struct oz_endpoint *ep,
+			struct urb *urb)
+{
+	u8 data_len, available_space, copy_len;
+
+	memcpy(&data_len, &ep->buffer[ep->out_ix], sizeof(u8));
+	if (data_len <= urb->transfer_buffer_length)
+		available_space = data_len;
+	else
+		available_space = urb->transfer_buffer_length;
+
+	if (++ep->out_ix == ep->buffer_size)
+		ep->out_ix = 0;
+	copy_len = ep->buffer_size - ep->out_ix;
+	if (copy_len >= available_space)
+		copy_len = available_space;
+	memcpy(urb->transfer_buffer, &ep->buffer[ep->out_ix], copy_len);
+
+	if (copy_len < available_space) {
+		memcpy((urb->transfer_buffer + copy_len), ep->buffer,
+						(available_space - copy_len));
+		ep->out_ix = available_space - copy_len;
+	} else {
+		ep->out_ix += copy_len;
+	}
+	urb->actual_length = available_space;
+	if (ep->out_ix == ep->buffer_size)
+		ep->out_ix = 0;
+
+	ep->buffered_units--;
+	oz_trace("Trying to give back buffered frame of size=%d\n",
+						available_space);
+	oz_complete_urb(port->ozhcd->hcd, urb, 0, 0);
+}
+
+/*------------------------------------------------------------------------------
+ * Context: softirq
+ */
 static int oz_enqueue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 			struct urb *urb, u8 req_id)
 {
@@ -452,6 +490,18 @@ static int oz_enqueue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 		ep = port->in_ep[ep_addr];
 	else
 		ep = port->out_ep[ep_addr];
+
+	/*For interrupt endpoint check for buffered data
+	* & complete urb
+	*/
+	if (((ep->attrib & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT)
+						 && ep->buffered_units > 0) {
+		oz_free_urb_link(urbl);
+		spin_unlock_bh(&port->ozhcd->hcd_lock);
+		oz_complete_buffered_urb(port, ep, urb);
+		return 0;
+	}
+
 	if (ep && port->hpd) {
 		list_add_tail(&urbl->link, &ep->urb_list);
 		if (!in_dir && ep_addr && (ep->credit < 0)) {
@@ -883,13 +933,14 @@ void oz_hcd_control_cnf(void *hport, u8 req_id, u8 rcode, u8 *data,
 	} else {
 		int copy_len;
 		oz_trace("VENDOR-CLASS - cnf\n");
-		if (data_len <= urb->transfer_buffer_length)
-			copy_len = data_len;
-		else
-			copy_len = urb->transfer_buffer_length;
-		if (copy_len)
+		if (data_len) {
+			if (data_len <= urb->transfer_buffer_length)
+				copy_len = data_len;
+			else
+				copy_len = urb->transfer_buffer_length;
 			memcpy(urb->transfer_buffer, data, copy_len);
-		urb->actual_length = copy_len;
+			urb->actual_length = copy_len;
+		}
 		oz_complete_urb(hcd, urb, 0, 0);
 	}
 }
@@ -961,6 +1012,9 @@ void oz_hcd_data_ind(void *hport, u8 endpoint, u8 *data, int data_len)
 			urb->actual_length = copy_len;
 			oz_complete_urb(port->ozhcd->hcd, urb, 0, 0);
 			return;
+		} else {
+			oz_trace("buffering frame as URB is not available\n");
+			oz_hcd_buffer_data(ep, data, data_len);
 		}
 		break;
 	case USB_ENDPOINT_XFER_ISOC:
@@ -1000,7 +1054,7 @@ int oz_hcd_heartbeat(void *hport)
 		ep = ep_from_link(e);
 		if (ep->credit < 0)
 			continue;
-		ep->credit += (now - ep->last_jiffies);
+		ep->credit += jiffies_to_msecs(now - ep->last_jiffies);
 		if (ep->credit > ep->credit_ceiling)
 			ep->credit = ep->credit_ceiling;
 		oz_event_log(OZ_EVT_EP_CREDIT, ep->ep_num, 0, 0, ep->credit);
@@ -1009,13 +1063,12 @@ int oz_hcd_heartbeat(void *hport)
 			urbl = list_first_entry(&ep->urb_list,
 				struct oz_urb_link, link);
 			urb = urbl->urb;
-			if (ep->credit < urb->number_of_packets)
+			if ((ep->credit + 1) < urb->number_of_packets)
 				break;
 			ep->credit -= urb->number_of_packets;
 			oz_event_log(OZ_EVT_EP_CREDIT, ep->ep_num, 0, 0,
 				ep->credit);
-			list_del(&urbl->link);
-			list_add_tail(&urbl->link, &xfr_list);
+			list_move_tail(&urbl->link, &xfr_list);
 		}
 	}
 	spin_unlock_bh(&ozhcd->hcd_lock);
@@ -1052,7 +1105,7 @@ int oz_hcd_heartbeat(void *hport)
 			}
 			continue;
 		}
-		ep->credit += (now - ep->last_jiffies);
+		ep->credit += jiffies_to_msecs(now - ep->last_jiffies);
 		oz_event_log(OZ_EVT_EP_CREDIT, ep->ep_num | USB_DIR_IN,
 			0, 0, ep->credit);
 		ep->last_jiffies = now;
@@ -1064,7 +1117,7 @@ int oz_hcd_heartbeat(void *hport)
 			int len = 0;
 			int copy_len;
 			int i;
-			if (ep->credit < urb->number_of_packets)
+			if ((ep->credit + 1) < urb->number_of_packets)
 				break;
 			if (ep->buffered_units < urb->number_of_packets)
 				break;
@@ -1096,8 +1149,7 @@ int oz_hcd_heartbeat(void *hport)
 			urb->error_count = 0;
 			urb->start_frame = ep->start_frame;
 			ep->start_frame += urb->number_of_packets;
-			list_del(&urbl->link);
-			list_add_tail(&urbl->link, &xfr_list);
+			list_move_tail(&urbl->link, &xfr_list);
 			ep->credit -= urb->number_of_packets;
 			oz_event_log(OZ_EVT_EP_CREDIT, ep->ep_num | USB_DIR_IN,
 				0, 0, ep->credit);
@@ -1129,8 +1181,7 @@ int oz_hcd_heartbeat(void *hport)
 				oz_trace("%ld: Request 0x%p timeout\n",
 						now, urbl->urb);
 				urbl->submit_jiffies = now;
-				list_del(e);
-				list_add_tail(e, &xfr_list);
+				list_move_tail(e, &xfr_list);
 			}
 		}
 		if (!list_empty(&ep->urb_list))
@@ -1167,10 +1218,16 @@ static int oz_build_endpoints_for_interface(struct usb_hcd *hcd,
 		int buffer_size = 0;
 
 		oz_trace("%d bEndpointAddress = %x\n", i, ep_addr);
-		if ((ep_addr & USB_ENDPOINT_DIR_MASK) &&
-			((hep->desc.bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-			== USB_ENDPOINT_XFER_ISOC)) {
-			buffer_size = 24*1024;
+		if (ep_addr & USB_ENDPOINT_DIR_MASK) {
+			switch (hep->desc.bmAttributes &
+						USB_ENDPOINT_XFERTYPE_MASK) {
+			case USB_ENDPOINT_XFER_ISOC:
+				buffer_size = 24*1024;
+				break;
+			case USB_ENDPOINT_XFER_INT:
+				buffer_size = 128;
+				break;
+			}
 		}
 
 		ep = oz_ep_alloc(mem_flags, buffer_size);
@@ -1247,16 +1304,14 @@ static void oz_clean_endpoints_for_interface(struct usb_hcd *hcd,
 			port->out_ep[i] = 0;
 			/* Remove from isoc list if present.
 			 */
-			list_del(e);
-			list_add_tail(e, &ep_list);
+			list_move_tail(e, &ep_list);
 		}
 		/* Gather IN endpoints.
 		 */
 		if ((mask & (1<<(i+OZ_NB_ENDPOINTS))) && port->in_ep[i]) {
 			e = &port->in_ep[i]->link;
 			port->in_ep[i] = 0;
-			list_del(e);
-			list_add_tail(e, &ep_list);
+			list_move_tail(e, &ep_list);
 		}
 	}
 	spin_unlock_bh(&ozhcd->hcd_lock);
@@ -1458,6 +1513,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 		int data_len = 0;
 		if ((setup->bRequestType & USB_DIR_IN) == 0)
 			data_len = wlength;
+		urb->actual_length = data_len;
 		if (oz_usb_control_req(port->hpd, req_id, setup,
 				urb->transfer_buffer, data_len)) {
 			rc = -ENOMEM;
diff --git a/drivers/staging/ozwpan/ozmain.c b/drivers/staging/ozwpan/ozmain.c
index c1ed6b2522ec..ef6c5ab753ee 100644
--- a/drivers/staging/ozwpan/ozmain.c
+++ b/drivers/staging/ozwpan/ozmain.c
@@ -59,6 +59,6 @@ module_exit(ozwpan_exit);
 
 MODULE_AUTHOR("Chris Kelly");
 MODULE_DESCRIPTION("Ozmo Devices USB over WiFi hcd driver");
-MODULE_VERSION("1.0.10");
+MODULE_VERSION("1.0.13");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/ozwpan/ozpd.c b/drivers/staging/ozwpan/ozpd.c
index 6c287ac6eaea..0b3648ce9687 100644
--- a/drivers/staging/ozwpan/ozpd.c
+++ b/drivers/staging/ozwpan/ozpd.c
@@ -24,12 +24,6 @@
 /*------------------------------------------------------------------------------
  */
 #define OZ_MAX_TX_POOL_SIZE	6
-/* Maximum number of uncompleted isoc frames that can be pending in network.
- */
-#define OZ_MAX_SUBMITTED_ISOC	16
-/* Maximum number of uncompleted isoc frames that can be pending in Tx Queue.
- */
-#define OZ_MAX_TX_QUEUE_ISOC	32
 /*------------------------------------------------------------------------------
  */
 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd);
@@ -752,8 +746,7 @@ int oz_isoc_stream_create(struct oz_pd *pd, u8 ep_num)
  */
 static void oz_isoc_stream_free(struct oz_isoc_stream *st)
 {
-	if (st->skb)
-		kfree_skb(st->skb);
+	kfree_skb(st->skb);
 	kfree(st);
 }
 /*------------------------------------------------------------------------------
@@ -854,7 +847,7 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, u8 *data, int len)
 		if (!(pd->mode & OZ_F_ISOC_ANYTIME)) {
 			struct oz_tx_frame *isoc_unit = NULL;
 			int nb = pd->nb_queued_isoc_frames;
-			if (nb >= OZ_MAX_TX_QUEUE_ISOC) {
+			if (nb >= pd->isoc_latency) {
 				oz_trace2(OZ_TRACE_TX_FRAMES,
 						"Dropping ISOC Unit nb= %d\n",
 									nb);
diff --git a/drivers/staging/ozwpan/ozpd.h b/drivers/staging/ozwpan/ozpd.h
index ddf1341b4e67..d35b0ea44f67 100644
--- a/drivers/staging/ozwpan/ozpd.h
+++ b/drivers/staging/ozwpan/ozpd.h
@@ -82,6 +82,7 @@ struct oz_pd {
 	u8		heartbeat_requested;
 	u8		mode;
 	u8		ms_per_isoc;
+	unsigned	isoc_latency;
 	unsigned	max_stream_buffering;
 	int		nb_queued_frames;
 	int		nb_queued_isoc_frames;
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index a50ab18a5987..cfb5160d1ebe 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -220,6 +220,19 @@ static struct oz_pd *oz_connect_req(struct oz_pd *cur_pd, struct oz_elt *elt,
 		pd->ms_per_isoc = body->ms_per_isoc;
 		if (!pd->ms_per_isoc)
 			pd->ms_per_isoc = 4;
+
+		switch (body->ms_isoc_latency & OZ_LATENCY_MASK) {
+		case OZ_ONE_MS_LATENCY:
+			pd->isoc_latency = (body->ms_isoc_latency &
+					~OZ_LATENCY_MASK) / pd->ms_per_isoc;
+			break;
+		case OZ_TEN_MS_LATENCY:
+			pd->isoc_latency = ((body->ms_isoc_latency &
+				~OZ_LATENCY_MASK) * 10) / pd->ms_per_isoc;
+			break;
+		default:
+			pd->isoc_latency = OZ_MAX_TX_QUEUE_ISOC;
+		}
 	}
 	if (body->max_len_div16)
 		pd->max_tx_size = ((u16)body->max_len_div16)<<4;
diff --git a/drivers/staging/ozwpan/ozproto.h b/drivers/staging/ozwpan/ozproto.h
index 89aea28bd8d5..755a08d0e1ca 100644
--- a/drivers/staging/ozwpan/ozproto.h
+++ b/drivers/staging/ozwpan/ozproto.h
@@ -14,7 +14,7 @@
 
 /* Converts millisecs to jiffies.
  */
-#define oz_ms_to_jiffies(__x)	(((__x)*1000)/HZ)
+#define oz_ms_to_jiffies(__x)	msecs_to_jiffies(__x)
 
 /* Quantum milliseconds.
  */
@@ -30,6 +30,12 @@
 /* Maximun sizes of tx frames. */
 #define OZ_MAX_TX_SIZE		1514
 
+/* Maximum number of uncompleted isoc frames that can be pending in network. */
+#define OZ_MAX_SUBMITTED_ISOC	16
+
+/* Maximum number of uncompleted isoc frames that can be pending in Tx Queue. */
+#define OZ_MAX_TX_QUEUE_ISOC	32
+
 /* Application handler functions.
  */
 typedef int (*oz_app_init_fn_t)(void);
diff --git a/drivers/staging/ozwpan/ozprotocol.h b/drivers/staging/ozwpan/ozprotocol.h
index 1e4edbeb61cd..17b09b9a5b08 100644
--- a/drivers/staging/ozwpan/ozprotocol.h
+++ b/drivers/staging/ozwpan/ozprotocol.h
@@ -65,6 +65,10 @@ struct oz_hdr {
 
 #define OZ_LAST_PN_HALF_CYCLE	127
 
+#define OZ_LATENCY_MASK		0xc0
+#define OZ_ONE_MS_LATENCY	0x40
+#define OZ_TEN_MS_LATENCY	0x80
+
 /* Connect request data structure.
  */
 struct oz_elt_connect_req {
@@ -73,7 +77,7 @@ struct oz_elt_connect_req {
 	u8	pd_info;
 	u8	session_id;
 	u8	presleep;
-	u8	resv2;
+	u8	ms_isoc_latency;
 	u8	host_vendor;
 	u8	keep_alive;
 	u16	apps;
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 39f9982c2708..6e9f7090c451 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -137,8 +137,8 @@
 #define r_ctr(x)        (parport_read_control((x)->port))
 #define r_dtr(x)        (parport_read_data((x)->port))
 #define r_str(x)        (parport_read_status((x)->port))
-#define w_ctr(x, y)     do { parport_write_control((x)->port, (y)); } while (0)
-#define w_dtr(x, y)     do { parport_write_data((x)->port, (y)); } while (0)
+#define w_ctr(x, y)     (parport_write_control((x)->port, (y)))
+#define w_dtr(x, y)     (parport_write_data((x)->port, (y)))
 
 /* this defines which bits are to be used and which ones to be ignored */
 /* logical or of the output bits involved in the scan matrix */
@@ -757,38 +757,38 @@ static void lcd_backlight(int on)
 		return;
 
 	/* The backlight is activated by setting the AUTOFEED line to +5V  */
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	bits.bl = on;
 	panel_set_bits();
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send a command to the LCD panel in serial mode */
 static void lcd_write_cmd_s(int cmd)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	lcd_send_serial(0x1F);	/* R/W=W, RS=0 */
 	lcd_send_serial(cmd & 0x0F);
 	lcd_send_serial((cmd >> 4) & 0x0F);
 	udelay(40);		/* the shortest command takes at least 40 us */
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send data to the LCD panel in serial mode */
 static void lcd_write_data_s(int data)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
 	lcd_send_serial(data & 0x0F);
 	lcd_send_serial((data >> 4) & 0x0F);
 	udelay(40);		/* the shortest data takes at least 40 us */
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send a command to the LCD panel in 8 bits parallel mode */
 static void lcd_write_cmd_p8(int cmd)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, cmd);
 	udelay(20);	/* maintain the data during 20 us before the strobe */
@@ -804,13 +804,13 @@ static void lcd_write_cmd_p8(int cmd)
 	set_ctrl_bits();
 
 	udelay(120);	/* the shortest command takes at least 120 us */
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send data to the LCD panel in 8 bits parallel mode */
 static void lcd_write_data_p8(int data)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, data);
 	udelay(20);	/* maintain the data during 20 us before the strobe */
@@ -826,27 +826,27 @@ static void lcd_write_data_p8(int data)
 	set_ctrl_bits();
 
 	udelay(45);	/* the shortest data takes at least 45 us */
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send a command to the TI LCD panel */
 static void lcd_write_cmd_tilcd(int cmd)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	/* present the data to the control port */
 	w_ctr(pprt, cmd);
 	udelay(60);
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 /* send data to the TI LCD panel */
 static void lcd_write_data_tilcd(int data)
 {
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, data);
 	udelay(60);
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 }
 
 static void lcd_gotoxy(void)
@@ -879,14 +879,14 @@ static void lcd_clear_fast_s(void)
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
 
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
 		lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
 		lcd_send_serial(' ' & 0x0F);
 		lcd_send_serial((' ' >> 4) & 0x0F);
 		udelay(40);	/* the shortest data takes at least 40 us */
 	}
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
@@ -899,7 +899,7 @@ static void lcd_clear_fast_p8(void)
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
 
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
 		/* present the data to the data port */
 		w_dtr(pprt, ' ');
@@ -921,7 +921,7 @@ static void lcd_clear_fast_p8(void)
 		/* the shortest data takes at least 45 us */
 		udelay(45);
 	}
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
@@ -934,14 +934,14 @@ static void lcd_clear_fast_tilcd(void)
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
 
-	spin_lock(&pprt_lock);
+	spin_lock_irq(&pprt_lock);
 	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
 		/* present the data to the data port */
 		w_dtr(pprt, ' ');
 		udelay(60);
 	}
 
-	spin_unlock(&pprt_lock);
+	spin_unlock_irq(&pprt_lock);
 
 	lcd_addr_x = lcd_addr_y = 0;
 	lcd_gotoxy();
@@ -1197,7 +1197,7 @@ static inline int handle_lcd_special_code(void)
 		break;
 	}
 
-	/* Check wether one flag was changed */
+	/* Check whether one flag was changed */
 	if (oldflags != lcd_flags) {
 		/* check whether one of B,C,D flags were changed */
 		if ((oldflags ^ lcd_flags) &
@@ -1212,7 +1212,7 @@ static inline int handle_lcd_special_code(void)
 			lcd_write_cmd(0x30
 				      | ((lcd_flags & LCD_FLAG_F) ? 4 : 0)
 				      | ((lcd_flags & LCD_FLAG_N) ? 8 : 0));
-		/* check wether L flag was changed */
+		/* check whether L flag was changed */
 		else if ((oldflags ^ lcd_flags) & (LCD_FLAG_L)) {
 			if (lcd_flags & (LCD_FLAG_L))
 				lcd_backlight(1);
@@ -1886,11 +1886,11 @@ static void panel_process_inputs(void)
 static void panel_scan_timer(void)
 {
 	if (keypad_enabled && keypad_initialized) {
-		if (spin_trylock(&pprt_lock)) {
+		if (spin_trylock_irq(&pprt_lock)) {
 			phys_scan_contacts();
 
 			/* no need for the parport anymore */
-			spin_unlock(&pprt_lock);
+			spin_unlock_irq(&pprt_lock);
 		}
 
 		if (!inputs_stable || phys_curr != phys_prev)
diff --git a/drivers/staging/ramster/Kconfig b/drivers/staging/ramster/Kconfig
index 8349887827dc..843c54101438 100644
--- a/drivers/staging/ramster/Kconfig
+++ b/drivers/staging/ramster/Kconfig
@@ -1,13 +1,30 @@
+config ZCACHE2
+	bool "Dynamic compression of swap pages and clean pagecache pages"
+	depends on CRYPTO=y && SWAP=y && CLEANCACHE && FRONTSWAP && !ZCACHE 
+	select CRYPTO_LZO
+	default n
+	help
+	  Zcache2 doubles RAM efficiency while providing a significant
+	  performance boosts on many workloads.  Zcache2 uses
+	  compression and an in-kernel implementation of transcendent
+	  memory to store clean page cache pages and swap in RAM,
+	  providing a noticeable reduction in disk I/O.  Zcache2
+	  is a complete rewrite of the older zcache; it was intended to
+	  be a merge but that has been blocked due to political and
+	  technical disagreements.  It is intended that they will merge
+	  again in the future.  Until then, zcache2 is a single-node
+	  version of ramster.
+
 config RAMSTER
 	bool "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
-	depends on (CLEANCACHE || FRONTSWAP) && CONFIGFS_FS=y && !ZCACHE && !XVMALLOC && !HIGHMEM && NET
-	select LZO_COMPRESS
-	select LZO_DECOMPRESS
+	depends on CONFIGFS_FS=y && SYSFS=y && !HIGHMEM && ZCACHE2=y
+	# must ensure struct page is 8-byte aligned
+	select HAVE_ALIGNED_STRUCT_PAGE if !64_BIT
 	default n
 	help
 	  RAMster allows RAM on other machines in a cluster to be utilized
 	  dynamically and symmetrically instead of swapping to a local swap
 	  disk, thus improving performance on memory-constrained workloads
 	  while minimizing total RAM across the cluster.  RAMster, like
-	  zcache, compresses swap pages into local RAM, but then remotifies
+	  zcache2, compresses swap pages into local RAM, but then remotifies
 	  the compressed pages to another node in the RAMster cluster.
diff --git a/drivers/staging/ramster/Makefile b/drivers/staging/ramster/Makefile
index bcc13c87f996..2d8b9d0a6a8b 100644
--- a/drivers/staging/ramster/Makefile
+++ b/drivers/staging/ramster/Makefile
@@ -1 +1,6 @@
-obj-$(CONFIG_RAMSTER)	+=	zcache-main.o tmem.o r2net.o xvmalloc.o cluster/
+zcache-y	:=		zcache-main.o tmem.o zbud.o
+zcache-$(CONFIG_RAMSTER)	+=	ramster/ramster.o ramster/r2net.o
+zcache-$(CONFIG_RAMSTER)	+=	ramster/nodemanager.o ramster/tcp.o
+zcache-$(CONFIG_RAMSTER)	+=	ramster/heartbeat.o ramster/masklog.o
+
+obj-$(CONFIG_ZCACHE2)	+=	zcache.o
diff --git a/drivers/staging/ramster/TODO b/drivers/staging/ramster/TODO
deleted file mode 100644
index 46fcf0c58acf..000000000000
--- a/drivers/staging/ramster/TODO
+++ /dev/null
@@ -1,13 +0,0 @@
-For this staging driver, RAMster duplicates code from drivers/staging/zcache
-then incorporates changes to the local copy of the code.  For V5, it also
-directly incorporates the soon-to-be-removed drivers/staging/zram/xvmalloc.[ch]
-as all testing has been done with xvmalloc rather than the new zsmalloc.
-Before RAMster can be promoted from staging, the zcache and RAMster drivers
-should be either merged or reorganized to separate out common code.
-
-Until V4, RAMster duplicated code from fs/ocfs2/cluster, but this made
-RAMster incompatible with ocfs2 running in the same kernel and included
-lots of code that could be removed.  As of V5, the ocfs2 code has been
-mined and made RAMster-specific, made to communicate with a userland
-ramster-tools package rather than ocfs2-tools, and can co-exist with ocfs2
-both in the same kernel and in userland on the same machine.
diff --git a/drivers/staging/ramster/cluster/Makefile b/drivers/staging/ramster/cluster/Makefile
deleted file mode 100644
index 9c6943652c01..000000000000
--- a/drivers/staging/ramster/cluster/Makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-obj-$(CONFIG_RAMSTER) += ramster_nodemanager.o
-
-ramster_nodemanager-objs := heartbeat.o masklog.o nodemanager.o tcp.o
diff --git a/drivers/staging/ramster/ramster.h b/drivers/staging/ramster/ramster.h
index 0c9455e8dcd8..1b71aea2ff62 100644
--- a/drivers/staging/ramster/ramster.h
+++ b/drivers/staging/ramster/ramster.h
@@ -1,118 +1,59 @@
+
 /*
- * ramster.h
+ * zcache/ramster.h
  *
- * Peer-to-peer transcendent memory
+ * Placeholder to resolve ramster references when !CONFIG_RAMSTER
+ * Real ramster.h lives in ramster subdirectory.
  *
  * Copyright (c) 2009-2012, Dan Magenheimer, Oracle Corp.
  */
 
-#ifndef _RAMSTER_H_
-#define _RAMSTER_H_
-
-/*
- * format of remote pampd:
- *   bit 0 == intransit
- *   bit 1 == is_remote... if this bit is set, then
- *   bit 2-9 == remotenode
- *   bit 10-22 == size
- *   bit 23-30 == cksum
- */
-#define FAKE_PAMPD_INTRANSIT_BITS	1
-#define FAKE_PAMPD_ISREMOTE_BITS	1
-#define FAKE_PAMPD_REMOTENODE_BITS	8
-#define FAKE_PAMPD_REMOTESIZE_BITS	13
-#define FAKE_PAMPD_CHECKSUM_BITS	8
-
-#define FAKE_PAMPD_INTRANSIT_SHIFT	0
-#define FAKE_PAMPD_ISREMOTE_SHIFT	(FAKE_PAMPD_INTRANSIT_SHIFT + \
-					 FAKE_PAMPD_INTRANSIT_BITS)
-#define FAKE_PAMPD_REMOTENODE_SHIFT	(FAKE_PAMPD_ISREMOTE_SHIFT + \
-					 FAKE_PAMPD_ISREMOTE_BITS)
-#define FAKE_PAMPD_REMOTESIZE_SHIFT	(FAKE_PAMPD_REMOTENODE_SHIFT + \
-					 FAKE_PAMPD_REMOTENODE_BITS)
-#define FAKE_PAMPD_CHECKSUM_SHIFT	(FAKE_PAMPD_REMOTESIZE_SHIFT + \
-					 FAKE_PAMPD_REMOTESIZE_BITS)
+#ifndef _ZCACHE_RAMSTER_H_
+#define _ZCACHE_RAMSTER_H_
 
-#define FAKE_PAMPD_MASK(x)		((1UL << (x)) - 1)
-
-static inline void *pampd_make_remote(int remotenode, size_t size,
-					unsigned char cksum)
+#ifdef CONFIG_RAMSTER
+#include "ramster/ramster.h"
+#else
+static inline void ramster_init(bool x, bool y, bool z)
 {
-	unsigned long fake_pampd = 0;
-	fake_pampd |= 1UL << FAKE_PAMPD_ISREMOTE_SHIFT;
-	fake_pampd |= ((unsigned long)remotenode &
-			FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTENODE_BITS)) <<
-				FAKE_PAMPD_REMOTENODE_SHIFT;
-	fake_pampd |= ((unsigned long)size &
-			FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTESIZE_BITS)) <<
-				FAKE_PAMPD_REMOTESIZE_SHIFT;
-	fake_pampd |= ((unsigned long)cksum &
-			FAKE_PAMPD_MASK(FAKE_PAMPD_CHECKSUM_BITS)) <<
-				FAKE_PAMPD_CHECKSUM_SHIFT;
-	return (void *)fake_pampd;
 }
 
-static inline unsigned int pampd_remote_node(void *pampd)
+static inline void ramster_register_pamops(struct tmem_pamops *p)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-	return (fake_pampd >> FAKE_PAMPD_REMOTENODE_SHIFT) &
-		FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTENODE_BITS);
 }
 
-static inline unsigned int pampd_remote_size(void *pampd)
+static inline int ramster_remotify_pageframe(bool b)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-	return (fake_pampd >> FAKE_PAMPD_REMOTESIZE_SHIFT) &
-		FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTESIZE_BITS);
+	return 0;
 }
 
-static inline unsigned char pampd_remote_cksum(void *pampd)
+static inline void *ramster_pampd_free(void *v, struct tmem_pool *p,
+			struct tmem_oid *o, uint32_t u, bool b)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-	return (fake_pampd >> FAKE_PAMPD_CHECKSUM_SHIFT) &
-		FAKE_PAMPD_MASK(FAKE_PAMPD_CHECKSUM_BITS);
+	return NULL;
 }
 
-static inline bool pampd_is_remote(void *pampd)
+static inline int ramster_do_preload_flnode(struct tmem_pool *p)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-	return (fake_pampd >> FAKE_PAMPD_ISREMOTE_SHIFT) &
-		FAKE_PAMPD_MASK(FAKE_PAMPD_ISREMOTE_BITS);
+	return -1;
 }
 
-static inline bool pampd_is_intransit(void *pampd)
+static inline bool pampd_is_remote(void *v)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-	return (fake_pampd >> FAKE_PAMPD_INTRANSIT_SHIFT) &
-		FAKE_PAMPD_MASK(FAKE_PAMPD_INTRANSIT_BITS);
+	return false;
 }
 
-/* note that it is a BUG for intransit to be set without isremote also set */
-static inline void *pampd_mark_intransit(void *pampd)
+static inline void ramster_count_foreign_pages(bool b, int i)
 {
-	unsigned long fake_pampd = (unsigned long)pampd;
-
-	fake_pampd |= 1UL << FAKE_PAMPD_ISREMOTE_SHIFT;
-	fake_pampd |= 1UL << FAKE_PAMPD_INTRANSIT_SHIFT;
-	return (void *)fake_pampd;
 }
 
-static inline void *pampd_mask_intransit_and_remote(void *marked_pampd)
+static inline void ramster_cpu_up(int cpu)
 {
-	unsigned long pampd = (unsigned long)marked_pampd;
-
-	pampd &= ~(1UL << FAKE_PAMPD_INTRANSIT_SHIFT);
-	pampd &= ~(1UL << FAKE_PAMPD_ISREMOTE_SHIFT);
-	return (void *)pampd;
 }
 
-extern int ramster_remote_async_get(struct tmem_xhandle *,
-				bool, int, size_t, uint8_t, void *extra);
-extern int ramster_remote_put(struct tmem_xhandle *, char *, size_t,
-				bool, int *);
-extern int ramster_remote_flush(struct tmem_xhandle *, int);
-extern int ramster_remote_flush_object(struct tmem_xhandle *, int);
-extern int r2net_register_handlers(void);
-extern int r2net_remote_target_node_set(int);
+static inline void ramster_cpu_down(int cpu)
+{
+}
+#endif
 
-#endif /* _TMEM_H */
+#endif /* _ZCACHE_RAMSTER_H */
diff --git a/drivers/staging/ramster/cluster/heartbeat.c b/drivers/staging/ramster/ramster/heartbeat.c
index 00209490756e..75d3fe80b055 100644
--- a/drivers/staging/ramster/cluster/heartbeat.c
+++ b/drivers/staging/ramster/ramster/heartbeat.c
@@ -172,8 +172,7 @@ static void r2hb_hb_group_drop_item(struct config_group *group,
 					   struct config_item *item)
 {
 	if (r2hb_global_heartbeat_active()) {
-		printk(KERN_NOTICE "ramster: Heartbeat %s "
-			"on region %s (%s)\n",
+		pr_notice("ramster: Heartbeat %s on region %s (%s)\n",
 			"stopped/aborted", config_item_name(item),
 			"no region");
 	}
@@ -265,8 +264,7 @@ ssize_t r2hb_hb_group_mode_store(struct r2hb_hb_group *group,
 
 		ret = r2hb_global_hearbeat_mode_set(i);
 		if (!ret)
-			printk(KERN_NOTICE "ramster: Heartbeat mode "
-						"set to %s\n",
+			pr_notice("ramster: Heartbeat mode set to %s\n",
 			       r2hb_heartbeat_mode_desc[i]);
 		return count;
 	}
diff --git a/drivers/staging/ramster/cluster/heartbeat.h b/drivers/staging/ramster/ramster/heartbeat.h
index 6cbc775bd63b..6cbc775bd63b 100644
--- a/drivers/staging/ramster/cluster/heartbeat.h
+++ b/drivers/staging/ramster/ramster/heartbeat.h
diff --git a/drivers/staging/ramster/cluster/masklog.c b/drivers/staging/ramster/ramster/masklog.c
index 1261d8579aae..1261d8579aae 100644
--- a/drivers/staging/ramster/cluster/masklog.c
+++ b/drivers/staging/ramster/ramster/masklog.c
diff --git a/drivers/staging/ramster/cluster/masklog.h b/drivers/staging/ramster/ramster/masklog.h
index 918ae110b699..918ae110b699 100644
--- a/drivers/staging/ramster/cluster/masklog.h
+++ b/drivers/staging/ramster/ramster/masklog.h
diff --git a/drivers/staging/ramster/cluster/nodemanager.c b/drivers/staging/ramster/ramster/nodemanager.c
index de0e5c8da6ea..c0f48158735d 100644
--- a/drivers/staging/ramster/cluster/nodemanager.c
+++ b/drivers/staging/ramster/ramster/nodemanager.c
@@ -528,7 +528,8 @@ static ssize_t r2nm_cluster_attr_idle_timeout_ms_write(
 			     r2net_num_connected_peers());
 			ret = -EINVAL;
 		} else if (val <= cluster->cl_keepalive_delay_ms) {
-			mlog(ML_NOTICE, "r2net: idle timeout must be larger "
+			mlog(ML_NOTICE,
+			     "r2net: idle timeout must be larger "
 			     "than keepalive delay\n");
 			ret = -EINVAL;
 		} else {
@@ -563,7 +564,8 @@ static ssize_t r2nm_cluster_attr_keepalive_delay_ms_write(
 			     r2net_num_connected_peers());
 			ret = -EINVAL;
 		} else if (val >= cluster->cl_idle_timeout_ms) {
-			mlog(ML_NOTICE, "r2net: keepalive delay must be "
+			mlog(ML_NOTICE,
+			     "r2net: keepalive delay must be "
 			     "smaller than idle timeout\n");
 			ret = -EINVAL;
 		} else {
@@ -612,7 +614,7 @@ static ssize_t r2nm_cluster_attr_fence_method_write(
 		if (strncasecmp(page, r2nm_fence_method_desc[i], count - 1))
 			continue;
 		if (cluster->cl_fence_method != i) {
-			printk(KERN_INFO "ramster: Changing fence method to %s\n",
+			pr_info("ramster: Changing fence method to %s\n",
 			       r2nm_fence_method_desc[i]);
 			cluster->cl_fence_method = i;
 		}
@@ -967,7 +969,7 @@ static int __init init_r2nm(void)
 	mutex_init(&r2nm_cluster_group.cs_subsys.su_mutex);
 	ret = configfs_register_subsystem(&r2nm_cluster_group.cs_subsys);
 	if (ret) {
-		printk(KERN_ERR "nodemanager: Registration returned %d\n", ret);
+		pr_err("nodemanager: Registration returned %d\n", ret);
 		goto out_callbacks;
 	}
 
@@ -988,5 +990,6 @@ out:
 MODULE_AUTHOR("Oracle");
 MODULE_LICENSE("GPL");
 
-module_init(init_r2nm)
-module_exit(exit_r2nm)
+/* module_init(init_r2nm) */
+late_initcall(init_r2nm);
+/* module_exit(exit_r2nm) */
diff --git a/drivers/staging/ramster/cluster/nodemanager.h b/drivers/staging/ramster/ramster/nodemanager.h
index 41a04df5842c..41a04df5842c 100644
--- a/drivers/staging/ramster/cluster/nodemanager.h
+++ b/drivers/staging/ramster/ramster/nodemanager.h
diff --git a/drivers/staging/ramster/r2net.c b/drivers/staging/ramster/ramster/r2net.c
index fc830c3ac74b..34818dc65612 100644
--- a/drivers/staging/ramster/r2net.c
+++ b/drivers/staging/ramster/ramster/r2net.c
@@ -1,7 +1,7 @@
 /*
  * r2net.c
  *
- * Copyright (c) 2011, Dan Magenheimer, Oracle Corp.
+ * Copyright (c) 2011-2012, Dan Magenheimer, Oracle Corp.
  *
  * Ramster_r2net provides an interface between zcache and r2net.
  *
@@ -9,10 +9,10 @@
  */
 
 #include <linux/list.h>
-#include "cluster/tcp.h"
-#include "cluster/nodemanager.h"
-#include "tmem.h"
-#include "zcache.h"
+#include "tcp.h"
+#include "nodemanager.h"
+#include "../tmem.h"
+#include "../zcache.h"
 #include "ramster.h"
 
 #define RAMSTER_TESTING
@@ -33,7 +33,7 @@ enum {
 #define RMSTR_R2NET_MAX_LEN \
 		(R2NET_MAX_PAYLOAD_BYTES - sizeof(struct tmem_xhandle))
 
-#include "cluster/tcp_internal.h"
+#include "tcp_internal.h"
 
 static struct r2nm_node *r2net_target_node;
 static int r2net_target_nodenum;
@@ -72,8 +72,8 @@ static int ramster_remote_async_get_request_handler(struct r2net_msg *msg,
 	*(struct tmem_xhandle *)pdata = xh;
 	pdata += sizeof(struct tmem_xhandle);
 	local_irq_save(flags);
-	found = zcache_get(xh.client_id, xh.pool_id, &xh.oid, xh.index,
-				pdata, &size, 1, get_and_free ? 1 : -1);
+	found = zcache_get_page(xh.client_id, xh.pool_id, &xh.oid, xh.index,
+				pdata, &size, true, get_and_free ? 1 : -1);
 	local_irq_restore(flags);
 	if (found < 0) {
 		/* a zero size indicates the get failed */
@@ -98,7 +98,7 @@ static int ramster_remote_async_get_reply_handler(struct r2net_msg *msg,
 	in += sizeof(struct tmem_xhandle);
 	datalen -= sizeof(struct tmem_xhandle);
 	BUG_ON(datalen < 0 || datalen > PAGE_SIZE);
-	ret = zcache_localify(xh->pool_id, &xh->oid, xh->index,
+	ret = ramster_localify(xh->pool_id, &xh->oid, xh->index,
 				in, datalen, xh->extra);
 #ifdef RAMSTER_TESTING
 	if (ret == -EEXIST)
@@ -123,8 +123,8 @@ int ramster_remote_put_handler(struct r2net_msg *msg,
 	p += sizeof(struct tmem_xhandle);
 	zcache_autocreate_pool(xh->client_id, xh->pool_id, ephemeral);
 	local_irq_save(flags);
-	ret = zcache_put(xh->client_id, xh->pool_id, &xh->oid, xh->index,
-				p, datalen, 1, ephemeral ? 1 : -1);
+	ret = zcache_put_page(xh->client_id, xh->pool_id, &xh->oid, xh->index,
+				p, datalen, true, ephemeral);
 	local_irq_restore(flags);
 	return ret;
 }
@@ -137,7 +137,8 @@ int ramster_remote_flush_handler(struct r2net_msg *msg,
 
 	xh = (struct tmem_xhandle *)p;
 	p += sizeof(struct tmem_xhandle);
-	(void)zcache_flush(xh->client_id, xh->pool_id, &xh->oid, xh->index);
+	(void)zcache_flush_page(xh->client_id, xh->pool_id,
+					&xh->oid, xh->index);
 	return 0;
 }
 
@@ -153,15 +154,16 @@ int ramster_remote_flobj_handler(struct r2net_msg *msg,
 	return 0;
 }
 
-int ramster_remote_async_get(struct tmem_xhandle *xh, bool free, int remotenode,
+int r2net_remote_async_get(struct tmem_xhandle *xh, bool free, int remotenode,
 				size_t expect_size, uint8_t expect_cksum,
 				void *extra)
 {
-	int ret = -1, status;
+	int nodenum, ret = -1, status;
 	struct r2nm_node *node = NULL;
 	struct kvec vec[1];
 	size_t veclen = 1;
 	u32 msg_type;
+	struct r2net_node *nn;
 
 	node = r2nm_get_node_by_num(remotenode);
 	if (node == NULL)
@@ -172,6 +174,21 @@ int ramster_remote_async_get(struct tmem_xhandle *xh, bool free, int remotenode,
 	xh->extra = extra;
 	vec[0].iov_len = sizeof(*xh);
 	vec[0].iov_base = xh;
+
+	node = r2net_target_node;
+	if (!node)
+		goto out;
+
+	nodenum = r2net_target_nodenum;
+
+	r2nm_node_get(node);
+	nn = r2net_nn_from_num(nodenum);
+	if (nn->nn_persistent_error || !nn->nn_sc_valid) {
+		ret = -ENOTCONN;
+		r2nm_node_put(node);
+		goto out;
+	}
+
 	if (free)
 		msg_type = RMSTR_TMEM_ASYNC_GET_AND_FREE_REQUEST;
 	else
@@ -180,8 +197,13 @@ int ramster_remote_async_get(struct tmem_xhandle *xh, bool free, int remotenode,
 					vec, veclen, remotenode, &status);
 	r2nm_node_put(node);
 	if (ret < 0) {
+		if (ret == -ENOTCONN || ret == -EHOSTDOWN)
+			goto out;
+		if (ret == -EAGAIN)
+			goto out;
 		/* FIXME handle bad message possibilities here? */
-		pr_err("UNTESTED ret<0 in ramster_remote_async_get\n");
+		pr_err("UNTESTED ret<0 in ramster_remote_async_get: ret=%d\n",
+				ret);
 	}
 	ret = status;
 out:
@@ -219,7 +241,7 @@ static void ramster_check_irq_counts(void)
 }
 #endif
 
-int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
+int r2net_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 				bool ephemeral, int *remotenode)
 {
 	int nodenum, ret = -1, status;
@@ -227,9 +249,7 @@ int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 	struct kvec vec[2];
 	size_t veclen = 2;
 	u32 msg_type;
-#ifdef RAMSTER_TESTING
 	struct r2net_node *nn;
-#endif
 
 	BUG_ON(size > RMSTR_R2NET_MAX_LEN);
 	xh->client_id = r2nm_this_node(); /* which node is putting */
@@ -237,6 +257,7 @@ int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 	vec[0].iov_base = xh;
 	vec[1].iov_len = size;
 	vec[1].iov_base = data;
+
 	node = r2net_target_node;
 	if (!node)
 		goto out;
@@ -245,10 +266,12 @@ int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 
 	r2nm_node_get(node);
 
-#ifdef RAMSTER_TESTING
 	nn = r2net_nn_from_num(nodenum);
-	WARN_ON_ONCE(nn->nn_persistent_error || !nn->nn_sc_valid);
-#endif
+	if (nn->nn_persistent_error || !nn->nn_sc_valid) {
+		ret = -ENOTCONN;
+		r2nm_node_put(node);
+		goto out;
+	}
 
 	if (ephemeral)
 		msg_type = RMSTR_TMEM_PUT_EPH;
@@ -261,16 +284,6 @@ int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 
 	ret = r2net_send_message_vec(msg_type, RMSTR_KEY, vec, veclen,
 						nodenum, &status);
-#ifdef RAMSTER_TESTING
-	if (ret != 0) {
-		static unsigned long cnt;
-		cnt++;
-		if (!(cnt&(cnt-1)))
-			pr_err("ramster_remote_put: message failed, ret=%d, cnt=%lu\n",
-				ret, cnt);
-		ret = -1;
-	}
-#endif
 	if (ret < 0)
 		ret = -1;
 	else {
@@ -283,7 +296,7 @@ out:
 	return ret;
 }
 
-int ramster_remote_flush(struct tmem_xhandle *xh, int remotenode)
+int r2net_remote_flush(struct tmem_xhandle *xh, int remotenode)
 {
 	int ret = -1, status;
 	struct r2nm_node *node = NULL;
@@ -303,7 +316,7 @@ int ramster_remote_flush(struct tmem_xhandle *xh, int remotenode)
 	return ret;
 }
 
-int ramster_remote_flush_object(struct tmem_xhandle *xh, int remotenode)
+int r2net_remote_flush_object(struct tmem_xhandle *xh, int remotenode)
 {
 	int ret = -1, status;
 	struct r2nm_node *node = NULL;
diff --git a/drivers/staging/ramster/ramster/ramster.c b/drivers/staging/ramster/ramster/ramster.c
new file mode 100644
index 000000000000..c06709f39682
--- /dev/null
+++ b/drivers/staging/ramster/ramster/ramster.c
@@ -0,0 +1,985 @@
+/*
+ * ramster.c
+ *
+ * Copyright (c) 2010-2012, Dan Magenheimer, Oracle Corp.
+ *
+ * RAMster implements peer-to-peer transcendent memory, allowing a "cluster" of
+ * kernels to dynamically pool their RAM so that a RAM-hungry workload on one
+ * machine can temporarily and transparently utilize RAM on another machine
+ * which is presumably idle or running a non-RAM-hungry workload.
+ *
+ * RAMster combines a clustering and messaging foundation based on the ocfs2
+ * cluster layer with the in-kernel compression implementation of zcache, and
+ * adds code to glue them together.  When a page is "put" to RAMster, it is
+ * compressed and stored locally.  Periodically, a thread will "remotify" these
+ * pages by sending them via messages to a remote machine.  When the page is
+ * later needed as indicated by a page fault, a "get" is issued.  If the data
+ * is local, it is uncompressed and the fault is resolved.  If the data is
+ * remote, a message is sent to fetch the data and the faulting thread sleeps;
+ * when the data arrives, the thread awakens, the data is decompressed and
+ * the fault is resolved.
+
+ * As of V5, clusters up to eight nodes are supported; each node can remotify
+ * pages to one specified node, so clusters can be configured as clients to
+ * a "memory server".  Some simple policy is in place that will need to be
+ * refined over time.  Larger clusters and fault-resistant protocols can also
+ * be added over time.
+ */
+
+#include <linux/module.h>
+#include <linux/cpu.h>
+#include <linux/highmem.h>
+#include <linux/list.h>
+#include <linux/lzo.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/atomic.h>
+#include <linux/frontswap.h>
+#include "../tmem.h"
+#include "../zcache.h"
+#include "../zbud.h"
+#include "ramster.h"
+#include "ramster_nodemanager.h"
+#include "tcp.h"
+
+#define RAMSTER_TESTING
+
+#ifndef CONFIG_SYSFS
+#error "ramster needs sysfs to define cluster nodes to use"
+#endif
+
+static bool use_cleancache __read_mostly;
+static bool use_frontswap __read_mostly;
+static bool use_frontswap_exclusive_gets __read_mostly;
+
+/* These must be sysfs not debugfs as they are checked/used by userland!! */
+static unsigned long ramster_interface_revision __read_mostly =
+	R2NM_API_VERSION; /* interface revision must match userspace! */
+static unsigned long ramster_pers_remotify_enable __read_mostly;
+static unsigned long ramster_eph_remotify_enable __read_mostly;
+static atomic_t ramster_remote_pers_pages = ATOMIC_INIT(0);
+#define MANUAL_NODES 8
+static bool ramster_nodes_manual_up[MANUAL_NODES] __read_mostly;
+static int ramster_remote_target_nodenum __read_mostly = -1;
+
+/* these counters are made available via debugfs */
+static long ramster_flnodes;
+static atomic_t ramster_flnodes_atomic = ATOMIC_INIT(0);
+static unsigned long ramster_flnodes_max;
+static long ramster_foreign_eph_pages;
+static atomic_t ramster_foreign_eph_pages_atomic = ATOMIC_INIT(0);
+static unsigned long ramster_foreign_eph_pages_max;
+static long ramster_foreign_pers_pages;
+static atomic_t ramster_foreign_pers_pages_atomic = ATOMIC_INIT(0);
+static unsigned long ramster_foreign_pers_pages_max;
+static unsigned long ramster_eph_pages_remoted;
+static unsigned long ramster_pers_pages_remoted;
+static unsigned long ramster_eph_pages_remote_failed;
+static unsigned long ramster_pers_pages_remote_failed;
+static unsigned long ramster_remote_eph_pages_succ_get;
+static unsigned long ramster_remote_pers_pages_succ_get;
+static unsigned long ramster_remote_eph_pages_unsucc_get;
+static unsigned long ramster_remote_pers_pages_unsucc_get;
+static unsigned long ramster_pers_pages_remote_nomem;
+static unsigned long ramster_remote_objects_flushed;
+static unsigned long ramster_remote_object_flushes_failed;
+static unsigned long ramster_remote_pages_flushed;
+static unsigned long ramster_remote_page_flushes_failed;
+/* FIXME frontswap selfshrinking knobs in debugfs? */
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#define	zdfs	debugfs_create_size_t
+#define	zdfs64	debugfs_create_u64
+static int __init ramster_debugfs_init(void)
+{
+	struct dentry *root = debugfs_create_dir("ramster", NULL);
+	if (root == NULL)
+		return -ENXIO;
+
+	zdfs("eph_pages_remoted", S_IRUGO, root, &ramster_eph_pages_remoted);
+	zdfs("pers_pages_remoted", S_IRUGO, root, &ramster_pers_pages_remoted);
+	zdfs("eph_pages_remote_failed", S_IRUGO, root,
+			&ramster_eph_pages_remote_failed);
+	zdfs("pers_pages_remote_failed", S_IRUGO, root,
+			&ramster_pers_pages_remote_failed);
+	zdfs("remote_eph_pages_succ_get", S_IRUGO, root,
+			&ramster_remote_eph_pages_succ_get);
+	zdfs("remote_pers_pages_succ_get", S_IRUGO, root,
+			&ramster_remote_pers_pages_succ_get);
+	zdfs("remote_eph_pages_unsucc_get", S_IRUGO, root,
+			&ramster_remote_eph_pages_unsucc_get);
+	zdfs("remote_pers_pages_unsucc_get", S_IRUGO, root,
+			&ramster_remote_pers_pages_unsucc_get);
+	zdfs("pers_pages_remote_nomem", S_IRUGO, root,
+			&ramster_pers_pages_remote_nomem);
+	zdfs("remote_objects_flushed", S_IRUGO, root,
+			&ramster_remote_objects_flushed);
+	zdfs("remote_pages_flushed", S_IRUGO, root,
+			&ramster_remote_pages_flushed);
+	zdfs("remote_object_flushes_failed", S_IRUGO, root,
+			&ramster_remote_object_flushes_failed);
+	zdfs("remote_page_flushes_failed", S_IRUGO, root,
+			&ramster_remote_page_flushes_failed);
+	zdfs("foreign_eph_pages", S_IRUGO, root,
+			&ramster_foreign_eph_pages);
+	zdfs("foreign_eph_pages_max", S_IRUGO, root,
+			&ramster_foreign_eph_pages_max);
+	zdfs("foreign_pers_pages", S_IRUGO, root,
+			&ramster_foreign_pers_pages);
+	zdfs("foreign_pers_pages_max", S_IRUGO, root,
+			&ramster_foreign_pers_pages_max);
+	return 0;
+}
+#undef	zdebugfs
+#undef	zdfs64
+#endif
+
+static LIST_HEAD(ramster_rem_op_list);
+static DEFINE_SPINLOCK(ramster_rem_op_list_lock);
+static DEFINE_PER_CPU(struct ramster_preload, ramster_preloads);
+
+static DEFINE_PER_CPU(unsigned char *, ramster_remoteputmem1);
+static DEFINE_PER_CPU(unsigned char *, ramster_remoteputmem2);
+
+static struct kmem_cache *ramster_flnode_cache __read_mostly;
+
+static struct flushlist_node *ramster_flnode_alloc(struct tmem_pool *pool)
+{
+	struct flushlist_node *flnode = NULL;
+	struct ramster_preload *kp;
+
+	kp = &__get_cpu_var(ramster_preloads);
+	flnode = kp->flnode;
+	BUG_ON(flnode == NULL);
+	kp->flnode = NULL;
+	ramster_flnodes = atomic_inc_return(&ramster_flnodes_atomic);
+	if (ramster_flnodes > ramster_flnodes_max)
+		ramster_flnodes_max = ramster_flnodes;
+	return flnode;
+}
+
+/* the "flush list" asynchronously collects pages to remotely flush */
+#define FLUSH_ENTIRE_OBJECT ((uint32_t)-1)
+static void ramster_flnode_free(struct flushlist_node *flnode,
+				struct tmem_pool *pool)
+{
+	int flnodes;
+
+	flnodes = atomic_dec_return(&ramster_flnodes_atomic);
+	BUG_ON(flnodes < 0);
+	kmem_cache_free(ramster_flnode_cache, flnode);
+}
+
+int ramster_do_preload_flnode(struct tmem_pool *pool)
+{
+	struct ramster_preload *kp;
+	struct flushlist_node *flnode;
+	int ret = -ENOMEM;
+
+	BUG_ON(!irqs_disabled());
+	if (unlikely(ramster_flnode_cache == NULL))
+		BUG();
+	kp = &__get_cpu_var(ramster_preloads);
+	flnode = kmem_cache_alloc(ramster_flnode_cache, GFP_ATOMIC);
+	if (unlikely(flnode == NULL) && kp->flnode == NULL)
+		BUG();  /* FIXME handle more gracefully, but how??? */
+	else if (kp->flnode == NULL)
+		kp->flnode = flnode;
+	else
+		kmem_cache_free(ramster_flnode_cache, flnode);
+	return ret;
+}
+
+/*
+ * Called by the message handler after a (still compressed) page has been
+ * fetched from the remote machine in response to an "is_remote" tmem_get
+ * or persistent tmem_localify.  For a tmem_get, "extra" is the address of
+ * the page that is to be filled to successfully resolve the tmem_get; for
+ * a (persistent) tmem_localify, "extra" is NULL (as the data is placed only
+ * in the local zcache).  "data" points to "size" bytes of (compressed) data
+ * passed in the message.  In the case of a persistent remote get, if
+ * pre-allocation was successful (see ramster_repatriate_preload), the page
+ * is placed into both local zcache and at "extra".
+ */
+int ramster_localify(int pool_id, struct tmem_oid *oidp, uint32_t index,
+			char *data, unsigned int size, void *extra)
+{
+	int ret = -ENOENT;
+	unsigned long flags;
+	struct tmem_pool *pool;
+	bool eph, delete = false;
+	void *pampd, *saved_hb;
+	struct tmem_obj *obj;
+
+	pool = zcache_get_pool_by_id(LOCAL_CLIENT, pool_id);
+	if (unlikely(pool == NULL))
+		/* pool doesn't exist anymore */
+		goto out;
+	eph = is_ephemeral(pool);
+	local_irq_save(flags);  /* FIXME: maybe only disable softirqs? */
+	pampd = tmem_localify_get_pampd(pool, oidp, index, &obj, &saved_hb);
+	if (pampd == NULL) {
+		/* hmmm... must have been a flush while waiting */
+#ifdef RAMSTER_TESTING
+		pr_err("UNTESTED pampd==NULL in ramster_localify\n");
+#endif
+		if (eph)
+			ramster_remote_eph_pages_unsucc_get++;
+		else
+			ramster_remote_pers_pages_unsucc_get++;
+		obj = NULL;
+		goto finish;
+	} else if (unlikely(!pampd_is_remote(pampd))) {
+		/* hmmm... must have been a dup put while waiting */
+#ifdef RAMSTER_TESTING
+		pr_err("UNTESTED dup while waiting in ramster_localify\n");
+#endif
+		if (eph)
+			ramster_remote_eph_pages_unsucc_get++;
+		else
+			ramster_remote_pers_pages_unsucc_get++;
+		obj = NULL;
+		pampd = NULL;
+		ret = -EEXIST;
+		goto finish;
+	} else if (size == 0) {
+		/* no remote data, delete the local is_remote pampd */
+		pampd = NULL;
+		if (eph)
+			ramster_remote_eph_pages_unsucc_get++;
+		else
+			BUG();
+		delete = true;
+		goto finish;
+	}
+	if (pampd_is_intransit(pampd)) {
+		/*
+		 *  a pampd is marked intransit if it is remote and space has
+		 *  been allocated for it locally (note, only happens for
+		 *  persistent pages, in which case the remote copy is freed)
+		 */
+		BUG_ON(eph);
+		pampd = pampd_mask_intransit_and_remote(pampd);
+		zbud_copy_to_zbud(pampd, data, size);
+	} else {
+		/*
+		 * setting pampd to NULL tells tmem_localify_finish to leave
+		 * pampd alone... meaning it is left pointing to the
+		 * remote copy
+		 */
+		pampd = NULL;
+		obj = NULL;
+	}
+	/*
+	 * but in all cases, we decompress direct-to-memory to complete
+	 * the remotify and return success
+	 */
+	BUG_ON(extra == NULL);
+	zcache_decompress_to_page(data, size, (struct page *)extra);
+	if (eph)
+		ramster_remote_eph_pages_succ_get++;
+	else
+		ramster_remote_pers_pages_succ_get++;
+	ret = 0;
+finish:
+	tmem_localify_finish(obj, index, pampd, saved_hb, delete);
+	zcache_put_pool(pool);
+	local_irq_restore(flags);
+out:
+	return ret;
+}
+
+void ramster_pampd_new_obj(struct tmem_obj *obj)
+{
+	obj->extra = NULL;
+}
+
+void ramster_pampd_free_obj(struct tmem_pool *pool, struct tmem_obj *obj,
+				bool pool_destroy)
+{
+	struct flushlist_node *flnode;
+
+	BUG_ON(preemptible());
+	if (obj->extra == NULL)
+		return;
+	if (pool_destroy && is_ephemeral(pool))
+		/* FIXME don't bother with remote eph data for now */
+		return;
+	BUG_ON(!pampd_is_remote(obj->extra));
+	flnode = ramster_flnode_alloc(pool);
+	flnode->xh.client_id = pampd_remote_node(obj->extra);
+	flnode->xh.pool_id = pool->pool_id;
+	flnode->xh.oid = obj->oid;
+	flnode->xh.index = FLUSH_ENTIRE_OBJECT;
+	flnode->rem_op.op = RAMSTER_REMOTIFY_FLUSH_OBJ;
+	spin_lock(&ramster_rem_op_list_lock);
+	list_add(&flnode->rem_op.list, &ramster_rem_op_list);
+	spin_unlock(&ramster_rem_op_list_lock);
+}
+
+/*
+ * Called on a remote persistent tmem_get to attempt to preallocate
+ * local storage for the data contained in the remote persistent page.
+ * If successfully preallocated, returns the pampd, marked as remote and
+ * in_transit.  Else returns NULL.  Note that the appropriate tmem data
+ * structure must be locked.
+ */
+void *ramster_pampd_repatriate_preload(void *pampd, struct tmem_pool *pool,
+					struct tmem_oid *oidp, uint32_t index,
+					bool *intransit)
+{
+	int clen = pampd_remote_size(pampd), c;
+	void *ret_pampd = NULL;
+	unsigned long flags;
+	struct tmem_handle th;
+
+	BUG_ON(!pampd_is_remote(pampd));
+	BUG_ON(is_ephemeral(pool));
+	if (use_frontswap_exclusive_gets)
+		/* don't need local storage */
+		goto out;
+	if (pampd_is_intransit(pampd)) {
+		/*
+		 * to avoid multiple allocations (and maybe a memory leak)
+		 * don't preallocate if already in the process of being
+		 * repatriated
+		 */
+		*intransit = true;
+		goto out;
+	}
+	*intransit = false;
+	local_irq_save(flags);
+	th.client_id = pampd_remote_node(pampd);
+	th.pool_id = pool->pool_id;
+	th.oid = *oidp;
+	th.index = index;
+	ret_pampd = zcache_pampd_create(NULL, clen, true, false, &th);
+	if (ret_pampd != NULL) {
+		/*
+		 *  a pampd is marked intransit if it is remote and space has
+		 *  been allocated for it locally (note, only happens for
+		 *  persistent pages, in which case the remote copy is freed)
+		 */
+		ret_pampd = pampd_mark_intransit(ret_pampd);
+		c = atomic_dec_return(&ramster_remote_pers_pages);
+		WARN_ON_ONCE(c < 0);
+	} else {
+		ramster_pers_pages_remote_nomem++;
+	}
+	local_irq_restore(flags);
+out:
+	return ret_pampd;
+}
+
+/*
+ * Called on a remote tmem_get to invoke a message to fetch the page.
+ * Might sleep so no tmem locks can be held.  "extra" is passed
+ * all the way through the round-trip messaging to ramster_localify.
+ */
+int ramster_pampd_repatriate(void *fake_pampd, void *real_pampd,
+				struct tmem_pool *pool,
+				struct tmem_oid *oid, uint32_t index,
+				bool free, void *extra)
+{
+	struct tmem_xhandle xh;
+	int ret;
+
+	if (pampd_is_intransit(real_pampd))
+		/* have local space pre-reserved, so free remote copy */
+		free = true;
+	xh = tmem_xhandle_fill(LOCAL_CLIENT, pool, oid, index);
+	/* unreliable request/response for now */
+	ret = r2net_remote_async_get(&xh, free,
+					pampd_remote_node(fake_pampd),
+					pampd_remote_size(fake_pampd),
+					pampd_remote_cksum(fake_pampd),
+					extra);
+	return ret;
+}
+
+bool ramster_pampd_is_remote(void *pampd)
+{
+	return pampd_is_remote(pampd);
+}
+
+int ramster_pampd_replace_in_obj(void *new_pampd, struct tmem_obj *obj)
+{
+	int ret = -1;
+
+	if (new_pampd != NULL) {
+		if (obj->extra == NULL)
+			obj->extra = new_pampd;
+		/* enforce that all remote pages in an object reside
+		 * in the same node! */
+		else if (pampd_remote_node(new_pampd) !=
+				pampd_remote_node((void *)(obj->extra)))
+			BUG();
+		ret = 0;
+	}
+	return ret;
+}
+
+void *ramster_pampd_free(void *pampd, struct tmem_pool *pool,
+			      struct tmem_oid *oid, uint32_t index, bool acct)
+{
+	bool eph = is_ephemeral(pool);
+	void *local_pampd = NULL;
+	int c;
+
+	BUG_ON(preemptible());
+	BUG_ON(!pampd_is_remote(pampd));
+	WARN_ON(acct == false);
+	if (oid == NULL) {
+		/*
+		 * a NULL oid means to ignore this pampd free
+		 * as the remote freeing will be handled elsewhere
+		 */
+	} else if (eph) {
+		/* FIXME remote flush optional but probably good idea */
+	} else if (pampd_is_intransit(pampd)) {
+		/* did a pers remote get_and_free, so just free local */
+		local_pampd = pampd_mask_intransit_and_remote(pampd);
+	} else {
+		struct flushlist_node *flnode =
+			ramster_flnode_alloc(pool);
+
+		flnode->xh.client_id = pampd_remote_node(pampd);
+		flnode->xh.pool_id = pool->pool_id;
+		flnode->xh.oid = *oid;
+		flnode->xh.index = index;
+		flnode->rem_op.op = RAMSTER_REMOTIFY_FLUSH_PAGE;
+		spin_lock(&ramster_rem_op_list_lock);
+		list_add(&flnode->rem_op.list, &ramster_rem_op_list);
+		spin_unlock(&ramster_rem_op_list_lock);
+		c = atomic_dec_return(&ramster_remote_pers_pages);
+		WARN_ON_ONCE(c < 0);
+	}
+	return local_pampd;
+}
+
+void ramster_count_foreign_pages(bool eph, int count)
+{
+	int c;
+
+	BUG_ON(count != 1 && count != -1);
+	if (eph) {
+		if (count > 0) {
+			c = atomic_inc_return(
+					&ramster_foreign_eph_pages_atomic);
+			if (c > ramster_foreign_eph_pages_max)
+				ramster_foreign_eph_pages_max = c;
+		} else {
+			c = atomic_dec_return(&ramster_foreign_eph_pages_atomic);
+			WARN_ON_ONCE(c < 0);
+		}
+		ramster_foreign_eph_pages = c;
+	} else {
+		if (count > 0) {
+			c = atomic_inc_return(
+					&ramster_foreign_pers_pages_atomic);
+			if (c > ramster_foreign_pers_pages_max)
+				ramster_foreign_pers_pages_max = c;
+		} else {
+			c = atomic_dec_return(
+					&ramster_foreign_pers_pages_atomic);
+			WARN_ON_ONCE(c < 0);
+		}
+		ramster_foreign_pers_pages = c;
+	}
+}
+
+/*
+ * For now, just push over a few pages every few seconds to
+ * ensure that it basically works
+ */
+static struct workqueue_struct *ramster_remotify_workqueue;
+static void ramster_remotify_process(struct work_struct *work);
+static DECLARE_DELAYED_WORK(ramster_remotify_worker,
+		ramster_remotify_process);
+
+static void ramster_remotify_queue_delayed_work(unsigned long delay)
+{
+	if (!queue_delayed_work(ramster_remotify_workqueue,
+				&ramster_remotify_worker, delay))
+		pr_err("ramster_remotify: bad workqueue\n");
+}
+
+static void ramster_remote_flush_page(struct flushlist_node *flnode)
+{
+	struct tmem_xhandle *xh;
+	int remotenode, ret;
+
+	preempt_disable();
+	xh = &flnode->xh;
+	remotenode = flnode->xh.client_id;
+	ret = r2net_remote_flush(xh, remotenode);
+	if (ret >= 0)
+		ramster_remote_pages_flushed++;
+	else
+		ramster_remote_page_flushes_failed++;
+	preempt_enable_no_resched();
+	ramster_flnode_free(flnode, NULL);
+}
+
+static void ramster_remote_flush_object(struct flushlist_node *flnode)
+{
+	struct tmem_xhandle *xh;
+	int remotenode, ret;
+
+	preempt_disable();
+	xh = &flnode->xh;
+	remotenode = flnode->xh.client_id;
+	ret = r2net_remote_flush_object(xh, remotenode);
+	if (ret >= 0)
+		ramster_remote_objects_flushed++;
+	else
+		ramster_remote_object_flushes_failed++;
+	preempt_enable_no_resched();
+	ramster_flnode_free(flnode, NULL);
+}
+
+int ramster_remotify_pageframe(bool eph)
+{
+	struct tmem_xhandle xh;
+	unsigned int size;
+	int remotenode, ret, zbuds;
+	struct tmem_pool *pool;
+	unsigned long flags;
+	unsigned char cksum;
+	char *p;
+	int i, j;
+	unsigned char *tmpmem[2];
+	struct tmem_handle th[2];
+	unsigned int zsize[2];
+
+	tmpmem[0] = __get_cpu_var(ramster_remoteputmem1);
+	tmpmem[1] = __get_cpu_var(ramster_remoteputmem2);
+	local_bh_disable();
+	zbuds = zbud_make_zombie_lru(&th[0], &tmpmem[0], &zsize[0], eph);
+	/* now OK to release lock set in caller */
+	local_bh_enable();
+	if (zbuds == 0)
+		goto out;
+	BUG_ON(zbuds > 2);
+	for (i = 0; i < zbuds; i++) {
+		xh.client_id = th[i].client_id;
+		xh.pool_id = th[i].pool_id;
+		xh.oid = th[i].oid;
+		xh.index = th[i].index;
+		size = zsize[i];
+		BUG_ON(size == 0 || size > zbud_max_buddy_size());
+		for (p = tmpmem[i], cksum = 0, j = 0; j < size; j++)
+			cksum += *p++;
+		ret = r2net_remote_put(&xh, tmpmem[i], size, eph, &remotenode);
+		if (ret != 0) {
+		/*
+		 * This is some form of a memory leak... if the remote put
+		 * fails, there will never be another attempt to remotify
+		 * this page.  But since we've dropped the zv pointer,
+		 * the page may have been freed or the data replaced
+		 * so we can't just "put it back" in the remote op list.
+		 * Even if we could, not sure where to put it in the list
+		 * because there may be flushes that must be strictly
+		 * ordered vs the put.  So leave this as a FIXME for now.
+		 * But count them so we know if it becomes a problem.
+		 */
+			if (eph)
+				ramster_eph_pages_remote_failed++;
+			else
+				ramster_pers_pages_remote_failed++;
+			break;
+		} else {
+			if (!eph)
+				atomic_inc(&ramster_remote_pers_pages);
+		}
+		if (eph)
+			ramster_eph_pages_remoted++;
+		else
+			ramster_pers_pages_remoted++;
+		/*
+		 * data was successfully remoted so change the local version to
+		 * point to the remote node where it landed
+		 */
+		local_bh_disable();
+		pool = zcache_get_pool_by_id(LOCAL_CLIENT, xh.pool_id);
+		local_irq_save(flags);
+		(void)tmem_replace(pool, &xh.oid, xh.index,
+				pampd_make_remote(remotenode, size, cksum));
+		local_irq_restore(flags);
+		zcache_put_pool(pool);
+		local_bh_enable();
+	}
+out:
+	return zbuds;
+}
+
+static void zcache_do_remotify_flushes(void)
+{
+	struct ramster_remotify_hdr *rem_op;
+	union remotify_list_node *u;
+
+	while (1) {
+		spin_lock(&ramster_rem_op_list_lock);
+		if (list_empty(&ramster_rem_op_list)) {
+			spin_unlock(&ramster_rem_op_list_lock);
+			goto out;
+		}
+		rem_op = list_first_entry(&ramster_rem_op_list,
+				struct ramster_remotify_hdr, list);
+		list_del_init(&rem_op->list);
+		spin_unlock(&ramster_rem_op_list_lock);
+		u = (union remotify_list_node *)rem_op;
+		switch (rem_op->op) {
+		case RAMSTER_REMOTIFY_FLUSH_PAGE:
+			ramster_remote_flush_page((struct flushlist_node *)u);
+			break;
+		case RAMSTER_REMOTIFY_FLUSH_OBJ:
+			ramster_remote_flush_object((struct flushlist_node *)u);
+			break;
+		default:
+			BUG();
+		}
+	}
+out:
+	return;
+}
+
+static void ramster_remotify_process(struct work_struct *work)
+{
+	static bool remotify_in_progress;
+	int i;
+
+	BUG_ON(irqs_disabled());
+	if (remotify_in_progress)
+		goto requeue;
+	if (ramster_remote_target_nodenum == -1)
+		goto requeue;
+	remotify_in_progress = true;
+	if (use_cleancache && ramster_eph_remotify_enable) {
+		for (i = 0; i < 100; i++) {
+			zcache_do_remotify_flushes();
+			(void)ramster_remotify_pageframe(true);
+		}
+	}
+	if (use_frontswap && ramster_pers_remotify_enable) {
+		for (i = 0; i < 100; i++) {
+			zcache_do_remotify_flushes();
+			(void)ramster_remotify_pageframe(false);
+		}
+	}
+	remotify_in_progress = false;
+requeue:
+	ramster_remotify_queue_delayed_work(HZ);
+}
+
+void __init ramster_remotify_init(void)
+{
+	unsigned long n = 60UL;
+	ramster_remotify_workqueue =
+		create_singlethread_workqueue("ramster_remotify");
+	ramster_remotify_queue_delayed_work(n * HZ);
+}
+
+static ssize_t ramster_manual_node_up_show(struct kobject *kobj,
+				struct kobj_attribute *attr, char *buf)
+{
+	int i;
+	char *p = buf;
+	for (i = 0; i < MANUAL_NODES; i++)
+		if (ramster_nodes_manual_up[i])
+			p += sprintf(p, "%d ", i);
+	p += sprintf(p, "\n");
+	return p - buf;
+}
+
+static ssize_t ramster_manual_node_up_store(struct kobject *kobj,
+		struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int err;
+	unsigned long node_num;
+
+	err = kstrtoul(buf, 10, &node_num);
+	if (err) {
+		pr_err("ramster: bad strtoul?\n");
+		return -EINVAL;
+	}
+	if (node_num >= MANUAL_NODES) {
+		pr_err("ramster: bad node_num=%lu?\n", node_num);
+		return -EINVAL;
+	}
+	if (ramster_nodes_manual_up[node_num]) {
+		pr_err("ramster: node %d already up, ignoring\n",
+							(int)node_num);
+	} else {
+		ramster_nodes_manual_up[node_num] = true;
+		r2net_hb_node_up_manual((int)node_num);
+	}
+	return count;
+}
+
+static struct kobj_attribute ramster_manual_node_up_attr = {
+	.attr = { .name = "manual_node_up", .mode = 0644 },
+	.show = ramster_manual_node_up_show,
+	.store = ramster_manual_node_up_store,
+};
+
+static ssize_t ramster_remote_target_nodenum_show(struct kobject *kobj,
+				struct kobj_attribute *attr, char *buf)
+{
+	if (ramster_remote_target_nodenum == -1UL)
+		return sprintf(buf, "unset\n");
+	else
+		return sprintf(buf, "%d\n", ramster_remote_target_nodenum);
+}
+
+static ssize_t ramster_remote_target_nodenum_store(struct kobject *kobj,
+		struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int err;
+	unsigned long node_num;
+
+	err = kstrtoul(buf, 10, &node_num);
+	if (err) {
+		pr_err("ramster: bad strtoul?\n");
+		return -EINVAL;
+	} else if (node_num == -1UL) {
+		pr_err("ramster: disabling all remotification, "
+			"data may still reside on remote nodes however\n");
+		return -EINVAL;
+	} else if (node_num >= MANUAL_NODES) {
+		pr_err("ramster: bad node_num=%lu?\n", node_num);
+		return -EINVAL;
+	} else if (!ramster_nodes_manual_up[node_num]) {
+		pr_err("ramster: node %d not up, ignoring setting "
+			"of remotification target\n", (int)node_num);
+	} else if (r2net_remote_target_node_set((int)node_num) >= 0) {
+		pr_info("ramster: node %d set as remotification target\n",
+				(int)node_num);
+		ramster_remote_target_nodenum = (int)node_num;
+	} else {
+		pr_err("ramster: bad num to node node_num=%d?\n",
+				(int)node_num);
+		return -EINVAL;
+	}
+	return count;
+}
+
+static struct kobj_attribute ramster_remote_target_nodenum_attr = {
+	.attr = { .name = "remote_target_nodenum", .mode = 0644 },
+	.show = ramster_remote_target_nodenum_show,
+	.store = ramster_remote_target_nodenum_store,
+};
+
+#define RAMSTER_SYSFS_RO(_name) \
+	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
+				struct kobj_attribute *attr, char *buf) \
+	{ \
+		return sprintf(buf, "%lu\n", ramster_##_name); \
+	} \
+	static struct kobj_attribute ramster_##_name##_attr = { \
+		.attr = { .name = __stringify(_name), .mode = 0444 }, \
+		.show = ramster_##_name##_show, \
+	}
+
+#define RAMSTER_SYSFS_RW(_name) \
+	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
+				struct kobj_attribute *attr, char *buf) \
+	{ \
+		return sprintf(buf, "%lu\n", ramster_##_name); \
+	} \
+	static ssize_t ramster_##_name##_store(struct kobject *kobj, \
+		struct kobj_attribute *attr, const char *buf, size_t count) \
+	{ \
+		int err; \
+		unsigned long enable; \
+		err = kstrtoul(buf, 10, &enable); \
+		if (err) \
+			return -EINVAL; \
+		ramster_##_name = enable; \
+		return count; \
+	} \
+	static struct kobj_attribute ramster_##_name##_attr = { \
+		.attr = { .name = __stringify(_name), .mode = 0644 }, \
+		.show = ramster_##_name##_show, \
+		.store = ramster_##_name##_store, \
+	}
+
+#define RAMSTER_SYSFS_RO_ATOMIC(_name) \
+	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
+				struct kobj_attribute *attr, char *buf) \
+	{ \
+	    return sprintf(buf, "%d\n", atomic_read(&ramster_##_name)); \
+	} \
+	static struct kobj_attribute ramster_##_name##_attr = { \
+		.attr = { .name = __stringify(_name), .mode = 0444 }, \
+		.show = ramster_##_name##_show, \
+	}
+
+RAMSTER_SYSFS_RO(interface_revision);
+RAMSTER_SYSFS_RO_ATOMIC(remote_pers_pages);
+RAMSTER_SYSFS_RW(pers_remotify_enable);
+RAMSTER_SYSFS_RW(eph_remotify_enable);
+
+static struct attribute *ramster_attrs[] = {
+	&ramster_interface_revision_attr.attr,
+	&ramster_remote_pers_pages_attr.attr,
+	&ramster_manual_node_up_attr.attr,
+	&ramster_remote_target_nodenum_attr.attr,
+	&ramster_pers_remotify_enable_attr.attr,
+	&ramster_eph_remotify_enable_attr.attr,
+	NULL,
+};
+
+static struct attribute_group ramster_attr_group = {
+	.attrs = ramster_attrs,
+	.name = "ramster",
+};
+
+/*
+ * frontswap selfshrinking
+ */
+
+/* In HZ, controls frequency of worker invocation. */
+static unsigned int selfshrink_interval __read_mostly = 5;
+/* Enable/disable with sysfs. */
+static bool frontswap_selfshrinking __read_mostly;
+
+static void selfshrink_process(struct work_struct *work);
+static DECLARE_DELAYED_WORK(selfshrink_worker, selfshrink_process);
+
+/* Enable/disable with kernel boot option. */
+static bool use_frontswap_selfshrink __initdata = true;
+
+/*
+ * The default values for the following parameters were deemed reasonable
+ * by experimentation, may be workload-dependent, and can all be
+ * adjusted via sysfs.
+ */
+
+/* Control rate for frontswap shrinking. Higher hysteresis is slower. */
+static unsigned int frontswap_hysteresis __read_mostly = 20;
+
+/*
+ * Number of selfshrink worker invocations to wait before observing that
+ * frontswap selfshrinking should commence. Note that selfshrinking does
+ * not use a separate worker thread.
+ */
+static unsigned int frontswap_inertia __read_mostly = 3;
+
+/* Countdown to next invocation of frontswap_shrink() */
+static unsigned long frontswap_inertia_counter;
+
+/*
+ * Invoked by the selfshrink worker thread, uses current number of pages
+ * in frontswap (frontswap_curr_pages()), previous status, and control
+ * values (hysteresis and inertia) to determine if frontswap should be
+ * shrunk and what the new frontswap size should be.  Note that
+ * frontswap_shrink is essentially a partial swapoff that immediately
+ * transfers pages from the "swap device" (frontswap) back into kernel
+ * RAM; despite the name, frontswap "shrinking" is very different from
+ * the "shrinker" interface used by the kernel MM subsystem to reclaim
+ * memory.
+ */
+static void frontswap_selfshrink(void)
+{
+	static unsigned long cur_frontswap_pages;
+	static unsigned long last_frontswap_pages;
+	static unsigned long tgt_frontswap_pages;
+
+	last_frontswap_pages = cur_frontswap_pages;
+	cur_frontswap_pages = frontswap_curr_pages();
+	if (!cur_frontswap_pages ||
+			(cur_frontswap_pages > last_frontswap_pages)) {
+		frontswap_inertia_counter = frontswap_inertia;
+		return;
+	}
+	if (frontswap_inertia_counter && --frontswap_inertia_counter)
+		return;
+	if (cur_frontswap_pages <= frontswap_hysteresis)
+		tgt_frontswap_pages = 0;
+	else
+		tgt_frontswap_pages = cur_frontswap_pages -
+			(cur_frontswap_pages / frontswap_hysteresis);
+	frontswap_shrink(tgt_frontswap_pages);
+}
+
+static int __init ramster_nofrontswap_selfshrink_setup(char *s)
+{
+	use_frontswap_selfshrink = false;
+	return 1;
+}
+
+__setup("noselfshrink", ramster_nofrontswap_selfshrink_setup);
+
+static void selfshrink_process(struct work_struct *work)
+{
+	if (frontswap_selfshrinking && frontswap_enabled) {
+		frontswap_selfshrink();
+		schedule_delayed_work(&selfshrink_worker,
+			selfshrink_interval * HZ);
+	}
+}
+
+void ramster_cpu_up(int cpu)
+{
+	unsigned char *p1 = kzalloc(PAGE_SIZE, GFP_KERNEL | __GFP_REPEAT);
+	unsigned char *p2 = kzalloc(PAGE_SIZE, GFP_KERNEL | __GFP_REPEAT);
+	BUG_ON(!p1 || !p2);
+	per_cpu(ramster_remoteputmem1, cpu) = p1;
+	per_cpu(ramster_remoteputmem2, cpu) = p2;
+}
+
+void ramster_cpu_down(int cpu)
+{
+	struct ramster_preload *kp;
+
+	kfree(per_cpu(ramster_remoteputmem1, cpu));
+	per_cpu(ramster_remoteputmem1, cpu) = NULL;
+	kfree(per_cpu(ramster_remoteputmem2, cpu));
+	per_cpu(ramster_remoteputmem2, cpu) = NULL;
+	kp = &per_cpu(ramster_preloads, cpu);
+	if (kp->flnode) {
+		kmem_cache_free(ramster_flnode_cache, kp->flnode);
+		kp->flnode = NULL;
+	}
+}
+
+void ramster_register_pamops(struct tmem_pamops *pamops)
+{
+	pamops->free_obj = ramster_pampd_free_obj;
+	pamops->new_obj = ramster_pampd_new_obj;
+	pamops->replace_in_obj = ramster_pampd_replace_in_obj;
+	pamops->is_remote = ramster_pampd_is_remote;
+	pamops->repatriate = ramster_pampd_repatriate;
+	pamops->repatriate_preload = ramster_pampd_repatriate_preload;
+}
+
+void __init ramster_init(bool cleancache, bool frontswap,
+				bool frontswap_exclusive_gets)
+{
+	int ret = 0;
+
+	if (cleancache)
+		use_cleancache = true;
+	if (frontswap)
+		use_frontswap = true;
+	if (frontswap_exclusive_gets)
+		use_frontswap_exclusive_gets = true;
+	ramster_debugfs_init();
+	ret = sysfs_create_group(mm_kobj, &ramster_attr_group);
+	if (ret)
+		pr_err("ramster: can't create sysfs for ramster\n");
+	(void)r2net_register_handlers();
+	INIT_LIST_HEAD(&ramster_rem_op_list);
+	ramster_flnode_cache = kmem_cache_create("ramster_flnode",
+				sizeof(struct flushlist_node), 0, 0, NULL);
+	frontswap_selfshrinking = use_frontswap_selfshrink;
+	if (frontswap_selfshrinking) {
+		pr_info("ramster: Initializing frontswap selfshrink driver.\n");
+		schedule_delayed_work(&selfshrink_worker,
+					selfshrink_interval * HZ);
+	}
+	ramster_remotify_init();
+}
diff --git a/drivers/staging/ramster/ramster/ramster.h b/drivers/staging/ramster/ramster/ramster.h
new file mode 100644
index 000000000000..12ae56f09ca4
--- /dev/null
+++ b/drivers/staging/ramster/ramster/ramster.h
@@ -0,0 +1,161 @@
+/*
+ * ramster.h
+ *
+ * Peer-to-peer transcendent memory
+ *
+ * Copyright (c) 2009-2012, Dan Magenheimer, Oracle Corp.
+ */
+
+#ifndef _RAMSTER_RAMSTER_H_
+#define _RAMSTER_RAMSTER_H_
+
+#include "../tmem.h"
+
+enum ramster_remotify_op {
+	RAMSTER_REMOTIFY_FLUSH_PAGE,
+	RAMSTER_REMOTIFY_FLUSH_OBJ,
+};
+
+struct ramster_remotify_hdr {
+	enum ramster_remotify_op op;
+	struct list_head list;
+};
+
+struct flushlist_node {
+	struct ramster_remotify_hdr rem_op;
+	struct tmem_xhandle xh;
+};
+
+struct ramster_preload {
+	struct flushlist_node *flnode;
+};
+
+union remotify_list_node {
+	struct ramster_remotify_hdr rem_op;
+	struct {
+		struct ramster_remotify_hdr rem_op;
+		struct tmem_handle th;
+	} zbud_hdr;
+	struct flushlist_node flist;
+};
+
+/*
+ * format of remote pampd:
+ *   bit 0 is reserved for zbud (in-page buddy selection)
+ *   bit 1 == intransit
+ *   bit 2 == is_remote... if this bit is set, then
+ *   bit 3-10 == remotenode
+ *   bit 11-23 == size
+ *   bit 24-31 == cksum
+ */
+#define FAKE_PAMPD_INTRANSIT_BITS	1
+#define FAKE_PAMPD_ISREMOTE_BITS	1
+#define FAKE_PAMPD_REMOTENODE_BITS	8
+#define FAKE_PAMPD_REMOTESIZE_BITS	13
+#define FAKE_PAMPD_CHECKSUM_BITS	8
+
+#define FAKE_PAMPD_INTRANSIT_SHIFT	1
+#define FAKE_PAMPD_ISREMOTE_SHIFT	(FAKE_PAMPD_INTRANSIT_SHIFT + \
+					 FAKE_PAMPD_INTRANSIT_BITS)
+#define FAKE_PAMPD_REMOTENODE_SHIFT	(FAKE_PAMPD_ISREMOTE_SHIFT + \
+					 FAKE_PAMPD_ISREMOTE_BITS)
+#define FAKE_PAMPD_REMOTESIZE_SHIFT	(FAKE_PAMPD_REMOTENODE_SHIFT + \
+					 FAKE_PAMPD_REMOTENODE_BITS)
+#define FAKE_PAMPD_CHECKSUM_SHIFT	(FAKE_PAMPD_REMOTESIZE_SHIFT + \
+					 FAKE_PAMPD_REMOTESIZE_BITS)
+
+#define FAKE_PAMPD_MASK(x)		((1UL << (x)) - 1)
+
+static inline void *pampd_make_remote(int remotenode, size_t size,
+					unsigned char cksum)
+{
+	unsigned long fake_pampd = 0;
+	fake_pampd |= 1UL << FAKE_PAMPD_ISREMOTE_SHIFT;
+	fake_pampd |= ((unsigned long)remotenode &
+			FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTENODE_BITS)) <<
+				FAKE_PAMPD_REMOTENODE_SHIFT;
+	fake_pampd |= ((unsigned long)size &
+			FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTESIZE_BITS)) <<
+				FAKE_PAMPD_REMOTESIZE_SHIFT;
+	fake_pampd |= ((unsigned long)cksum &
+			FAKE_PAMPD_MASK(FAKE_PAMPD_CHECKSUM_BITS)) <<
+				FAKE_PAMPD_CHECKSUM_SHIFT;
+	return (void *)fake_pampd;
+}
+
+static inline unsigned int pampd_remote_node(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+	return (fake_pampd >> FAKE_PAMPD_REMOTENODE_SHIFT) &
+		FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTENODE_BITS);
+}
+
+static inline unsigned int pampd_remote_size(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+	return (fake_pampd >> FAKE_PAMPD_REMOTESIZE_SHIFT) &
+		FAKE_PAMPD_MASK(FAKE_PAMPD_REMOTESIZE_BITS);
+}
+
+static inline unsigned char pampd_remote_cksum(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+	return (fake_pampd >> FAKE_PAMPD_CHECKSUM_SHIFT) &
+		FAKE_PAMPD_MASK(FAKE_PAMPD_CHECKSUM_BITS);
+}
+
+static inline bool pampd_is_remote(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+	return (fake_pampd >> FAKE_PAMPD_ISREMOTE_SHIFT) &
+		FAKE_PAMPD_MASK(FAKE_PAMPD_ISREMOTE_BITS);
+}
+
+static inline bool pampd_is_intransit(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+	return (fake_pampd >> FAKE_PAMPD_INTRANSIT_SHIFT) &
+		FAKE_PAMPD_MASK(FAKE_PAMPD_INTRANSIT_BITS);
+}
+
+/* note that it is a BUG for intransit to be set without isremote also set */
+static inline void *pampd_mark_intransit(void *pampd)
+{
+	unsigned long fake_pampd = (unsigned long)pampd;
+
+	fake_pampd |= 1UL << FAKE_PAMPD_ISREMOTE_SHIFT;
+	fake_pampd |= 1UL << FAKE_PAMPD_INTRANSIT_SHIFT;
+	return (void *)fake_pampd;
+}
+
+static inline void *pampd_mask_intransit_and_remote(void *marked_pampd)
+{
+	unsigned long pampd = (unsigned long)marked_pampd;
+
+	pampd &= ~(1UL << FAKE_PAMPD_INTRANSIT_SHIFT);
+	pampd &= ~(1UL << FAKE_PAMPD_ISREMOTE_SHIFT);
+	return (void *)pampd;
+}
+
+extern int r2net_remote_async_get(struct tmem_xhandle *,
+				bool, int, size_t, uint8_t, void *extra);
+extern int r2net_remote_put(struct tmem_xhandle *, char *, size_t,
+				bool, int *);
+extern int r2net_remote_flush(struct tmem_xhandle *, int);
+extern int r2net_remote_flush_object(struct tmem_xhandle *, int);
+extern int r2net_register_handlers(void);
+extern int r2net_remote_target_node_set(int);
+
+extern int ramster_remotify_pageframe(bool);
+extern void ramster_init(bool, bool, bool);
+extern void ramster_register_pamops(struct tmem_pamops *);
+extern int ramster_localify(int, struct tmem_oid *oidp, uint32_t, char *,
+				unsigned int, void *);
+extern void *ramster_pampd_free(void *, struct tmem_pool *, struct tmem_oid *,
+				uint32_t, bool);
+extern void ramster_count_foreign_pages(bool, int);
+extern int ramster_do_preload_flnode(struct tmem_pool *);
+extern void ramster_cpu_up(int);
+extern void ramster_cpu_down(int);
+
+#endif /* _RAMSTER_RAMSTER_H */
diff --git a/drivers/staging/ramster/cluster/ramster_nodemanager.h b/drivers/staging/ramster/ramster/ramster_nodemanager.h
index 49f879d943ab..49f879d943ab 100644
--- a/drivers/staging/ramster/cluster/ramster_nodemanager.h
+++ b/drivers/staging/ramster/ramster/ramster_nodemanager.h
diff --git a/drivers/staging/ramster/cluster/tcp.c b/drivers/staging/ramster/ramster/tcp.c
index d0a07d722b61..aa2a1a763aa4 100644
--- a/drivers/staging/ramster/cluster/tcp.c
+++ b/drivers/staging/ramster/ramster/tcp.c
@@ -541,8 +541,7 @@ static void r2net_set_nn_state(struct r2net_node *nn,
 	}
 
 	if (was_valid && !valid) {
-		printk(KERN_NOTICE "ramster: No longer connected to "
-		       SC_NODEF_FMT "\n",
+		pr_notice("ramster: No longer connected to " SC_NODEF_FMT "\n",
 			old_sc->sc_node->nd_name, old_sc->sc_node->nd_num,
 			&old_sc->sc_node->nd_ipv4_address,
 			ntohs(old_sc->sc_node->nd_ipv4_port));
@@ -551,7 +550,7 @@ static void r2net_set_nn_state(struct r2net_node *nn,
 
 	if (!was_valid && valid) {
 		cancel_delayed_work(&nn->nn_connect_expired);
-		printk(KERN_NOTICE "ramster: %s " SC_NODEF_FMT "\n",
+		pr_notice("ramster: %s " SC_NODEF_FMT "\n",
 		       r2nm_this_node() > sc->sc_node->nd_num ?
 		       "Connected to" : "Accepted connection from",
 		       sc->sc_node->nd_name, sc->sc_node->nd_num,
@@ -644,7 +643,7 @@ static void r2net_state_change(struct sock *sk)
 		r2net_sc_queue_work(sc, &sc->sc_connect_work);
 		break;
 	default:
-		printk(KERN_INFO "ramster: Connection to "
+		pr_info("ramster: Connection to "
 			SC_NODEF_FMT " shutdown, state %d\n",
 			sc->sc_node->nd_name, sc->sc_node->nd_num,
 			&sc->sc_node->nd_ipv4_address,
@@ -1160,7 +1159,8 @@ int r2net_send_message_vec(u32 msg_type, u32 key, struct kvec *caller_vec,
 
 	/* wait on other node's handler */
 	r2net_set_nst_status_time(&nst);
-	wait_event(nsw.ns_wq, r2net_nsw_completed(nn, &nsw));
+	wait_event(nsw.ns_wq, r2net_nsw_completed(nn, &nsw) ||
+			nn->nn_persistent_error || !nn->nn_sc_valid);
 
 	r2net_update_send_stats(&nst, sc);
 
@@ -1325,8 +1325,10 @@ static int r2net_process_message(struct r2net_sock_container *sc,
 	if (be16_to_cpu(hdr->data_len) > nmh->nh_max_len)
 		syserr = R2NET_ERR_OVERFLOW;
 
-	if (syserr != R2NET_ERR_NONE)
+	if (syserr != R2NET_ERR_NONE) {
+		pr_err("ramster_r2net, message length problem\n");
 		goto out_respond;
+	}
 
 	r2net_set_func_start_time(sc);
 	sc->sc_msg_key = be32_to_cpu(hdr->key);
@@ -1393,7 +1395,7 @@ static int r2net_check_handshake(struct r2net_sock_container *sc)
 	struct r2net_node *nn = r2net_nn_from_num(sc->sc_node->nd_num);
 
 	if (hand->protocol_version != cpu_to_be64(R2NET_PROTOCOL_VERSION)) {
-		printk(KERN_NOTICE "ramster: " SC_NODEF_FMT " Advertised net "
+		pr_notice("ramster: " SC_NODEF_FMT " Advertised net "
 		       "protocol version %llu but %llu is required. "
 		       "Disconnecting.\n", sc->sc_node->nd_name,
 			sc->sc_node->nd_num, &sc->sc_node->nd_ipv4_address,
@@ -1413,7 +1415,7 @@ static int r2net_check_handshake(struct r2net_sock_container *sc)
 	 */
 	if (be32_to_cpu(hand->r2net_idle_timeout_ms) !=
 				r2net_idle_timeout()) {
-		printk(KERN_NOTICE "ramster: " SC_NODEF_FMT " uses a network "
+		pr_notice("ramster: " SC_NODEF_FMT " uses a network "
 		       "idle timeout of %u ms, but we use %u ms locally. "
 		       "Disconnecting.\n", sc->sc_node->nd_name,
 			sc->sc_node->nd_num, &sc->sc_node->nd_ipv4_address,
@@ -1426,7 +1428,7 @@ static int r2net_check_handshake(struct r2net_sock_container *sc)
 
 	if (be32_to_cpu(hand->r2net_keepalive_delay_ms) !=
 			r2net_keepalive_delay()) {
-		printk(KERN_NOTICE "ramster: " SC_NODEF_FMT " uses a keepalive "
+		pr_notice("ramster: " SC_NODEF_FMT " uses a keepalive "
 		       "delay of %u ms, but we use %u ms locally. "
 		       "Disconnecting.\n", sc->sc_node->nd_name,
 			sc->sc_node->nd_num, &sc->sc_node->nd_ipv4_address,
@@ -1439,7 +1441,7 @@ static int r2net_check_handshake(struct r2net_sock_container *sc)
 
 	if (be32_to_cpu(hand->r2hb_heartbeat_timeout_ms) !=
 			R2HB_MAX_WRITE_TIMEOUT_MS) {
-		printk(KERN_NOTICE "ramster: " SC_NODEF_FMT " uses a heartbeat "
+		pr_notice("ramster: " SC_NODEF_FMT " uses a heartbeat "
 		       "timeout of %u ms, but we use %u ms locally. "
 		       "Disconnecting.\n", sc->sc_node->nd_name,
 			sc->sc_node->nd_num, &sc->sc_node->nd_ipv4_address,
@@ -1516,6 +1518,7 @@ static int r2net_advance_rx(struct r2net_sock_container *sc)
 				if (be16_to_cpu(hdr->data_len) >
 				    R2NET_MAX_PAYLOAD_BYTES)
 					ret = -EOVERFLOW;
+				WARN_ON_ONCE(ret == -EOVERFLOW);
 			}
 		}
 		if (ret <= 0)
@@ -1583,7 +1586,6 @@ static void r2net_rx_until_empty(struct work_struct *work)
 		/* not permanent so read failed handshake can retry */
 		r2net_ensure_shutdown(nn, sc, 0);
 	}
-
 	sc_put(sc);
 }
 
@@ -1659,6 +1661,7 @@ static void r2net_sc_send_keep_req(struct work_struct *work)
 static void r2net_idle_timer(unsigned long data)
 {
 	struct r2net_sock_container *sc = (struct r2net_sock_container *)data;
+	struct r2net_node *nn = r2net_nn_from_num(sc->sc_node->nd_num);
 #ifdef CONFIG_DEBUG_FS
 	unsigned long msecs = ktime_to_ms(ktime_get()) -
 		ktime_to_ms(sc->sc_tv_timer);
@@ -1666,7 +1669,7 @@ static void r2net_idle_timer(unsigned long data)
 	unsigned long msecs = r2net_idle_timeout();
 #endif
 
-	printk(KERN_NOTICE "ramster: Connection to " SC_NODEF_FMT " has been "
+	pr_notice("ramster: Connection to " SC_NODEF_FMT " has been "
 	       "idle for %lu.%lu secs, shutting it down.\n",
 		sc->sc_node->nd_name, sc->sc_node->nd_num,
 		&sc->sc_node->nd_ipv4_address, ntohs(sc->sc_node->nd_ipv4_port),
@@ -1676,13 +1679,8 @@ static void r2net_idle_timer(unsigned long data)
 	 * Initialize the nn_timeout so that the next connection attempt
 	 * will continue in r2net_start_connect.
 	 */
-	/* Avoid spurious shutdowns... not sure if this is still necessary */
-	pr_err("ramster_idle_timer, skipping shutdown work\n");
-#if 0
-	/* old code used to do these two lines */
 	atomic_set(&nn->nn_timeout, 1);
 	r2net_sc_queue_work(sc, &sc->sc_shutdown_work);
-#endif
 }
 
 static void r2net_sc_reset_idle_timer(struct r2net_sock_container *sc)
@@ -1807,7 +1805,7 @@ static void r2net_start_connect(struct work_struct *work)
 
 out:
 	if (ret) {
-		printk(KERN_NOTICE "ramster: Connect attempt to " SC_NODEF_FMT
+		pr_notice("ramster: Connect attempt to " SC_NODEF_FMT
 		       " failed with errno %d\n", sc->sc_node->nd_name,
 			sc->sc_node->nd_num, &sc->sc_node->nd_ipv4_address,
 			ntohs(sc->sc_node->nd_ipv4_port), ret);
@@ -1833,7 +1831,7 @@ static void r2net_connect_expired(struct work_struct *work)
 
 	spin_lock(&nn->nn_lock);
 	if (!nn->nn_sc_valid) {
-		printk(KERN_NOTICE "ramster: No connection established with "
+		pr_notice("ramster: No connection established with "
 		       "node %u after %u.%u seconds, giving up.\n",
 		     r2net_num_from_nn(nn),
 		     r2net_idle_timeout() / 1000,
@@ -1969,7 +1967,7 @@ static int r2net_accept_one(struct socket *sock)
 
 	node = r2nm_get_node_by_ip(sin.sin_addr.s_addr);
 	if (node == NULL) {
-		printk(KERN_NOTICE "ramster: Attempt to connect from unknown "
+		pr_notice("ramster: Attempt to connect from unknown "
 		       "node at %pI4:%d\n", &sin.sin_addr.s_addr,
 		       ntohs(sin.sin_port));
 		ret = -EINVAL;
@@ -1978,7 +1976,7 @@ static int r2net_accept_one(struct socket *sock)
 
 	if (r2nm_this_node() >= node->nd_num) {
 		local_node = r2nm_get_node_by_num(r2nm_this_node());
-		printk(KERN_NOTICE "ramster: Unexpected connect attempt seen "
+		pr_notice("ramster: Unexpected connect attempt seen "
 		       "at node '%s' (%u, %pI4:%d) from node '%s' (%u, "
 		       "%pI4:%d)\n", local_node->nd_name, local_node->nd_num,
 		       &(local_node->nd_ipv4_address),
@@ -2008,7 +2006,7 @@ static int r2net_accept_one(struct socket *sock)
 		ret = 0;
 	spin_unlock(&nn->nn_lock);
 	if (ret) {
-		printk(KERN_NOTICE "ramster: Attempt to connect from node '%s' "
+		pr_notice("ramster: Attempt to connect from node '%s' "
 		       "at %pI4:%d but it already has an open connection\n",
 		       node->nd_name, &sin.sin_addr.s_addr,
 		       ntohs(sin.sin_port));
@@ -2091,8 +2089,7 @@ static int r2net_open_listening_sock(__be32 addr, __be16 port)
 
 	ret = sock_create(PF_INET, SOCK_STREAM, IPPROTO_TCP, &sock);
 	if (ret < 0) {
-		printk(KERN_ERR "ramster: Error %d while creating socket\n",
-			ret);
+		pr_err("ramster: Error %d while creating socket\n", ret);
 		goto out;
 	}
 
@@ -2106,17 +2103,17 @@ static int r2net_open_listening_sock(__be32 addr, __be16 port)
 	r2net_listen_sock = sock;
 	INIT_WORK(&r2net_listen_work, r2net_accept_many);
 
-	sock->sk->sk_reuse = SK_CAN_REUSE;
+	sock->sk->sk_reuse = /* SK_CAN_REUSE FIXME FOR 3.4 */ 1;
 	ret = sock->ops->bind(sock, (struct sockaddr *)&sin, sizeof(sin));
 	if (ret < 0) {
-		printk(KERN_ERR "ramster: Error %d while binding socket at "
-			"%pI4:%u\n", ret, &addr, ntohs(port));
+		pr_err("ramster: Error %d while binding socket at %pI4:%u\n",
+			ret, &addr, ntohs(port));
 		goto out;
 	}
 
 	ret = sock->ops->listen(sock, 64);
 	if (ret < 0)
-		printk(KERN_ERR "ramster: Error %d while listening on %pI4:%u\n",
+		pr_err("ramster: Error %d while listening on %pI4:%u\n",
 		       ret, &addr, ntohs(port));
 
 out:
diff --git a/drivers/staging/ramster/cluster/tcp.h b/drivers/staging/ramster/ramster/tcp.h
index 9d05833452b5..9d05833452b5 100644
--- a/drivers/staging/ramster/cluster/tcp.h
+++ b/drivers/staging/ramster/ramster/tcp.h
diff --git a/drivers/staging/ramster/cluster/tcp_internal.h b/drivers/staging/ramster/ramster/tcp_internal.h
index 4d8cc9f96fd2..4d8cc9f96fd2 100644
--- a/drivers/staging/ramster/cluster/tcp_internal.h
+++ b/drivers/staging/ramster/ramster/tcp_internal.h
diff --git a/drivers/staging/ramster/tmem.c b/drivers/staging/ramster/tmem.c
index 8f2f6892d8d3..a2b7e03b6062 100644
--- a/drivers/staging/ramster/tmem.c
+++ b/drivers/staging/ramster/tmem.c
@@ -1,33 +1,43 @@
 /*
  * In-kernel transcendent memory (generic implementation)
  *
- * Copyright (c) 2009-2011, Dan Magenheimer, Oracle Corp.
+ * Copyright (c) 2009-2012, Dan Magenheimer, Oracle Corp.
  *
  * The primary purpose of Transcedent Memory ("tmem") is to map object-oriented
  * "handles" (triples containing a pool id, and object id, and an index), to
  * pages in a page-accessible memory (PAM).  Tmem references the PAM pages via
  * an abstract "pampd" (PAM page-descriptor), which can be operated on by a
  * set of functions (pamops).  Each pampd contains some representation of
- * PAGE_SIZE bytes worth of data. Tmem must support potentially millions of
- * pages and must be able to insert, find, and delete these pages at a
- * potential frequency of thousands per second concurrently across many CPUs,
- * (and, if used with KVM, across many vcpus across many guests).
- * Tmem is tracked with a hierarchy of data structures, organized by
- * the elements in a handle-tuple: pool_id, object_id, and page index.
- * One or more "clients" (e.g. guests) each provide one or more tmem_pools.
- * Each pool, contains a hash table of rb_trees of tmem_objs.  Each
- * tmem_obj contains a radix-tree-like tree of pointers, with intermediate
- * nodes called tmem_objnodes.  Each leaf pointer in this tree points to
- * a pampd, which is accessible only through a small set of callbacks
- * registered by the PAM implementation (see tmem_register_pamops). Tmem
- * does all memory allocation via a set of callbacks registered by the tmem
- * host implementation (e.g. see tmem_register_hostops).
+ * PAGE_SIZE bytes worth of data. For those familiar with key-value stores,
+ * the tmem handle is a three-level hierarchical key, and the value is always
+ * reconstituted (but not necessarily stored) as PAGE_SIZE bytes and is
+ * referenced in the datastore by the pampd.  The hierarchy is required
+ * to ensure that certain invalidation functions can be performed efficiently
+ * (i.e. flush all indexes associated with this object_id, or
+ * flush all objects associated with this pool).
+ *
+ * Tmem must support potentially millions of pages and must be able to insert,
+ * find, and delete these pages at a potential frequency of thousands per
+ * second concurrently across many CPUs, (and, if used with KVM, across many
+ * vcpus across many guests).  Tmem is tracked with a hierarchy of data
+ * structures, organized by the elements in the handle-tuple: pool_id,
+ * object_id, and page index.  One or more "clients" (e.g. guests) each
+ * provide one or more tmem_pools.  Each pool, contains a hash table of
+ * rb_trees of tmem_objs.  Each tmem_obj contains a radix-tree-like tree
+ * of pointers, with intermediate nodes called tmem_objnodes.  Each leaf
+ * pointer in this tree points to a pampd, which is accessible only through
+ * a small set of callbacks registered by the PAM implementation (see
+ * tmem_register_pamops). Tmem only needs to memory allocation for objs
+ * and objnodes and this is done via a set of callbacks that must be
+ * registered by the tmem host implementation (e.g. see tmem_register_hostops).
  */
 
 #include <linux/list.h>
 #include <linux/spinlock.h>
 #include <linux/atomic.h>
+#ifdef CONFIG_RAMSTER
 #include <linux/delay.h>
+#endif
 
 #include "tmem.h"
 
@@ -52,7 +62,7 @@ void tmem_register_hostops(struct tmem_hostops *m)
 
 /*
  * A tmem host implementation must use this function to register
- * callbacks for a page-accessible memory (PAM) implementation
+ * callbacks for a page-accessible memory (PAM) implementation.
  */
 static struct tmem_pamops tmem_pamops;
 
@@ -67,42 +77,62 @@ void tmem_register_pamops(struct tmem_pamops *m)
  * So an rb_tree is an ideal data structure to manage tmem_objs.  But because
  * of the potentially huge number of tmem_objs, each pool manages a hashtable
  * of rb_trees to reduce search, insert, delete, and rebalancing time.
- * Each hashbucket also has a lock to manage concurrent access.
+ * Each hashbucket also has a lock to manage concurrent access and no
+ * searches, inserts, or deletions can be performed unless the lock is held.
+ * As a result, care must be taken to ensure tmem routines are not called
+ * recursively; the vast majority of the time, a recursive call may work
+ * but a deadlock will occur a small fraction of the time due to the
+ * hashbucket lock.
  *
- * The following routines manage tmem_objs.  When any tmem_obj is accessed,
- * the hashbucket lock must be held.
+ * The following routines manage tmem_objs.  In all of these routines,
+ * the hashbucket lock is already held.
  */
 
-/* searches for object==oid in pool, returns locked object if found */
-static struct tmem_obj *tmem_obj_find(struct tmem_hashbucket *hb,
-					struct tmem_oid *oidp)
+/* Search for object==oid in pool, returns object if found. */
+static struct tmem_obj *__tmem_obj_find(struct tmem_hashbucket *hb,
+					struct tmem_oid *oidp,
+					struct rb_node **parent,
+					struct rb_node ***link)
 {
-	struct rb_node *rbnode;
-	struct tmem_obj *obj;
+	struct rb_node *_parent = NULL, **rbnode;
+	struct tmem_obj *obj = NULL;
 
-	rbnode = hb->obj_rb_root.rb_node;
-	while (rbnode) {
-		BUG_ON(RB_EMPTY_NODE(rbnode));
-		obj = rb_entry(rbnode, struct tmem_obj, rb_tree_node);
+	rbnode = &hb->obj_rb_root.rb_node;
+	while (*rbnode) {
+		BUG_ON(RB_EMPTY_NODE(*rbnode));
+		_parent = *rbnode;
+		obj = rb_entry(*rbnode, struct tmem_obj,
+			       rb_tree_node);
 		switch (tmem_oid_compare(oidp, &obj->oid)) {
 		case 0: /* equal */
 			goto out;
 		case -1:
-			rbnode = rbnode->rb_left;
+			rbnode = &(*rbnode)->rb_left;
 			break;
 		case 1:
-			rbnode = rbnode->rb_right;
+			rbnode = &(*rbnode)->rb_right;
 			break;
 		}
 	}
+
+	if (parent)
+		*parent = _parent;
+	if (link)
+		*link = rbnode;
 	obj = NULL;
 out:
 	return obj;
 }
 
-static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *);
+static struct tmem_obj *tmem_obj_find(struct tmem_hashbucket *hb,
+					struct tmem_oid *oidp)
+{
+	return __tmem_obj_find(hb, oidp, NULL, NULL);
+}
 
-/* free an object that has no more pampds in it */
+static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *, bool);
+
+/* Free an object that has no more pampds in it. */
 static void tmem_obj_free(struct tmem_obj *obj, struct tmem_hashbucket *hb)
 {
 	struct tmem_pool *pool;
@@ -113,7 +143,7 @@ static void tmem_obj_free(struct tmem_obj *obj, struct tmem_hashbucket *hb)
 	pool = obj->pool;
 	BUG_ON(pool == NULL);
 	if (obj->objnode_tree_root != NULL) /* may be "stump" with no leaves */
-		tmem_pampd_destroy_all_in_obj(obj);
+		tmem_pampd_destroy_all_in_obj(obj, false);
 	BUG_ON(obj->objnode_tree_root != NULL);
 	BUG_ON((long)obj->objnode_count != 0);
 	atomic_dec(&pool->obj_count);
@@ -125,15 +155,14 @@ static void tmem_obj_free(struct tmem_obj *obj, struct tmem_hashbucket *hb)
 }
 
 /*
- * initialize, and insert an tmem_object_root (called only if find failed)
+ * Initialize, and insert an tmem_object_root (called only if find failed).
  */
 static void tmem_obj_init(struct tmem_obj *obj, struct tmem_hashbucket *hb,
 					struct tmem_pool *pool,
 					struct tmem_oid *oidp)
 {
 	struct rb_root *root = &hb->obj_rb_root;
-	struct rb_node **new = &(root->rb_node), *parent = NULL;
-	struct tmem_obj *this;
+	struct rb_node **new = NULL, *parent = NULL;
 
 	BUG_ON(pool == NULL);
 	atomic_inc(&pool->obj_count);
@@ -143,24 +172,15 @@ static void tmem_obj_init(struct tmem_obj *obj, struct tmem_hashbucket *hb,
 	obj->oid = *oidp;
 	obj->objnode_count = 0;
 	obj->pampd_count = 0;
-	(*tmem_pamops.new_obj)(obj);
+#ifdef CONFIG_RAMSTER
+	if (tmem_pamops.new_obj != NULL)
+		(*tmem_pamops.new_obj)(obj);
+#endif
 	SET_SENTINEL(obj, OBJ);
-	while (*new) {
-		BUG_ON(RB_EMPTY_NODE(*new));
-		this = rb_entry(*new, struct tmem_obj, rb_tree_node);
-		parent = *new;
-		switch (tmem_oid_compare(oidp, &this->oid)) {
-		case 0:
-			BUG(); /* already present; should never happen! */
-			break;
-		case -1:
-			new = &(*new)->rb_left;
-			break;
-		case 1:
-			new = &(*new)->rb_right;
-			break;
-		}
-	}
+
+	if (__tmem_obj_find(hb, oidp, &parent, &new))
+		BUG();
+
 	rb_link_node(&obj->rb_tree_node, parent, new);
 	rb_insert_color(&obj->rb_tree_node, root);
 }
@@ -170,7 +190,7 @@ static void tmem_obj_init(struct tmem_obj *obj, struct tmem_hashbucket *hb,
  * "ephemeral" vs "persistent".  These attributes apply to all tmem_objs
  * and all pampds that belong to a tmem_pool.  A tmem_pool is created
  * or deleted relatively rarely (for example, when a filesystem is
- * mounted or unmounted.
+ * mounted or unmounted).
  */
 
 /* flush all data from a pool and, optionally, free it */
@@ -188,7 +208,7 @@ static void tmem_pool_flush(struct tmem_pool *pool, bool destroy)
 		while (rbnode != NULL) {
 			obj = rb_entry(rbnode, struct tmem_obj, rb_tree_node);
 			rbnode = rb_next(rbnode);
-			tmem_pampd_destroy_all_in_obj(obj);
+			tmem_pampd_destroy_all_in_obj(obj, true);
 			tmem_obj_free(obj, hb);
 			(*tmem_hostops.obj_free)(obj, pool);
 		}
@@ -274,7 +294,7 @@ static void tmem_objnode_free(struct tmem_objnode *objnode)
 }
 
 /*
- * lookup index in object and return associated pampd (or NULL if not found)
+ * Lookup index in object and return associated pampd (or NULL if not found).
  */
 static void **__tmem_pampd_lookup_in_obj(struct tmem_obj *obj, uint32_t index)
 {
@@ -316,6 +336,7 @@ static void *tmem_pampd_lookup_in_obj(struct tmem_obj *obj, uint32_t index)
 	return slot != NULL ? *slot : NULL;
 }
 
+#ifdef CONFIG_RAMSTER
 static void *tmem_pampd_replace_in_obj(struct tmem_obj *obj, uint32_t index,
 					void *new_pampd, bool no_free)
 {
@@ -333,6 +354,7 @@ static void *tmem_pampd_replace_in_obj(struct tmem_obj *obj, uint32_t index,
 	}
 	return ret;
 }
+#endif
 
 static int tmem_pampd_add_to_obj(struct tmem_obj *obj, uint32_t index,
 					void *pampd)
@@ -470,7 +492,7 @@ out:
 	return slot;
 }
 
-/* recursively walk the objnode_tree destroying pampds and objnodes */
+/* Recursively walk the objnode_tree destroying pampds and objnodes. */
 static void tmem_objnode_node_destroy(struct tmem_obj *obj,
 					struct tmem_objnode *objnode,
 					unsigned int ht)
@@ -495,7 +517,8 @@ static void tmem_objnode_node_destroy(struct tmem_obj *obj,
 	}
 }
 
-static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *obj)
+static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *obj,
+						bool pool_destroy)
 {
 	if (obj->objnode_tree_root == NULL)
 		return;
@@ -510,7 +533,10 @@ static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *obj)
 		obj->objnode_tree_height = 0;
 	}
 	obj->objnode_tree_root = NULL;
-	(*tmem_pamops.free_obj)(obj->pool, obj);
+#ifdef CONFIG_RAMSTER
+	if (tmem_pamops.free_obj != NULL)
+		(*tmem_pamops.free_obj)(obj->pool, obj, pool_destroy);
+#endif
 }
 
 /*
@@ -523,17 +549,16 @@ static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *obj)
  */
 
 /*
- * "Put" a page, e.g. copy a page from the kernel into newly allocated
- * PAM space (if such space is available).  Tmem_put is complicated by
- * a corner case: What if a page with matching handle already exists in
- * tmem?  To guarantee coherency, one of two actions is necessary: Either
- * the data for the page must be overwritten, or the page must be
- * "flushed" so that the data is not accessible to a subsequent "get".
- * Since these "duplicate puts" are relatively rare, this implementation
- * always flushes for simplicity.
+ * "Put" a page, e.g. associate the passed pampd with the passed handle.
+ * Tmem_put is complicated by a corner case: What if a page with matching
+ * handle already exists in tmem?  To guarantee coherency, one of two
+ * actions is necessary: Either the data for the page must be overwritten,
+ * or the page must be "flushed" so that the data is not accessible to a
+ * subsequent "get".  Since these "duplicate puts" are relatively rare,
+ * this implementation always flushes for simplicity.
  */
 int tmem_put(struct tmem_pool *pool, struct tmem_oid *oidp, uint32_t index,
-		char *data, size_t size, bool raw, int ephemeral)
+		bool raw, void *pampd_to_use)
 {
 	struct tmem_obj *obj = NULL, *objfound = NULL, *objnew = NULL;
 	void *pampd = NULL, *pampd_del = NULL;
@@ -566,19 +591,17 @@ int tmem_put(struct tmem_pool *pool, struct tmem_oid *oidp, uint32_t index,
 	}
 	BUG_ON(obj == NULL);
 	BUG_ON(((objnew != obj) && (objfound != obj)) || (objnew == objfound));
-	pampd = (*tmem_pamops.create)(data, size, raw, ephemeral,
-					obj->pool, &obj->oid, index);
-	if (unlikely(pampd == NULL))
-		goto free;
+	pampd = pampd_to_use;
+	BUG_ON(pampd_to_use == NULL);
 	ret = tmem_pampd_add_to_obj(obj, index, pampd);
 	if (unlikely(ret == -ENOMEM))
 		/* may have partially built objnode tree ("stump") */
 		goto delete_and_free;
+	(*tmem_pamops.create_finish)(pampd, is_ephemeral(pool));
 	goto out;
 
 delete_and_free:
 	(void)tmem_pampd_delete_from_obj(obj, index);
-free:
 	if (pampd)
 		(*tmem_pamops.free)(pampd, pool, NULL, 0, true);
 	if (objnew) {
@@ -590,6 +613,16 @@ out:
 	return ret;
 }
 
+#ifdef CONFIG_RAMSTER
+/*
+ * For ramster only:  The following routines provide a two-step sequence
+ * to allow the caller to replace a pampd in the tmem data structures with
+ * another pampd. Here, we lookup the passed handle and, if found, return the
+ * associated pampd and object, leaving the hashbucket locked and returning
+ * a reference to it.  The caller is expected to immediately call the
+ * matching tmem_localify_finish routine which will handles the replacement
+ * and unlocks the hashbucket.
+ */
 void *tmem_localify_get_pampd(struct tmem_pool *pool, struct tmem_oid *oidp,
 				uint32_t index, struct tmem_obj **ret_obj,
 				void **saved_hb)
@@ -618,6 +651,7 @@ void tmem_localify_finish(struct tmem_obj *obj, uint32_t index,
 	if (pampd != NULL) {
 		BUG_ON(obj == NULL);
 		(void)tmem_pampd_replace_in_obj(obj, index, pampd, 1);
+		(*tmem_pamops.create_finish)(pampd, is_ephemeral(obj->pool));
 	} else if (delete) {
 		BUG_ON(obj == NULL);
 		(void)tmem_pampd_delete_from_obj(obj, index);
@@ -625,6 +659,9 @@ void tmem_localify_finish(struct tmem_obj *obj, uint32_t index,
 	spin_unlock(&hb->lock);
 }
 
+/*
+ * For ramster only.  Helper function to support asynchronous tmem_get.
+ */
 static int tmem_repatriate(void **ppampd, struct tmem_hashbucket *hb,
 				struct tmem_pool *pool, struct tmem_oid *oidp,
 				uint32_t index, bool free, char *data)
@@ -633,7 +670,6 @@ static int tmem_repatriate(void **ppampd, struct tmem_hashbucket *hb,
 	bool intransit = false;
 	int ret = 0;
 
-
 	if (!is_ephemeral(pool))
 		new_pampd = (*tmem_pamops.repatriate_preload)(
 				old_pampd, pool, oidp, index, &intransit);
@@ -646,60 +682,91 @@ static int tmem_repatriate(void **ppampd, struct tmem_hashbucket *hb,
 	if (!intransit)
 		ret = (*tmem_pamops.repatriate)(old_pampd, new_pampd, pool,
 						oidp, index, free, data);
+	if (ret == -EAGAIN) {
+		/* rare I think, but should cond_resched()??? */
+		usleep_range(10, 1000);
+	} else if (ret == -ENOTCONN || ret == -EHOSTDOWN) {
+		ret = -1;
+	} else if (ret != 0 && ret != -ENOENT) {
+		ret = -1;
+	}
+	/* note hb->lock has now been unlocked */
+	return ret;
+}
+
+/*
+ * For ramster only.  If a page in tmem matches the handle, replace the
+ * page so that any subsequent "get" gets the new page.  Returns 0 if
+ * there was a page to replace, else returns -1.
+ */
+int tmem_replace(struct tmem_pool *pool, struct tmem_oid *oidp,
+			uint32_t index, void *new_pampd)
+{
+	struct tmem_obj *obj;
+	int ret = -1;
+	struct tmem_hashbucket *hb;
+
+	hb = &pool->hashbucket[tmem_oid_hash(oidp)];
+	spin_lock(&hb->lock);
+	obj = tmem_obj_find(hb, oidp);
+	if (obj == NULL)
+		goto out;
+	new_pampd = tmem_pampd_replace_in_obj(obj, index, new_pampd, 0);
+	/* if we bug here, pamops wasn't properly set up for ramster */
+	BUG_ON(tmem_pamops.replace_in_obj == NULL);
+	ret = (*tmem_pamops.replace_in_obj)(new_pampd, obj);
+out:
+	spin_unlock(&hb->lock);
 	return ret;
 }
+#endif
 
 /*
- * "Get" a page, e.g. if one can be found, copy the tmem page with the
- * matching handle from PAM space to the kernel.  By tmem definition,
- * when a "get" is successful on an ephemeral page, the page is "flushed",
- * and when a "get" is successful on a persistent page, the page is retained
- * in tmem.  Note that to preserve
+ * "Get" a page, e.g. if a pampd can be found matching the passed handle,
+ * use a pamops callback to recreated the page from the pampd with the
+ * matching handle.  By tmem definition, when a "get" is successful on
+ * an ephemeral page, the page is "flushed", and when a "get" is successful
+ * on a persistent page, the page is retained in tmem.  Note that to preserve
  * coherency, "get" can never be skipped if tmem contains the data.
  * That is, if a get is done with a certain handle and fails, any
  * subsequent "get" must also fail (unless of course there is a
  * "put" done with the same handle).
-
  */
 int tmem_get(struct tmem_pool *pool, struct tmem_oid *oidp, uint32_t index,
-		char *data, size_t *size, bool raw, int get_and_free)
+		char *data, size_t *sizep, bool raw, int get_and_free)
 {
 	struct tmem_obj *obj;
-	void *pampd;
+	void *pampd = NULL;
 	bool ephemeral = is_ephemeral(pool);
 	int ret = -1;
 	struct tmem_hashbucket *hb;
 	bool free = (get_and_free == 1) || ((get_and_free == 0) && ephemeral);
-	bool lock_held = 0;
+	bool lock_held = false;
 	void **ppampd;
 
-again:
-	hb = &pool->hashbucket[tmem_oid_hash(oidp)];
-	spin_lock(&hb->lock);
-	lock_held = 1;
-	obj = tmem_obj_find(hb, oidp);
-	if (obj == NULL)
-		goto out;
-	ppampd = __tmem_pampd_lookup_in_obj(obj, index);
-	if (ppampd == NULL)
-		goto out;
-	if (tmem_pamops.is_remote(*ppampd)) {
-		ret = tmem_repatriate(ppampd, hb, pool, oidp,
-					index, free, data);
-		lock_held = 0; /* note hb->lock has been unlocked */
-		if (ret == -EAGAIN) {
-			/* rare I think, but should cond_resched()??? */
-			usleep_range(10, 1000);
-			goto again;
-		} else if (ret != 0) {
-			if (ret != -ENOENT)
-				pr_err("UNTESTED case in tmem_get, ret=%d\n",
-						ret);
-			ret = -1;
+	do {
+		hb = &pool->hashbucket[tmem_oid_hash(oidp)];
+		spin_lock(&hb->lock);
+		lock_held = true;
+		obj = tmem_obj_find(hb, oidp);
+		if (obj == NULL)
+			goto out;
+		ppampd = __tmem_pampd_lookup_in_obj(obj, index);
+		if (ppampd == NULL)
 			goto out;
+#ifdef CONFIG_RAMSTER
+		if ((tmem_pamops.is_remote != NULL) &&
+		     tmem_pamops.is_remote(*ppampd)) {
+			ret = tmem_repatriate(ppampd, hb, pool, oidp,
+						index, free, data);
+			/* tmem_repatriate releases hb->lock */
+			lock_held = false;
+			*sizep = PAGE_SIZE;
+			if (ret != -EAGAIN)
+				goto out;
 		}
-		goto out;
-	}
+#endif
+	} while (ret == -EAGAIN);
 	if (free)
 		pampd = tmem_pampd_delete_from_obj(obj, index);
 	else
@@ -715,10 +782,10 @@ again:
 	}
 	if (free)
 		ret = (*tmem_pamops.get_data_and_free)(
-				data, size, raw, pampd, pool, oidp, index);
+				data, sizep, raw, pampd, pool, oidp, index);
 	else
 		ret = (*tmem_pamops.get_data)(
-				data, size, raw, pampd, pool, oidp, index);
+				data, sizep, raw, pampd, pool, oidp, index);
 	if (ret < 0)
 		goto out;
 	ret = 0;
@@ -762,30 +829,6 @@ out:
 }
 
 /*
- * If a page in tmem matches the handle, replace the page so that any
- * subsequent "get" gets the new page.  Returns the new page if
- * there was a page to replace, else returns NULL.
- */
-int tmem_replace(struct tmem_pool *pool, struct tmem_oid *oidp,
-			uint32_t index, void *new_pampd)
-{
-	struct tmem_obj *obj;
-	int ret = -1;
-	struct tmem_hashbucket *hb;
-
-	hb = &pool->hashbucket[tmem_oid_hash(oidp)];
-	spin_lock(&hb->lock);
-	obj = tmem_obj_find(hb, oidp);
-	if (obj == NULL)
-		goto out;
-	new_pampd = tmem_pampd_replace_in_obj(obj, index, new_pampd, 0);
-	ret = (*tmem_pamops.replace_in_obj)(new_pampd, obj);
-out:
-	spin_unlock(&hb->lock);
-	return ret;
-}
-
-/*
  * "Flush" all pages in tmem matching this oid.
  */
 int tmem_flush_object(struct tmem_pool *pool, struct tmem_oid *oidp)
@@ -799,7 +842,7 @@ int tmem_flush_object(struct tmem_pool *pool, struct tmem_oid *oidp)
 	obj = tmem_obj_find(hb, oidp);
 	if (obj == NULL)
 		goto out;
-	tmem_pampd_destroy_all_in_obj(obj);
+	tmem_pampd_destroy_all_in_obj(obj, false);
 	tmem_obj_free(obj, hb);
 	(*tmem_hostops.obj_free)(obj, pool);
 	ret = 0;
diff --git a/drivers/staging/ramster/tmem.h b/drivers/staging/ramster/tmem.h
index 47f1918c8314..adbe5a8f28aa 100644
--- a/drivers/staging/ramster/tmem.h
+++ b/drivers/staging/ramster/tmem.h
@@ -3,23 +3,20 @@
  *
  * Transcendent memory
  *
- * Copyright (c) 2009-2011, Dan Magenheimer, Oracle Corp.
+ * Copyright (c) 2009-2012, Dan Magenheimer, Oracle Corp.
  */
 
 #ifndef _TMEM_H_
 #define _TMEM_H_
 
+#include <linux/types.h>
 #include <linux/highmem.h>
 #include <linux/hash.h>
 #include <linux/atomic.h>
 
 /*
- * These are pre-defined by the Xen<->Linux ABI
+ * These are defined by the Xen<->Linux ABI so should remain consistent
  */
-#define TMEM_PUT_PAGE			4
-#define TMEM_GET_PAGE			5
-#define TMEM_FLUSH_PAGE			6
-#define TMEM_FLUSH_OBJECT		7
 #define TMEM_POOL_PERSIST		1
 #define TMEM_POOL_SHARED		2
 #define TMEM_POOL_PRECOMPRESSED		4
@@ -31,7 +28,7 @@
  * sentinels have proven very useful for debugging but can be removed
  * or disabled before final merge.
  */
-#define SENTINELS
+#undef SENTINELS
 #ifdef SENTINELS
 #define DECL_SENTINEL uint32_t sentinel;
 #define SET_SENTINEL(_x, _y) (_x->sentinel = _y##_SENTINEL)
@@ -46,7 +43,7 @@
 #define ASSERT_INVERTED_SENTINEL(_x, _y) do { } while (0)
 #endif
 
-#define ASSERT_SPINLOCK(_l)	WARN_ON(!spin_is_locked(_l))
+#define ASSERT_SPINLOCK(_l)	lockdep_assert_held(_l)
 
 /*
  * A pool is the highest-level data structure managed by tmem and
@@ -88,31 +85,6 @@ struct tmem_oid {
 	uint64_t oid[3];
 };
 
-struct tmem_xhandle {
-	uint8_t client_id;
-	uint8_t xh_data_cksum;
-	uint16_t xh_data_size;
-	uint16_t pool_id;
-	struct tmem_oid oid;
-	uint32_t index;
-	void *extra;
-};
-
-static inline struct tmem_xhandle tmem_xhandle_fill(uint16_t client_id,
-					struct tmem_pool *pool,
-					struct tmem_oid *oidp,
-					uint32_t index)
-{
-	struct tmem_xhandle xh;
-	xh.client_id = client_id;
-	xh.xh_data_cksum = (uint8_t)-1;
-	xh.xh_data_size = (uint16_t)-1;
-	xh.pool_id = pool->pool_id;
-	xh.oid = *oidp;
-	xh.index = index;
-	return xh;
-}
-
 static inline void tmem_oid_set_invalid(struct tmem_oid *oidp)
 {
 	oidp->oid[0] = oidp->oid[1] = oidp->oid[2] = -1UL;
@@ -154,6 +126,34 @@ static inline unsigned tmem_oid_hash(struct tmem_oid *oidp)
 				TMEM_HASH_BUCKET_BITS);
 }
 
+#ifdef CONFIG_RAMSTER
+struct tmem_xhandle {
+	uint8_t client_id;
+	uint8_t xh_data_cksum;
+	uint16_t xh_data_size;
+	uint16_t pool_id;
+	struct tmem_oid oid;
+	uint32_t index;
+	void *extra;
+};
+
+static inline struct tmem_xhandle tmem_xhandle_fill(uint16_t client_id,
+					struct tmem_pool *pool,
+					struct tmem_oid *oidp,
+					uint32_t index)
+{
+	struct tmem_xhandle xh;
+	xh.client_id = client_id;
+	xh.xh_data_cksum = (uint8_t)-1;
+	xh.xh_data_size = (uint16_t)-1;
+	xh.pool_id = pool->pool_id;
+	xh.oid = *oidp;
+	xh.index = index;
+	return xh;
+}
+#endif
+
+
 /*
  * A tmem_obj contains an identifier (oid), pointers to the parent
  * pool and the rb_tree to which it belongs, counters, and an ordered
@@ -171,11 +171,15 @@ struct tmem_obj {
 	unsigned int objnode_tree_height;
 	unsigned long objnode_count;
 	long pampd_count;
-	/* for current design of ramster, all pages belonging to
+#ifdef CONFIG_RAMSTER
+	/*
+	 * for current design of ramster, all pages belonging to
 	 * an object reside on the same remotenode and extra is
 	 * used to record the number of the remotenode so a
-	 * flush-object operation can specify it */
-	void *extra; /* for use by pampd implementation */
+	 * flush-object operation can specify it
+	 */
+	void *extra; /* for private use by pampd implementation */
+#endif
 	DECL_SENTINEL
 };
 
@@ -193,10 +197,17 @@ struct tmem_objnode {
 	unsigned int slots_in_use;
 };
 
+struct tmem_handle {
+	struct tmem_oid oid; /* 24 bytes */
+	uint32_t index;
+	uint16_t pool_id;
+	uint16_t client_id;
+};
+
+
 /* pampd abstract datatype methods provided by the PAM implementation */
 struct tmem_pamops {
-	void *(*create)(char *, size_t, bool, int,
-			struct tmem_pool *, struct tmem_oid *, uint32_t);
+	void (*create_finish)(void *, bool);
 	int (*get_data)(char *, size_t *, bool, void *, struct tmem_pool *,
 				struct tmem_oid *, uint32_t);
 	int (*get_data_and_free)(char *, size_t *, bool, void *,
@@ -204,14 +215,16 @@ struct tmem_pamops {
 				uint32_t);
 	void (*free)(void *, struct tmem_pool *,
 				struct tmem_oid *, uint32_t, bool);
-	void (*free_obj)(struct tmem_pool *, struct tmem_obj *);
-	bool (*is_remote)(void *);
+#ifdef CONFIG_RAMSTER
+	void (*new_obj)(struct tmem_obj *);
+	void (*free_obj)(struct tmem_pool *, struct tmem_obj *, bool);
 	void *(*repatriate_preload)(void *, struct tmem_pool *,
 					struct tmem_oid *, uint32_t, bool *);
 	int (*repatriate)(void *, void *, struct tmem_pool *,
 				struct tmem_oid *, uint32_t, bool, void *);
-	void (*new_obj)(struct tmem_obj *);
+	bool (*is_remote)(void *);
 	int (*replace_in_obj)(void *, struct tmem_obj *);
+#endif
 };
 extern void tmem_register_pamops(struct tmem_pamops *m);
 
@@ -226,9 +239,15 @@ extern void tmem_register_hostops(struct tmem_hostops *m);
 
 /* core tmem accessor functions */
 extern int tmem_put(struct tmem_pool *, struct tmem_oid *, uint32_t index,
-			char *, size_t, bool, int);
+			bool, void *);
 extern int tmem_get(struct tmem_pool *, struct tmem_oid *, uint32_t index,
 			char *, size_t *, bool, int);
+extern int tmem_flush_page(struct tmem_pool *, struct tmem_oid *,
+			uint32_t index);
+extern int tmem_flush_object(struct tmem_pool *, struct tmem_oid *);
+extern int tmem_destroy_pool(struct tmem_pool *);
+extern void tmem_new_pool(struct tmem_pool *, uint32_t);
+#ifdef CONFIG_RAMSTER
 extern int tmem_replace(struct tmem_pool *, struct tmem_oid *, uint32_t index,
 			void *);
 extern void *tmem_localify_get_pampd(struct tmem_pool *, struct tmem_oid *,
@@ -236,9 +255,5 @@ extern void *tmem_localify_get_pampd(struct tmem_pool *, struct tmem_oid *,
 				   void **);
 extern void tmem_localify_finish(struct tmem_obj *, uint32_t index,
 				 void *, void *, bool);
-extern int tmem_flush_page(struct tmem_pool *, struct tmem_oid *,
-			uint32_t index);
-extern int tmem_flush_object(struct tmem_pool *, struct tmem_oid *);
-extern int tmem_destroy_pool(struct tmem_pool *);
-extern void tmem_new_pool(struct tmem_pool *, uint32_t);
+#endif
 #endif /* _TMEM_H */
diff --git a/drivers/staging/ramster/xvmalloc.c b/drivers/staging/ramster/xvmalloc.c
deleted file mode 100644
index 44ceb0b823a9..000000000000
--- a/drivers/staging/ramster/xvmalloc.c
+++ /dev/null
@@ -1,509 +0,0 @@
-/*
- * xvmalloc memory allocator
- *
- * Copyright (C) 2008, 2009, 2010  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the licence that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-#ifdef CONFIG_ZRAM_DEBUG
-#define DEBUG
-#endif
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/bitops.h>
-#include <linux/errno.h>
-#include <linux/highmem.h>
-#include <linux/init.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-
-#include "xvmalloc.h"
-#include "xvmalloc_int.h"
-
-static void stat_inc(u64 *value)
-{
-	*value = *value + 1;
-}
-
-static void stat_dec(u64 *value)
-{
-	*value = *value - 1;
-}
-
-static int test_flag(struct block_header *block, enum blockflags flag)
-{
-	return block->prev & BIT(flag);
-}
-
-static void set_flag(struct block_header *block, enum blockflags flag)
-{
-	block->prev |= BIT(flag);
-}
-
-static void clear_flag(struct block_header *block, enum blockflags flag)
-{
-	block->prev &= ~BIT(flag);
-}
-
-/*
- * Given <page, offset> pair, provide a dereferencable pointer.
- * This is called from xv_malloc/xv_free path, so it
- * needs to be fast.
- */
-static void *get_ptr_atomic(struct page *page, u16 offset)
-{
-	unsigned char *base;
-
-	base = kmap_atomic(page);
-	return base + offset;
-}
-
-static void put_ptr_atomic(void *ptr)
-{
-	kunmap_atomic(ptr);
-}
-
-static u32 get_blockprev(struct block_header *block)
-{
-	return block->prev & PREV_MASK;
-}
-
-static void set_blockprev(struct block_header *block, u16 new_offset)
-{
-	block->prev = new_offset | (block->prev & FLAGS_MASK);
-}
-
-static struct block_header *BLOCK_NEXT(struct block_header *block)
-{
-	return (struct block_header *)
-		((char *)block + block->size + XV_ALIGN);
-}
-
-/*
- * Get index of free list containing blocks of maximum size
- * which is less than or equal to given size.
- */
-static u32 get_index_for_insert(u32 size)
-{
-	if (unlikely(size > XV_MAX_ALLOC_SIZE))
-		size = XV_MAX_ALLOC_SIZE;
-	size &= ~FL_DELTA_MASK;
-	return (size - XV_MIN_ALLOC_SIZE) >> FL_DELTA_SHIFT;
-}
-
-/*
- * Get index of free list having blocks of size greater than
- * or equal to requested size.
- */
-static u32 get_index(u32 size)
-{
-	if (unlikely(size < XV_MIN_ALLOC_SIZE))
-		size = XV_MIN_ALLOC_SIZE;
-	size = ALIGN(size, FL_DELTA);
-	return (size - XV_MIN_ALLOC_SIZE) >> FL_DELTA_SHIFT;
-}
-
-/**
- * find_block - find block of at least given size
- * @pool: memory pool to search from
- * @size: size of block required
- * @page: page containing required block
- * @offset: offset within the page where block is located.
- *
- * Searches two level bitmap to locate block of at least
- * the given size. If such a block is found, it provides
- * <page, offset> to identify this block and returns index
- * in freelist where we found this block.
- * Otherwise, returns 0 and <page, offset> params are not touched.
- */
-static u32 find_block(struct xv_pool *pool, u32 size,
-			struct page **page, u32 *offset)
-{
-	ulong flbitmap, slbitmap;
-	u32 flindex, slindex, slbitstart;
-
-	/* There are no free blocks in this pool */
-	if (!pool->flbitmap)
-		return 0;
-
-	/* Get freelist index corresponding to this size */
-	slindex = get_index(size);
-	slbitmap = pool->slbitmap[slindex / BITS_PER_LONG];
-	slbitstart = slindex % BITS_PER_LONG;
-
-	/*
-	 * If freelist is not empty at this index, we found the
-	 * block - head of this list. This is approximate best-fit match.
-	 */
-	if (test_bit(slbitstart, &slbitmap)) {
-		*page = pool->freelist[slindex].page;
-		*offset = pool->freelist[slindex].offset;
-		return slindex;
-	}
-
-	/*
-	 * No best-fit found. Search a bit further in bitmap for a free block.
-	 * Second level bitmap consists of series of 32-bit chunks. Search
-	 * further in the chunk where we expected a best-fit, starting from
-	 * index location found above.
-	 */
-	slbitstart++;
-	slbitmap >>= slbitstart;
-
-	/* Skip this search if we were already at end of this bitmap chunk */
-	if ((slbitstart != BITS_PER_LONG) && slbitmap) {
-		slindex += __ffs(slbitmap) + 1;
-		*page = pool->freelist[slindex].page;
-		*offset = pool->freelist[slindex].offset;
-		return slindex;
-	}
-
-	/* Now do a full two-level bitmap search to find next nearest fit */
-	flindex = slindex / BITS_PER_LONG;
-
-	flbitmap = (pool->flbitmap) >> (flindex + 1);
-	if (!flbitmap)
-		return 0;
-
-	flindex += __ffs(flbitmap) + 1;
-	slbitmap = pool->slbitmap[flindex];
-	slindex = (flindex * BITS_PER_LONG) + __ffs(slbitmap);
-	*page = pool->freelist[slindex].page;
-	*offset = pool->freelist[slindex].offset;
-
-	return slindex;
-}
-
-/*
- * Insert block at <page, offset> in freelist of given pool.
- * freelist used depends on block size.
- */
-static void insert_block(struct xv_pool *pool, struct page *page, u32 offset,
-			struct block_header *block)
-{
-	u32 flindex, slindex;
-	struct block_header *nextblock;
-
-	slindex = get_index_for_insert(block->size);
-	flindex = slindex / BITS_PER_LONG;
-
-	block->link.prev_page = NULL;
-	block->link.prev_offset = 0;
-	block->link.next_page = pool->freelist[slindex].page;
-	block->link.next_offset = pool->freelist[slindex].offset;
-	pool->freelist[slindex].page = page;
-	pool->freelist[slindex].offset = offset;
-
-	if (block->link.next_page) {
-		nextblock = get_ptr_atomic(block->link.next_page,
-					block->link.next_offset);
-		nextblock->link.prev_page = page;
-		nextblock->link.prev_offset = offset;
-		put_ptr_atomic(nextblock);
-		/* If there was a next page then the free bits are set. */
-		return;
-	}
-
-	__set_bit(slindex % BITS_PER_LONG, &pool->slbitmap[flindex]);
-	__set_bit(flindex, &pool->flbitmap);
-}
-
-/*
- * Remove block from freelist. Index 'slindex' identifies the freelist.
- */
-static void remove_block(struct xv_pool *pool, struct page *page, u32 offset,
-			struct block_header *block, u32 slindex)
-{
-	u32 flindex = slindex / BITS_PER_LONG;
-	struct block_header *tmpblock;
-
-	if (block->link.prev_page) {
-		tmpblock = get_ptr_atomic(block->link.prev_page,
-				block->link.prev_offset);
-		tmpblock->link.next_page = block->link.next_page;
-		tmpblock->link.next_offset = block->link.next_offset;
-		put_ptr_atomic(tmpblock);
-	}
-
-	if (block->link.next_page) {
-		tmpblock = get_ptr_atomic(block->link.next_page,
-				block->link.next_offset);
-		tmpblock->link.prev_page = block->link.prev_page;
-		tmpblock->link.prev_offset = block->link.prev_offset;
-		put_ptr_atomic(tmpblock);
-	}
-
-	/* Is this block is at the head of the freelist? */
-	if (pool->freelist[slindex].page == page
-	   && pool->freelist[slindex].offset == offset) {
-
-		pool->freelist[slindex].page = block->link.next_page;
-		pool->freelist[slindex].offset = block->link.next_offset;
-
-		if (pool->freelist[slindex].page) {
-			struct block_header *tmpblock;
-			tmpblock = get_ptr_atomic(pool->freelist[slindex].page,
-					pool->freelist[slindex].offset);
-			tmpblock->link.prev_page = NULL;
-			tmpblock->link.prev_offset = 0;
-			put_ptr_atomic(tmpblock);
-		} else {
-			/* This freelist bucket is empty */
-			__clear_bit(slindex % BITS_PER_LONG,
-				    &pool->slbitmap[flindex]);
-			if (!pool->slbitmap[flindex])
-				__clear_bit(flindex, &pool->flbitmap);
-		}
-	}
-
-	block->link.prev_page = NULL;
-	block->link.prev_offset = 0;
-	block->link.next_page = NULL;
-	block->link.next_offset = 0;
-}
-
-/*
- * Allocate a page and add it to freelist of given pool.
- */
-static int grow_pool(struct xv_pool *pool, gfp_t flags)
-{
-	struct page *page;
-	struct block_header *block;
-
-	page = alloc_page(flags);
-	if (unlikely(!page))
-		return -ENOMEM;
-
-	stat_inc(&pool->total_pages);
-
-	spin_lock(&pool->lock);
-	block = get_ptr_atomic(page, 0);
-
-	block->size = PAGE_SIZE - XV_ALIGN;
-	set_flag(block, BLOCK_FREE);
-	clear_flag(block, PREV_FREE);
-	set_blockprev(block, 0);
-
-	insert_block(pool, page, 0, block);
-
-	put_ptr_atomic(block);
-	spin_unlock(&pool->lock);
-
-	return 0;
-}
-
-/*
- * Create a memory pool. Allocates freelist, bitmaps and other
- * per-pool metadata.
- */
-struct xv_pool *xv_create_pool(void)
-{
-	u32 ovhd_size;
-	struct xv_pool *pool;
-
-	ovhd_size = roundup(sizeof(*pool), PAGE_SIZE);
-	pool = kzalloc(ovhd_size, GFP_KERNEL);
-	if (!pool)
-		return NULL;
-
-	spin_lock_init(&pool->lock);
-
-	return pool;
-}
-EXPORT_SYMBOL_GPL(xv_create_pool);
-
-void xv_destroy_pool(struct xv_pool *pool)
-{
-	kfree(pool);
-}
-EXPORT_SYMBOL_GPL(xv_destroy_pool);
-
-/**
- * xv_malloc - Allocate block of given size from pool.
- * @pool: pool to allocate from
- * @size: size of block to allocate
- * @page: page no. that holds the object
- * @offset: location of object within page
- *
- * On success, <page, offset> identifies block allocated
- * and 0 is returned. On failure, <page, offset> is set to
- * 0 and -ENOMEM is returned.
- *
- * Allocation requests with size > XV_MAX_ALLOC_SIZE will fail.
- */
-int xv_malloc(struct xv_pool *pool, u32 size, struct page **page,
-		u32 *offset, gfp_t flags)
-{
-	int error;
-	u32 index, tmpsize, origsize, tmpoffset;
-	struct block_header *block, *tmpblock;
-
-	*page = NULL;
-	*offset = 0;
-	origsize = size;
-
-	if (unlikely(!size || size > XV_MAX_ALLOC_SIZE))
-		return -ENOMEM;
-
-	size = ALIGN(size, XV_ALIGN);
-
-	spin_lock(&pool->lock);
-
-	index = find_block(pool, size, page, offset);
-
-	if (!*page) {
-		spin_unlock(&pool->lock);
-		if (flags & GFP_NOWAIT)
-			return -ENOMEM;
-		error = grow_pool(pool, flags);
-		if (unlikely(error))
-			return error;
-
-		spin_lock(&pool->lock);
-		index = find_block(pool, size, page, offset);
-	}
-
-	if (!*page) {
-		spin_unlock(&pool->lock);
-		return -ENOMEM;
-	}
-
-	block = get_ptr_atomic(*page, *offset);
-
-	remove_block(pool, *page, *offset, block, index);
-
-	/* Split the block if required */
-	tmpoffset = *offset + size + XV_ALIGN;
-	tmpsize = block->size - size;
-	tmpblock = (struct block_header *)((char *)block + size + XV_ALIGN);
-	if (tmpsize) {
-		tmpblock->size = tmpsize - XV_ALIGN;
-		set_flag(tmpblock, BLOCK_FREE);
-		clear_flag(tmpblock, PREV_FREE);
-
-		set_blockprev(tmpblock, *offset);
-		if (tmpblock->size >= XV_MIN_ALLOC_SIZE)
-			insert_block(pool, *page, tmpoffset, tmpblock);
-
-		if (tmpoffset + XV_ALIGN + tmpblock->size != PAGE_SIZE) {
-			tmpblock = BLOCK_NEXT(tmpblock);
-			set_blockprev(tmpblock, tmpoffset);
-		}
-	} else {
-		/* This block is exact fit */
-		if (tmpoffset != PAGE_SIZE)
-			clear_flag(tmpblock, PREV_FREE);
-	}
-
-	block->size = origsize;
-	clear_flag(block, BLOCK_FREE);
-
-	put_ptr_atomic(block);
-	spin_unlock(&pool->lock);
-
-	*offset += XV_ALIGN;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(xv_malloc);
-
-/*
- * Free block identified with <page, offset>
- */
-void xv_free(struct xv_pool *pool, struct page *page, u32 offset)
-{
-	void *page_start;
-	struct block_header *block, *tmpblock;
-
-	offset -= XV_ALIGN;
-
-	spin_lock(&pool->lock);
-
-	page_start = get_ptr_atomic(page, 0);
-	block = (struct block_header *)((char *)page_start + offset);
-
-	/* Catch double free bugs */
-	BUG_ON(test_flag(block, BLOCK_FREE));
-
-	block->size = ALIGN(block->size, XV_ALIGN);
-
-	tmpblock = BLOCK_NEXT(block);
-	if (offset + block->size + XV_ALIGN == PAGE_SIZE)
-		tmpblock = NULL;
-
-	/* Merge next block if its free */
-	if (tmpblock && test_flag(tmpblock, BLOCK_FREE)) {
-		/*
-		 * Blocks smaller than XV_MIN_ALLOC_SIZE
-		 * are not inserted in any free list.
-		 */
-		if (tmpblock->size >= XV_MIN_ALLOC_SIZE) {
-			remove_block(pool, page,
-				    offset + block->size + XV_ALIGN, tmpblock,
-				    get_index_for_insert(tmpblock->size));
-		}
-		block->size += tmpblock->size + XV_ALIGN;
-	}
-
-	/* Merge previous block if its free */
-	if (test_flag(block, PREV_FREE)) {
-		tmpblock = (struct block_header *)((char *)(page_start) +
-						get_blockprev(block));
-		offset = offset - tmpblock->size - XV_ALIGN;
-
-		if (tmpblock->size >= XV_MIN_ALLOC_SIZE)
-			remove_block(pool, page, offset, tmpblock,
-				    get_index_for_insert(tmpblock->size));
-
-		tmpblock->size += block->size + XV_ALIGN;
-		block = tmpblock;
-	}
-
-	/* No used objects in this page. Free it. */
-	if (block->size == PAGE_SIZE - XV_ALIGN) {
-		put_ptr_atomic(page_start);
-		spin_unlock(&pool->lock);
-
-		__free_page(page);
-		stat_dec(&pool->total_pages);
-		return;
-	}
-
-	set_flag(block, BLOCK_FREE);
-	if (block->size >= XV_MIN_ALLOC_SIZE)
-		insert_block(pool, page, offset, block);
-
-	if (offset + block->size + XV_ALIGN != PAGE_SIZE) {
-		tmpblock = BLOCK_NEXT(block);
-		set_flag(tmpblock, PREV_FREE);
-		set_blockprev(tmpblock, offset);
-	}
-
-	put_ptr_atomic(page_start);
-	spin_unlock(&pool->lock);
-}
-EXPORT_SYMBOL_GPL(xv_free);
-
-u32 xv_get_object_size(void *obj)
-{
-	struct block_header *blk;
-
-	blk = (struct block_header *)((char *)(obj) - XV_ALIGN);
-	return blk->size;
-}
-EXPORT_SYMBOL_GPL(xv_get_object_size);
-
-/*
- * Returns total memory used by allocator (userdata + metadata)
- */
-u64 xv_get_total_size_bytes(struct xv_pool *pool)
-{
-	return pool->total_pages << PAGE_SHIFT;
-}
-EXPORT_SYMBOL_GPL(xv_get_total_size_bytes);
diff --git a/drivers/staging/ramster/xvmalloc.h b/drivers/staging/ramster/xvmalloc.h
deleted file mode 100644
index 5b1a81aa5faf..000000000000
--- a/drivers/staging/ramster/xvmalloc.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * xvmalloc memory allocator
- *
- * Copyright (C) 2008, 2009, 2010  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the licence that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-#ifndef _XV_MALLOC_H_
-#define _XV_MALLOC_H_
-
-#include <linux/types.h>
-
-struct xv_pool;
-
-struct xv_pool *xv_create_pool(void);
-void xv_destroy_pool(struct xv_pool *pool);
-
-int xv_malloc(struct xv_pool *pool, u32 size, struct page **page,
-			u32 *offset, gfp_t flags);
-void xv_free(struct xv_pool *pool, struct page *page, u32 offset);
-
-u32 xv_get_object_size(void *obj);
-u64 xv_get_total_size_bytes(struct xv_pool *pool);
-
-#endif
diff --git a/drivers/staging/ramster/xvmalloc_int.h b/drivers/staging/ramster/xvmalloc_int.h
deleted file mode 100644
index b5f1f7febcf6..000000000000
--- a/drivers/staging/ramster/xvmalloc_int.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * xvmalloc memory allocator
- *
- * Copyright (C) 2008, 2009, 2010  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the licence that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-#ifndef _XV_MALLOC_INT_H_
-#define _XV_MALLOC_INT_H_
-
-#include <linux/kernel.h>
-#include <linux/types.h>
-
-/* User configurable params */
-
-/* Must be power of two */
-#ifdef CONFIG_64BIT
-#define XV_ALIGN_SHIFT 3
-#else
-#define XV_ALIGN_SHIFT	2
-#endif
-#define XV_ALIGN	(1 << XV_ALIGN_SHIFT)
-#define XV_ALIGN_MASK	(XV_ALIGN - 1)
-
-/* This must be greater than sizeof(link_free) */
-#define XV_MIN_ALLOC_SIZE	32
-#define XV_MAX_ALLOC_SIZE	(PAGE_SIZE - XV_ALIGN)
-
-/*
- * Free lists are separated by FL_DELTA bytes
- * This value is 3 for 4k pages and 4 for 64k pages, for any
- * other page size, a conservative (PAGE_SHIFT - 9) is used.
- */
-#if PAGE_SHIFT == 16
-#define FL_DELTA_SHIFT 4
-#else
-#define FL_DELTA_SHIFT (PAGE_SHIFT - 9)
-#endif
-#define FL_DELTA	(1 << FL_DELTA_SHIFT)
-#define FL_DELTA_MASK	(FL_DELTA - 1)
-#define NUM_FREE_LISTS	((XV_MAX_ALLOC_SIZE - XV_MIN_ALLOC_SIZE) \
-				/ FL_DELTA + 1)
-
-#define MAX_FLI		DIV_ROUND_UP(NUM_FREE_LISTS, BITS_PER_LONG)
-
-/* End of user params */
-
-enum blockflags {
-	BLOCK_FREE,
-	PREV_FREE,
-	__NR_BLOCKFLAGS,
-};
-
-#define FLAGS_MASK	XV_ALIGN_MASK
-#define PREV_MASK	(~FLAGS_MASK)
-
-struct freelist_entry {
-	struct page *page;
-	u16 offset;
-	u16 pad;
-};
-
-struct link_free {
-	struct page *prev_page;
-	struct page *next_page;
-	u16 prev_offset;
-	u16 next_offset;
-};
-
-struct block_header {
-	union {
-		/* This common header must be XV_ALIGN bytes */
-		u8 common[XV_ALIGN];
-		struct {
-			u16 size;
-			u16 prev;
-		};
-	};
-	struct link_free link;
-};
-
-struct xv_pool {
-	ulong flbitmap;
-	ulong slbitmap[MAX_FLI];
-	u64 total_pages;	/* stats */
-	struct freelist_entry freelist[NUM_FREE_LISTS];
-	spinlock_t lock;
-};
-
-#endif
diff --git a/drivers/staging/ramster/zbud.c b/drivers/staging/ramster/zbud.c
new file mode 100644
index 000000000000..a7c436127aa1
--- /dev/null
+++ b/drivers/staging/ramster/zbud.c
@@ -0,0 +1,1060 @@
+/*
+ * zbud.c - Compression buddies allocator
+ *
+ * Copyright (c) 2010-2012, Dan Magenheimer, Oracle Corp.
+ *
+ * Compression buddies ("zbud") provides for efficiently packing two
+ * (or, possibly in the future, more) compressed pages ("zpages") into
+ * a single "raw" pageframe and for tracking both zpages and pageframes
+ * so that whole pageframes can be easily reclaimed in LRU-like order.
+ * It is designed to be used in conjunction with transcendent memory
+ * ("tmem"); for example separate LRU lists are maintained for persistent
+ * vs. ephemeral pages.
+ *
+ * A zbudpage is an overlay for a struct page and thus each zbudpage
+ * refers to a physical pageframe of RAM.  When the caller passes a
+ * struct page from the kernel's page allocator, zbud "transforms" it
+ * to a zbudpage which sets/uses a different set of fields than the
+ * struct-page and thus must "untransform" it back by reinitializing
+ * certain fields before the struct-page can be freed.  The fields
+ * of a zbudpage include a page lock for controlling access to the
+ * corresponding pageframe, and there is a size field for each zpage.
+ * Each zbudpage also lives on two linked lists: a "budlist" which is
+ * used to support efficient buddying of zpages; and an "lru" which
+ * is used for reclaiming pageframes in approximately least-recently-used
+ * order.
+ *
+ * A zbudpageframe is a pageframe divided up into aligned 64-byte "chunks"
+ * which contain the compressed data for zero, one, or two zbuds.  Contained
+ * with the compressed data is a tmem_handle which is a key to allow
+ * the same data to be found via the tmem interface so the zpage can
+ * be invalidated (for ephemeral pages) or repatriated to the swap cache
+ * (for persistent pages).  The contents of a zbudpageframe must never
+ * be accessed without holding the page lock for the corresponding
+ * zbudpage and, to accomodate highmem machines, the contents may
+ * only be examined or changes when kmapped.  Thus, when in use, a
+ * kmapped zbudpageframe is referred to in the zbud code as "void *zbpg".
+ *
+ * Note that the term "zbud" refers to the combination of a zpage and
+ * a tmem_handle that is stored as one of possibly two "buddied" zpages;
+ * it also generically refers to this allocator... sorry for any confusion.
+ *
+ * A zbudref is a pointer to a struct zbudpage (which can be cast to a
+ * struct page), with the LSB either cleared or set to indicate, respectively,
+ * the first or second zpage in the zbudpageframe. Since a zbudref can be
+ * cast to a pointer, it is used as the tmem "pampd" pointer and uniquely
+ * references a stored tmem page and so is the only zbud data structure
+ * externally visible to zbud.c/zbud.h.
+ *
+ * Since we wish to reclaim entire pageframes but zpages may be randomly
+ * added and deleted to any given pageframe, we approximate LRU by
+ * promoting a pageframe to MRU when a zpage is added to it, but
+ * leaving it at the current place in the list when a zpage is deleted
+ * from it.  As a side effect, zpages that are difficult to buddy (e.g.
+ * very large paages) will be reclaimed faster than average, which seems
+ * reasonable.
+ *
+ * In the current implementation, no more than two zpages may be stored in
+ * any pageframe and no zpage ever crosses a pageframe boundary.  While
+ * other zpage allocation mechanisms may allow greater density, this two
+ * zpage-per-pageframe limit both ensures simple reclaim of pageframes
+ * (including garbage collection of references to the contents of those
+ * pageframes from tmem data structures) AND avoids the need for compaction.
+ * With additional complexity, zbud could be modified to support storing
+ * up to three zpages per pageframe or, to handle larger average zpages,
+ * up to three zpages per pair of pageframes, but it is not clear if the
+ * additional complexity would be worth it.  So consider it an exercise
+ * for future developers.
+ *
+ * Note also that zbud does no page allocation or freeing.  This is so
+ * that the caller has complete control over and, for accounting, visibility
+ * into if/when pages are allocated and freed.
+ *
+ * Finally, note that zbud limits the size of zpages it can store; the
+ * caller must check the zpage size with zbud_max_buddy_size before
+ * storing it, else BUGs will result.  User beware.
+ */
+
+#include <linux/module.h>
+#include <linux/highmem.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/pagemap.h>
+#include <linux/atomic.h>
+#include <linux/bug.h>
+#include "tmem.h"
+#include "zcache.h"
+#include "zbud.h"
+
+/*
+ * We need to ensure that a struct zbudpage is never larger than a
+ * struct page.  This is checked with a BUG_ON in zbud_init.
+ *
+ * The unevictable field indicates that a zbud is being added to the
+ * zbudpage.  Since this is a two-phase process (due to tmem locking),
+ * this field locks the zbudpage against eviction when a zbud match
+ * or creation is in process.  Since this addition process may occur
+ * in parallel for two zbuds in one zbudpage, the field is a counter
+ * that must not exceed two.
+ */
+struct zbudpage {
+	union {
+		struct page page;
+		struct {
+			unsigned long space_for_flags;
+			struct {
+				unsigned zbud0_size:12;
+				unsigned zbud1_size:12;
+				unsigned unevictable:2;
+			};
+			struct list_head budlist;
+			struct list_head lru;
+		};
+	};
+};
+
+struct zbudref {
+	union {
+		struct zbudpage *zbudpage;
+		unsigned long zbudref;
+	};
+};
+
+#define CHUNK_SHIFT	6
+#define CHUNK_SIZE	(1 << CHUNK_SHIFT)
+#define CHUNK_MASK	(~(CHUNK_SIZE-1))
+#define NCHUNKS		(PAGE_SIZE >> CHUNK_SHIFT)
+#define MAX_CHUNK	(NCHUNKS-1)
+
+/*
+ * The following functions deal with the difference between struct
+ * page and struct zbudpage.  Note the hack of using the pageflags
+ * from struct page; this is to avoid duplicating all the complex
+ * pageflag macros.
+ */
+static inline void zbudpage_spin_lock(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	while (unlikely(test_and_set_bit_lock(PG_locked, &page->flags))) {
+		do {
+			cpu_relax();
+		} while (test_bit(PG_locked, &page->flags));
+	}
+}
+
+static inline void zbudpage_spin_unlock(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	clear_bit(PG_locked, &page->flags);
+}
+
+static inline int zbudpage_spin_trylock(struct zbudpage *zbudpage)
+{
+	return trylock_page((struct page *)zbudpage);
+}
+
+static inline int zbudpage_is_locked(struct zbudpage *zbudpage)
+{
+	return PageLocked((struct page *)zbudpage);
+}
+
+static inline void *kmap_zbudpage_atomic(struct zbudpage *zbudpage)
+{
+	return kmap_atomic((struct page *)zbudpage);
+}
+
+/*
+ * A dying zbudpage is an ephemeral page in the process of being evicted.
+ * Any data contained in the zbudpage is invalid and we are just waiting for
+ * the tmem pampds to be invalidated before freeing the page
+ */
+static inline int zbudpage_is_dying(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	return test_bit(PG_reclaim, &page->flags);
+}
+
+static inline void zbudpage_set_dying(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	set_bit(PG_reclaim, &page->flags);
+}
+
+static inline void zbudpage_clear_dying(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	clear_bit(PG_reclaim, &page->flags);
+}
+
+/*
+ * A zombie zbudpage is a persistent page in the process of being evicted.
+ * The data contained in the zbudpage is valid and we are just waiting for
+ * the tmem pampds to be invalidated before freeing the page
+ */
+static inline int zbudpage_is_zombie(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	return test_bit(PG_dirty, &page->flags);
+}
+
+static inline void zbudpage_set_zombie(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	set_bit(PG_dirty, &page->flags);
+}
+
+static inline void zbudpage_clear_zombie(struct zbudpage *zbudpage)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	clear_bit(PG_dirty, &page->flags);
+}
+
+static inline void kunmap_zbudpage_atomic(void *zbpg)
+{
+	kunmap_atomic(zbpg);
+}
+
+/*
+ * zbud "translation" and helper functions
+ */
+
+static inline struct zbudpage *zbudref_to_zbudpage(struct zbudref *zref)
+{
+	unsigned long zbud = (unsigned long)zref;
+	zbud &= ~1UL;
+	return (struct zbudpage *)zbud;
+}
+
+static inline struct zbudref *zbudpage_to_zbudref(struct zbudpage *zbudpage,
+							unsigned budnum)
+{
+	unsigned long zbud = (unsigned long)zbudpage;
+	BUG_ON(budnum > 1);
+	zbud |= budnum;
+	return (struct zbudref *)zbud;
+}
+
+static inline int zbudref_budnum(struct zbudref *zbudref)
+{
+	unsigned long zbud = (unsigned long)zbudref;
+	return zbud & 1UL;
+}
+
+static inline unsigned zbud_max_size(void)
+{
+	return MAX_CHUNK << CHUNK_SHIFT;
+}
+
+static inline unsigned zbud_size_to_chunks(unsigned size)
+{
+	BUG_ON(size == 0 || size > zbud_max_size());
+	return (size + CHUNK_SIZE - 1) >> CHUNK_SHIFT;
+}
+
+/* can only be used between kmap_zbudpage_atomic/kunmap_zbudpage_atomic! */
+static inline char *zbud_data(void *zbpg,
+			unsigned budnum, unsigned size)
+{
+	char *p;
+
+	BUG_ON(size == 0 || size > zbud_max_size());
+	p = (char *)zbpg;
+	if (budnum == 1)
+		p += PAGE_SIZE - ((size + CHUNK_SIZE - 1) & CHUNK_MASK);
+	return p;
+}
+
+/*
+ * These are all informative and exposed through debugfs... except for
+ * the arrays... anyone know how to do that?  To avoid confusion for
+ * debugfs viewers, some of these should also be atomic_long_t, but
+ * I don't know how to expose atomics via debugfs either...
+ */
+static unsigned long zbud_eph_pageframes;
+static unsigned long zbud_pers_pageframes;
+static unsigned long zbud_eph_zpages;
+static unsigned long zbud_pers_zpages;
+static u64 zbud_eph_zbytes;
+static u64 zbud_pers_zbytes;
+static unsigned long zbud_eph_evicted_pageframes;
+static unsigned long zbud_pers_evicted_pageframes;
+static unsigned long zbud_eph_cumul_zpages;
+static unsigned long zbud_pers_cumul_zpages;
+static u64 zbud_eph_cumul_zbytes;
+static u64 zbud_pers_cumul_zbytes;
+static unsigned long zbud_eph_cumul_chunk_counts[NCHUNKS];
+static unsigned long zbud_pers_cumul_chunk_counts[NCHUNKS];
+static unsigned long zbud_eph_buddied_count;
+static unsigned long zbud_pers_buddied_count;
+static unsigned long zbud_eph_unbuddied_count;
+static unsigned long zbud_pers_unbuddied_count;
+static unsigned long zbud_eph_zombie_count;
+static unsigned long zbud_pers_zombie_count;
+static atomic_t zbud_eph_zombie_atomic;
+static atomic_t zbud_pers_zombie_atomic;
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#define	zdfs	debugfs_create_size_t
+#define	zdfs64	debugfs_create_u64
+static int zbud_debugfs_init(void)
+{
+	struct dentry *root = debugfs_create_dir("zbud", NULL);
+	if (root == NULL)
+		return -ENXIO;
+
+	/*
+	 * would be nice to dump the sizes of the unbuddied
+	 * arrays, like was done with sysfs, but it doesn't
+	 * look like debugfs is flexible enough to do that
+	 */
+	zdfs64("eph_zbytes", S_IRUGO, root, &zbud_eph_zbytes);
+	zdfs64("eph_cumul_zbytes", S_IRUGO, root, &zbud_eph_cumul_zbytes);
+	zdfs64("pers_zbytes", S_IRUGO, root, &zbud_pers_zbytes);
+	zdfs64("pers_cumul_zbytes", S_IRUGO, root, &zbud_pers_cumul_zbytes);
+	zdfs("eph_cumul_zpages", S_IRUGO, root, &zbud_eph_cumul_zpages);
+	zdfs("eph_evicted_pageframes", S_IRUGO, root,
+				&zbud_eph_evicted_pageframes);
+	zdfs("eph_zpages", S_IRUGO, root, &zbud_eph_zpages);
+	zdfs("eph_pageframes", S_IRUGO, root, &zbud_eph_pageframes);
+	zdfs("eph_buddied_count", S_IRUGO, root, &zbud_eph_buddied_count);
+	zdfs("eph_unbuddied_count", S_IRUGO, root, &zbud_eph_unbuddied_count);
+	zdfs("pers_cumul_zpages", S_IRUGO, root, &zbud_pers_cumul_zpages);
+	zdfs("pers_evicted_pageframes", S_IRUGO, root,
+				&zbud_pers_evicted_pageframes);
+	zdfs("pers_zpages", S_IRUGO, root, &zbud_pers_zpages);
+	zdfs("pers_pageframes", S_IRUGO, root, &zbud_pers_pageframes);
+	zdfs("pers_buddied_count", S_IRUGO, root, &zbud_pers_buddied_count);
+	zdfs("pers_unbuddied_count", S_IRUGO, root, &zbud_pers_unbuddied_count);
+	zdfs("pers_zombie_count", S_IRUGO, root, &zbud_pers_zombie_count);
+	return 0;
+}
+#undef	zdfs
+#undef	zdfs64
+#endif
+
+/* protects the buddied list and all unbuddied lists */
+static DEFINE_SPINLOCK(zbud_eph_lists_lock);
+static DEFINE_SPINLOCK(zbud_pers_lists_lock);
+
+struct zbud_unbuddied {
+	struct list_head list;
+	unsigned count;
+};
+
+/* list N contains pages with N chunks USED and NCHUNKS-N unused */
+/* element 0 is never used but optimizing that isn't worth it */
+static struct zbud_unbuddied zbud_eph_unbuddied[NCHUNKS];
+static struct zbud_unbuddied zbud_pers_unbuddied[NCHUNKS];
+static LIST_HEAD(zbud_eph_lru_list);
+static LIST_HEAD(zbud_pers_lru_list);
+static LIST_HEAD(zbud_eph_buddied_list);
+static LIST_HEAD(zbud_pers_buddied_list);
+static LIST_HEAD(zbud_eph_zombie_list);
+static LIST_HEAD(zbud_pers_zombie_list);
+
+/*
+ * Given a struct page, transform it to a zbudpage so that it can be
+ * used by zbud and initialize fields as necessary.
+ */
+static inline struct zbudpage *zbud_init_zbudpage(struct page *page, bool eph)
+{
+	struct zbudpage *zbudpage = (struct zbudpage *)page;
+
+	BUG_ON(page == NULL);
+	INIT_LIST_HEAD(&zbudpage->budlist);
+	INIT_LIST_HEAD(&zbudpage->lru);
+	zbudpage->zbud0_size = 0;
+	zbudpage->zbud1_size = 0;
+	zbudpage->unevictable = 0;
+	if (eph)
+		zbud_eph_pageframes++;
+	else
+		zbud_pers_pageframes++;
+	return zbudpage;
+}
+
+/* "Transform" a zbudpage back to a struct page suitable to free. */
+static inline struct page *zbud_unuse_zbudpage(struct zbudpage *zbudpage,
+								bool eph)
+{
+	struct page *page = (struct page *)zbudpage;
+
+	BUG_ON(!list_empty(&zbudpage->budlist));
+	BUG_ON(!list_empty(&zbudpage->lru));
+	BUG_ON(zbudpage->zbud0_size != 0);
+	BUG_ON(zbudpage->zbud1_size != 0);
+	BUG_ON(!PageLocked(page));
+	BUG_ON(zbudpage->unevictable != 0);
+	BUG_ON(zbudpage_is_dying(zbudpage));
+	BUG_ON(zbudpage_is_zombie(zbudpage));
+	if (eph)
+		zbud_eph_pageframes--;
+	else
+		zbud_pers_pageframes--;
+	zbudpage_spin_unlock(zbudpage);
+	reset_page_mapcount(page);
+	init_page_count(page);
+	page->index = 0;
+	return page;
+}
+
+/* Mark a zbud as unused and do accounting */
+static inline void zbud_unuse_zbud(struct zbudpage *zbudpage,
+					int budnum, bool eph)
+{
+	unsigned size;
+
+	BUG_ON(!zbudpage_is_locked(zbudpage));
+	if (budnum == 0) {
+		size = zbudpage->zbud0_size;
+		zbudpage->zbud0_size = 0;
+	} else {
+		size = zbudpage->zbud1_size;
+		zbudpage->zbud1_size = 0;
+	}
+	if (eph) {
+		zbud_eph_zbytes -= size;
+		zbud_eph_zpages--;
+	} else {
+		zbud_pers_zbytes -= size;
+		zbud_pers_zpages--;
+	}
+}
+
+/*
+ * Given a zbudpage/budnum/size, a tmem handle, and a kmapped pointer
+ * to some data, set up the zbud appropriately including data copying
+ * and accounting.  Note that if cdata is NULL, the data copying is
+ * skipped.  (This is useful for lazy writes such as for RAMster.)
+ */
+static void zbud_init_zbud(struct zbudpage *zbudpage, struct tmem_handle *th,
+				bool eph, void *cdata,
+				unsigned budnum, unsigned size)
+{
+	char *to;
+	void *zbpg;
+	struct tmem_handle *to_th;
+	unsigned nchunks = zbud_size_to_chunks(size);
+
+	BUG_ON(!zbudpage_is_locked(zbudpage));
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	to = zbud_data(zbpg, budnum, size);
+	to_th = (struct tmem_handle *)to;
+	to_th->index = th->index;
+	to_th->oid = th->oid;
+	to_th->pool_id = th->pool_id;
+	to_th->client_id = th->client_id;
+	to += sizeof(struct tmem_handle);
+	if (cdata != NULL)
+		memcpy(to, cdata, size - sizeof(struct tmem_handle));
+	kunmap_zbudpage_atomic(zbpg);
+	if (budnum == 0)
+		zbudpage->zbud0_size = size;
+	else
+		zbudpage->zbud1_size = size;
+	if (eph) {
+		zbud_eph_cumul_chunk_counts[nchunks]++;
+		zbud_eph_zpages++;
+		zbud_eph_cumul_zpages++;
+		zbud_eph_zbytes += size;
+		zbud_eph_cumul_zbytes += size;
+	} else {
+		zbud_pers_cumul_chunk_counts[nchunks]++;
+		zbud_pers_zpages++;
+		zbud_pers_cumul_zpages++;
+		zbud_pers_zbytes += size;
+		zbud_pers_cumul_zbytes += size;
+	}
+}
+
+/*
+ * Given a locked dying zbudpage, read out the tmem handles from the data,
+ * unlock the page, then use the handles to tell tmem to flush out its
+ * references
+ */
+static void zbud_evict_tmem(struct zbudpage *zbudpage)
+{
+	int i, j;
+	uint32_t pool_id[2], client_id[2];
+	uint32_t index[2];
+	struct tmem_oid oid[2];
+	struct tmem_pool *pool;
+	void *zbpg;
+	struct tmem_handle *th;
+	unsigned size;
+
+	/* read out the tmem handles from the data and set aside */
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	for (i = 0, j = 0; i < 2; i++) {
+		size = (i == 0) ? zbudpage->zbud0_size : zbudpage->zbud1_size;
+		if (size) {
+			th = (struct tmem_handle *)zbud_data(zbpg, i, size);
+			client_id[j] = th->client_id;
+			pool_id[j] = th->pool_id;
+			oid[j] = th->oid;
+			index[j] = th->index;
+			j++;
+			zbud_unuse_zbud(zbudpage, i, true);
+		}
+	}
+	kunmap_zbudpage_atomic(zbpg);
+	zbudpage_spin_unlock(zbudpage);
+	/* zbudpage is now an unlocked dying... tell tmem to flush pointers */
+	for (i = 0; i < j; i++) {
+		pool = zcache_get_pool_by_id(client_id[i], pool_id[i]);
+		if (pool != NULL) {
+			tmem_flush_page(pool, &oid[i], index[i]);
+			zcache_put_pool(pool);
+		}
+	}
+}
+
+/*
+ * Externally callable zbud handling routines.
+ */
+
+/*
+ * Return the maximum size compressed page that can be stored (secretly
+ * setting aside space for the tmem handle.
+ */
+unsigned int zbud_max_buddy_size(void)
+{
+	return zbud_max_size() - sizeof(struct tmem_handle);
+}
+
+/*
+ * Given a zbud reference, free the corresponding zbud from all lists,
+ * mark it as unused, do accounting, and if the freeing of the zbud
+ * frees up an entire pageframe, return it to the caller (else NULL).
+ */
+struct page *zbud_free_and_delist(struct zbudref *zref, bool eph,
+				  unsigned int *zsize, unsigned int *zpages)
+{
+	unsigned long budnum = zbudref_budnum(zref);
+	struct zbudpage *zbudpage = zbudref_to_zbudpage(zref);
+	struct page *page = NULL;
+	unsigned chunks, bud_size, other_bud_size;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+	struct zbud_unbuddied *unbud =
+		eph ? zbud_eph_unbuddied : zbud_pers_unbuddied;
+
+
+	spin_lock(lists_lock);
+	zbudpage_spin_lock(zbudpage);
+	if (zbudpage_is_dying(zbudpage)) {
+		/* ignore dying zbudpage... see zbud_evict_pageframe_lru() */
+		zbudpage_spin_unlock(zbudpage);
+		spin_unlock(lists_lock);
+		*zpages = 0;
+		*zsize = 0;
+		goto out;
+	}
+	if (budnum == 0) {
+		bud_size = zbudpage->zbud0_size;
+		other_bud_size = zbudpage->zbud1_size;
+	} else {
+		bud_size = zbudpage->zbud1_size;
+		other_bud_size = zbudpage->zbud0_size;
+	}
+	*zsize = bud_size - sizeof(struct tmem_handle);
+	*zpages = 1;
+	zbud_unuse_zbud(zbudpage, budnum, eph);
+	if (other_bud_size == 0) { /* was unbuddied: unlist and free */
+		chunks = zbud_size_to_chunks(bud_size) ;
+		if (zbudpage_is_zombie(zbudpage)) {
+			if (eph)
+				zbud_pers_zombie_count =
+				  atomic_dec_return(&zbud_eph_zombie_atomic);
+			else
+				zbud_pers_zombie_count =
+				  atomic_dec_return(&zbud_pers_zombie_atomic);
+			zbudpage_clear_zombie(zbudpage);
+		} else {
+			BUG_ON(list_empty(&unbud[chunks].list));
+			list_del_init(&zbudpage->budlist);
+			unbud[chunks].count--;
+		}
+		list_del_init(&zbudpage->lru);
+		spin_unlock(lists_lock);
+		if (eph)
+			zbud_eph_unbuddied_count--;
+		else
+			zbud_pers_unbuddied_count--;
+		page = zbud_unuse_zbudpage(zbudpage, eph);
+	} else { /* was buddied: move remaining buddy to unbuddied list */
+		chunks = zbud_size_to_chunks(other_bud_size) ;
+		if (!zbudpage_is_zombie(zbudpage)) {
+			list_del_init(&zbudpage->budlist);
+			list_add_tail(&zbudpage->budlist, &unbud[chunks].list);
+			unbud[chunks].count++;
+		}
+		if (eph) {
+			zbud_eph_buddied_count--;
+			zbud_eph_unbuddied_count++;
+		} else {
+			zbud_pers_unbuddied_count++;
+			zbud_pers_buddied_count--;
+		}
+		/* don't mess with lru, no need to move it */
+		zbudpage_spin_unlock(zbudpage);
+		spin_unlock(lists_lock);
+	}
+out:
+	return page;
+}
+
+/*
+ * Given a tmem handle, and a kmapped pointer to compressed data of
+ * the given size, try to find an unbuddied zbudpage in which to
+ * create a zbud. If found, put it there, mark the zbudpage unevictable,
+ * and return a zbudref to it.  Else return NULL.
+ */
+struct zbudref *zbud_match_prep(struct tmem_handle *th, bool eph,
+				void *cdata, unsigned size)
+{
+	struct zbudpage *zbudpage = NULL, *zbudpage2;
+	unsigned long budnum = 0UL;
+	unsigned nchunks;
+	int i, found_good_buddy = 0;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+	struct zbud_unbuddied *unbud =
+		eph ? zbud_eph_unbuddied : zbud_pers_unbuddied;
+
+	size += sizeof(struct tmem_handle);
+	nchunks = zbud_size_to_chunks(size);
+	for (i = MAX_CHUNK - nchunks + 1; i > 0; i--) {
+		spin_lock(lists_lock);
+		if (!list_empty(&unbud[i].list)) {
+			list_for_each_entry_safe(zbudpage, zbudpage2,
+				    &unbud[i].list, budlist) {
+				if (zbudpage_spin_trylock(zbudpage)) {
+					found_good_buddy = i;
+					goto found_unbuddied;
+				}
+			}
+		}
+		spin_unlock(lists_lock);
+	}
+	zbudpage = NULL;
+	goto out;
+
+found_unbuddied:
+	BUG_ON(!zbudpage_is_locked(zbudpage));
+	BUG_ON(!((zbudpage->zbud0_size == 0) ^ (zbudpage->zbud1_size == 0)));
+	if (zbudpage->zbud0_size == 0)
+		budnum = 0UL;
+	else if (zbudpage->zbud1_size == 0)
+		budnum = 1UL;
+	list_del_init(&zbudpage->budlist);
+	if (eph) {
+		list_add_tail(&zbudpage->budlist, &zbud_eph_buddied_list);
+		unbud[found_good_buddy].count--;
+		zbud_eph_unbuddied_count--;
+		zbud_eph_buddied_count++;
+		/* "promote" raw zbudpage to most-recently-used */
+		list_del_init(&zbudpage->lru);
+		list_add_tail(&zbudpage->lru, &zbud_eph_lru_list);
+	} else {
+		list_add_tail(&zbudpage->budlist, &zbud_pers_buddied_list);
+		unbud[found_good_buddy].count--;
+		zbud_pers_unbuddied_count--;
+		zbud_pers_buddied_count++;
+		/* "promote" raw zbudpage to most-recently-used */
+		list_del_init(&zbudpage->lru);
+		list_add_tail(&zbudpage->lru, &zbud_pers_lru_list);
+	}
+	zbud_init_zbud(zbudpage, th, eph, cdata, budnum, size);
+	zbudpage->unevictable++;
+	BUG_ON(zbudpage->unevictable == 3);
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+out:
+	return zbudpage_to_zbudref(zbudpage, budnum);
+
+}
+
+/*
+ * Given a tmem handle, and a kmapped pointer to compressed data of
+ * the given size, and a newly allocated struct page, create an unevictable
+ * zbud in that new page and return a zbudref to it.
+ */
+struct zbudref *zbud_create_prep(struct tmem_handle *th, bool eph,
+					void *cdata, unsigned size,
+					struct page *newpage)
+{
+	struct zbudpage *zbudpage;
+	unsigned long budnum = 0;
+	unsigned nchunks;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+	struct zbud_unbuddied *unbud =
+		eph ? zbud_eph_unbuddied : zbud_pers_unbuddied;
+
+#if 0
+	/* this may be worth it later to support decompress-in-place? */
+	static unsigned long counter;
+	budnum = counter++ & 1;	/* alternate using zbud0 and zbud1 */
+#endif
+
+	if (size  > zbud_max_buddy_size())
+		return NULL;
+	if (newpage == NULL)
+		return NULL;
+
+	size += sizeof(struct tmem_handle);
+	nchunks = zbud_size_to_chunks(size) ;
+	spin_lock(lists_lock);
+	zbudpage = zbud_init_zbudpage(newpage, eph);
+	zbudpage_spin_lock(zbudpage);
+	list_add_tail(&zbudpage->budlist, &unbud[nchunks].list);
+	if (eph) {
+		list_add_tail(&zbudpage->lru, &zbud_eph_lru_list);
+		zbud_eph_unbuddied_count++;
+	} else {
+		list_add_tail(&zbudpage->lru, &zbud_pers_lru_list);
+		zbud_pers_unbuddied_count++;
+	}
+	unbud[nchunks].count++;
+	zbud_init_zbud(zbudpage, th, eph, cdata, budnum, size);
+	zbudpage->unevictable++;
+	BUG_ON(zbudpage->unevictable == 3);
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+	return zbudpage_to_zbudref(zbudpage, budnum);
+}
+
+/*
+ * Finish creation of a zbud by, assuming another zbud isn't being created
+ * in parallel, marking it evictable.
+ */
+void zbud_create_finish(struct zbudref *zref, bool eph)
+{
+	struct zbudpage *zbudpage = zbudref_to_zbudpage(zref);
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+
+	spin_lock(lists_lock);
+	zbudpage_spin_lock(zbudpage);
+	BUG_ON(zbudpage_is_dying(zbudpage));
+	zbudpage->unevictable--;
+	BUG_ON((int)zbudpage->unevictable < 0);
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+}
+
+/*
+ * Given a zbudref and a struct page, decompress the data from
+ * the zbud into the physical page represented by the struct page
+ * by upcalling to zcache_decompress
+ */
+int zbud_decompress(struct page *data_page, struct zbudref *zref, bool eph,
+			void (*decompress)(char *, unsigned int, char *))
+{
+	struct zbudpage *zbudpage = zbudref_to_zbudpage(zref);
+	unsigned long budnum = zbudref_budnum(zref);
+	void *zbpg;
+	char *to_va, *from_va;
+	unsigned size;
+	int ret = -1;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+
+	spin_lock(lists_lock);
+	zbudpage_spin_lock(zbudpage);
+	if (zbudpage_is_dying(zbudpage)) {
+		/* ignore dying zbudpage... see zbud_evict_pageframe_lru() */
+		goto out;
+	}
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	to_va = kmap_atomic(data_page);
+	if (budnum == 0)
+		size = zbudpage->zbud0_size;
+	else
+		size = zbudpage->zbud1_size;
+	BUG_ON(size == 0 || size > zbud_max_size());
+	from_va = zbud_data(zbpg, budnum, size);
+	from_va += sizeof(struct tmem_handle);
+	size -= sizeof(struct tmem_handle);
+	decompress(from_va, size, to_va);
+	kunmap_atomic(to_va);
+	kunmap_zbudpage_atomic(zbpg);
+	ret = 0;
+out:
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+	return ret;
+}
+
+/*
+ * Given a zbudref and a kernel pointer, copy the data from
+ * the zbud to the kernel pointer.
+ */
+int zbud_copy_from_zbud(char *to_va, struct zbudref *zref,
+				size_t *sizep, bool eph)
+{
+	struct zbudpage *zbudpage = zbudref_to_zbudpage(zref);
+	unsigned long budnum = zbudref_budnum(zref);
+	void *zbpg;
+	char *from_va;
+	unsigned size;
+	int ret = -1;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+
+	spin_lock(lists_lock);
+	zbudpage_spin_lock(zbudpage);
+	if (zbudpage_is_dying(zbudpage)) {
+		/* ignore dying zbudpage... see zbud_evict_pageframe_lru() */
+		goto out;
+	}
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	if (budnum == 0)
+		size = zbudpage->zbud0_size;
+	else
+		size = zbudpage->zbud1_size;
+	BUG_ON(size == 0 || size > zbud_max_size());
+	from_va = zbud_data(zbpg, budnum, size);
+	from_va += sizeof(struct tmem_handle);
+	size -= sizeof(struct tmem_handle);
+	*sizep = size;
+	memcpy(to_va, from_va, size);
+
+	kunmap_zbudpage_atomic(zbpg);
+	ret = 0;
+out:
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+	return ret;
+}
+
+/*
+ * Given a zbudref and a kernel pointer, copy the data from
+ * the kernel pointer to the zbud.
+ */
+int zbud_copy_to_zbud(struct zbudref *zref, char *from_va, bool eph)
+{
+	struct zbudpage *zbudpage = zbudref_to_zbudpage(zref);
+	unsigned long budnum = zbudref_budnum(zref);
+	void *zbpg;
+	char *to_va;
+	unsigned size;
+	int ret = -1;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+
+	spin_lock(lists_lock);
+	zbudpage_spin_lock(zbudpage);
+	if (zbudpage_is_dying(zbudpage)) {
+		/* ignore dying zbudpage... see zbud_evict_pageframe_lru() */
+		goto out;
+	}
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	if (budnum == 0)
+		size = zbudpage->zbud0_size;
+	else
+		size = zbudpage->zbud1_size;
+	BUG_ON(size == 0 || size > zbud_max_size());
+	to_va = zbud_data(zbpg, budnum, size);
+	to_va += sizeof(struct tmem_handle);
+	size -= sizeof(struct tmem_handle);
+	memcpy(to_va, from_va, size);
+
+	kunmap_zbudpage_atomic(zbpg);
+	ret = 0;
+out:
+	zbudpage_spin_unlock(zbudpage);
+	spin_unlock(lists_lock);
+	return ret;
+}
+
+/*
+ * Choose an ephemeral LRU zbudpage that is evictable (not locked), ensure
+ * there are no references to it remaining, and return the now unused
+ * (and re-init'ed) struct page and the total amount of compressed
+ * data that was evicted.
+ */
+struct page *zbud_evict_pageframe_lru(unsigned int *zsize, unsigned int *zpages)
+{
+	struct zbudpage *zbudpage = NULL, *zbudpage2;
+	struct zbud_unbuddied *unbud = zbud_eph_unbuddied;
+	struct page *page = NULL;
+	bool irqs_disabled = irqs_disabled();
+
+	/*
+	 * Since this can be called indirectly from cleancache_put, which
+	 * has interrupts disabled, as well as frontswap_put, which does not,
+	 * we need to be able to handle both cases, even though it is ugly.
+	 */
+	if (irqs_disabled)
+		spin_lock(&zbud_eph_lists_lock);
+	else
+		spin_lock_bh(&zbud_eph_lists_lock);
+	*zsize = 0;
+	if (list_empty(&zbud_eph_lru_list))
+		goto unlock_out;
+	list_for_each_entry_safe(zbudpage, zbudpage2, &zbud_eph_lru_list, lru) {
+		/* skip a locked zbudpage */
+		if (unlikely(!zbudpage_spin_trylock(zbudpage)))
+			continue;
+		/* skip an unevictable zbudpage */
+		if (unlikely(zbudpage->unevictable != 0)) {
+			zbudpage_spin_unlock(zbudpage);
+			continue;
+		}
+		/* got a locked evictable page */
+		goto evict_page;
+
+	}
+unlock_out:
+	/* no unlocked evictable pages, give up */
+	if (irqs_disabled)
+		spin_unlock(&zbud_eph_lists_lock);
+	else
+		spin_unlock_bh(&zbud_eph_lists_lock);
+	goto out;
+
+evict_page:
+	list_del_init(&zbudpage->budlist);
+	list_del_init(&zbudpage->lru);
+	zbudpage_set_dying(zbudpage);
+	/*
+	 * the zbudpage is now "dying" and attempts to read, write,
+	 * or delete data from it will be ignored
+	 */
+	if (zbudpage->zbud0_size != 0 && zbudpage->zbud1_size !=  0) {
+		*zsize = zbudpage->zbud0_size + zbudpage->zbud1_size -
+				(2 * sizeof(struct tmem_handle));
+		*zpages = 2;
+	} else if (zbudpage->zbud0_size != 0) {
+		unbud[zbud_size_to_chunks(zbudpage->zbud0_size)].count--;
+		*zsize = zbudpage->zbud0_size - sizeof(struct tmem_handle);
+		*zpages = 1;
+	} else if (zbudpage->zbud1_size != 0) {
+		unbud[zbud_size_to_chunks(zbudpage->zbud1_size)].count--;
+		*zsize = zbudpage->zbud1_size - sizeof(struct tmem_handle);
+		*zpages = 1;
+	} else {
+		BUG();
+	}
+	spin_unlock(&zbud_eph_lists_lock);
+	zbud_eph_evicted_pageframes++;
+	if (*zpages == 1)
+		zbud_eph_unbuddied_count--;
+	else
+		zbud_eph_buddied_count--;
+	zbud_evict_tmem(zbudpage);
+	zbudpage_spin_lock(zbudpage);
+	zbudpage_clear_dying(zbudpage);
+	page = zbud_unuse_zbudpage(zbudpage, true);
+	if (!irqs_disabled)
+		local_bh_enable();
+out:
+	return page;
+}
+
+/*
+ * Choose a persistent LRU zbudpage that is evictable (not locked), zombify it,
+ * read the tmem_handle(s) out of it into the passed array, and return the
+ * number of zbuds.  Caller must perform necessary tmem functions and,
+ * indirectly, zbud functions to fetch any valid data and cause the
+ * now-zombified zbudpage to eventually be freed.  We track the zombified
+ * zbudpage count so it is possible to observe if there is a leak.
+ FIXME: describe (ramster) case where data pointers are passed in for memcpy
+ */
+unsigned int zbud_make_zombie_lru(struct tmem_handle *th, unsigned char **data,
+					unsigned int *zsize, bool eph)
+{
+	struct zbudpage *zbudpage = NULL, *zbudpag2;
+	struct tmem_handle *thfrom;
+	char *from_va;
+	void *zbpg;
+	unsigned size;
+	int ret = 0, i;
+	spinlock_t *lists_lock =
+		eph ? &zbud_eph_lists_lock : &zbud_pers_lists_lock;
+	struct list_head *lru_list =
+		eph ? &zbud_eph_lru_list : &zbud_pers_lru_list;
+
+	spin_lock_bh(lists_lock);
+	if (list_empty(lru_list))
+		goto out;
+	list_for_each_entry_safe(zbudpage, zbudpag2, lru_list, lru) {
+		/* skip a locked zbudpage */
+		if (unlikely(!zbudpage_spin_trylock(zbudpage)))
+			continue;
+		/* skip an unevictable zbudpage */
+		if (unlikely(zbudpage->unevictable != 0)) {
+			zbudpage_spin_unlock(zbudpage);
+			continue;
+		}
+		/* got a locked evictable page */
+		goto zombify_page;
+	}
+	/* no unlocked evictable pages, give up */
+	goto out;
+
+zombify_page:
+	/* got an unlocked evictable page, zombify it */
+	list_del_init(&zbudpage->budlist);
+	zbudpage_set_zombie(zbudpage);
+	/* FIXME what accounting do I need to do here? */
+	list_del_init(&zbudpage->lru);
+	if (eph) {
+		list_add_tail(&zbudpage->lru, &zbud_eph_zombie_list);
+		zbud_eph_zombie_count =
+				atomic_inc_return(&zbud_eph_zombie_atomic);
+	} else {
+		list_add_tail(&zbudpage->lru, &zbud_pers_zombie_list);
+		zbud_pers_zombie_count =
+				atomic_inc_return(&zbud_pers_zombie_atomic);
+	}
+	/* FIXME what accounting do I need to do here? */
+	zbpg = kmap_zbudpage_atomic(zbudpage);
+	for (i = 0; i < 2; i++) {
+		size = (i == 0) ? zbudpage->zbud0_size : zbudpage->zbud1_size;
+		if (size) {
+			from_va = zbud_data(zbpg, i, size);
+			thfrom = (struct tmem_handle *)from_va;
+			from_va += sizeof(struct tmem_handle);
+			size -= sizeof(struct tmem_handle);
+			if (th != NULL)
+				th[ret] = *thfrom;
+			if (data != NULL)
+				memcpy(data[ret], from_va, size);
+			if (zsize != NULL)
+				*zsize++ = size;
+			ret++;
+		}
+	}
+	kunmap_zbudpage_atomic(zbpg);
+	zbudpage_spin_unlock(zbudpage);
+out:
+	spin_unlock_bh(lists_lock);
+	return ret;
+}
+
+void __init zbud_init(void)
+{
+	int i;
+
+#ifdef CONFIG_DEBUG_FS
+	zbud_debugfs_init();
+#endif
+	BUG_ON((sizeof(struct tmem_handle) * 2 > CHUNK_SIZE));
+	BUG_ON(sizeof(struct zbudpage) > sizeof(struct page));
+	for (i = 0; i < NCHUNKS; i++) {
+		INIT_LIST_HEAD(&zbud_eph_unbuddied[i].list);
+		INIT_LIST_HEAD(&zbud_pers_unbuddied[i].list);
+	}
+}
diff --git a/drivers/staging/ramster/zbud.h b/drivers/staging/ramster/zbud.h
new file mode 100644
index 000000000000..891e8a7d5aa5
--- /dev/null
+++ b/drivers/staging/ramster/zbud.h
@@ -0,0 +1,33 @@
+/*
+ * zbud.h
+ *
+ * Copyright (c) 2010-2012, Dan Magenheimer, Oracle Corp.
+ *
+ */
+
+#ifndef _ZBUD_H_
+#define _ZBUD_H_
+
+#include "tmem.h"
+
+struct zbudref;
+
+extern unsigned int zbud_max_buddy_size(void);
+extern struct zbudref *zbud_match_prep(struct tmem_handle *th, bool eph,
+						void *cdata, unsigned size);
+extern struct zbudref *zbud_create_prep(struct tmem_handle *th, bool eph,
+						void *cdata, unsigned size,
+						struct page *newpage);
+extern void zbud_create_finish(struct zbudref *, bool);
+extern int zbud_decompress(struct page *, struct zbudref *, bool,
+				void (*func)(char *, unsigned int, char *));
+extern int zbud_copy_from_zbud(char *, struct zbudref *, size_t *, bool);
+extern int zbud_copy_to_zbud(struct zbudref *, char *, bool);
+extern struct page *zbud_free_and_delist(struct zbudref *, bool eph,
+						unsigned int *, unsigned int *);
+extern struct page *zbud_evict_pageframe_lru(unsigned int *, unsigned int *);
+extern unsigned int zbud_make_zombie_lru(struct tmem_handle *, unsigned char **,
+						unsigned int *, bool);
+extern void zbud_init(void);
+
+#endif /* _ZBUD_H_ */
diff --git a/drivers/staging/ramster/zcache-main.c b/drivers/staging/ramster/zcache-main.c
index d46764b5aaba..a09dd5cc1cea 100644
--- a/drivers/staging/ramster/zcache-main.c
+++ b/drivers/staging/ramster/zcache-main.c
@@ -5,1392 +5,322 @@
  * Copyright (c) 2010,2011, Nitin Gupta
  *
  * Zcache provides an in-kernel "host implementation" for transcendent memory
- * and, thus indirectly, for cleancache and frontswap.  Zcache includes two
- * page-accessible memory [1] interfaces, both utilizing lzo1x compression:
- * 1) "compression buddies" ("zbud") is used for ephemeral pages
- * 2) xvmalloc is used for persistent pages.
- * Xvmalloc (based on the TLSF allocator) has very low fragmentation
- * so maximizes space efficiency, while zbud allows pairs (and potentially,
- * in the future, more than a pair of) compressed pages to be closely linked
- * so that reclaiming can be done via the kernel's physical-page-oriented
- * "shrinker" interface.
- *
- * [1] For a definition of page-accessible memory (aka PAM), see:
- *   http://marc.info/?l=linux-mm&m=127811271605009
- *  RAMSTER TODO:
- *   - handle remotifying of buddied pages (see zbud_remotify_zbpg)
- *   - kernel boot params: nocleancache/nofrontswap don't always work?!?
+ * ("tmem") and, thus indirectly, for cleancache and frontswap.  Zcache uses
+ * lzo1x compression to improve density and an embedded allocator called
+ * "zbud" which "buddies" two compressed pages semi-optimally in each physical
+ * pageframe.  Zbud is integrally tied into tmem to allow pageframes to
+ * be "reclaimed" efficiently.
  */
 
 #include <linux/module.h>
 #include <linux/cpu.h>
 #include <linux/highmem.h>
 #include <linux/list.h>
-#include <linux/lzo.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/types.h>
 #include <linux/atomic.h>
 #include <linux/math64.h>
-#include "tmem.h"
-#include "zcache.h"
-#include "ramster.h"
-#include "cluster/tcp.h"
+#include <linux/crypto.h>
 
-#include "xvmalloc.h"	/* temporary until change to zsmalloc */
-
-#define	RAMSTER_TESTING
-
-#if (!defined(CONFIG_CLEANCACHE) && !defined(CONFIG_FRONTSWAP))
-#error "ramster is useless without CONFIG_CLEANCACHE or CONFIG_FRONTSWAP"
-#endif
-#ifdef CONFIG_CLEANCACHE
 #include <linux/cleancache.h>
-#endif
-#ifdef CONFIG_FRONTSWAP
 #include <linux/frontswap.h>
-#endif
-
-enum ramster_remotify_op {
-	RAMSTER_REMOTIFY_EPH_PUT,
-	RAMSTER_REMOTIFY_PERS_PUT,
-	RAMSTER_REMOTIFY_FLUSH_PAGE,
-	RAMSTER_REMOTIFY_FLUSH_OBJ,
-	RAMSTER_INTRANSIT_PERS
-};
-
-struct ramster_remotify_hdr {
-	enum ramster_remotify_op op;
-	struct list_head list;
-};
-
-#define ZBH_SENTINEL  0x43214321
-#define ZBPG_SENTINEL  0xdeadbeef
-
-#define ZBUD_MAX_BUDS 2
-
-struct zbud_hdr {
-	struct ramster_remotify_hdr rem_op;
-	uint16_t client_id;
-	uint16_t pool_id;
-	struct tmem_oid oid;
-	uint32_t index;
-	uint16_t size; /* compressed size in bytes, zero means unused */
-	DECL_SENTINEL
-};
-
-#define ZVH_SENTINEL  0x43214321
-static const int zv_max_page_size = (PAGE_SIZE / 8) * 7;
-
-struct zv_hdr {
-	struct ramster_remotify_hdr rem_op;
-	uint16_t client_id;
-	uint16_t pool_id;
-	struct tmem_oid oid;
-	uint32_t index;
-	DECL_SENTINEL
-};
-
-struct flushlist_node {
-	struct ramster_remotify_hdr rem_op;
-	struct tmem_xhandle xh;
-};
-
-union {
-	struct ramster_remotify_hdr rem_op;
-	struct zv_hdr zv;
-	struct zbud_hdr zbud;
-	struct flushlist_node flist;
-} remotify_list_node;
-
-static LIST_HEAD(zcache_rem_op_list);
-static DEFINE_SPINLOCK(zcache_rem_op_list_lock);
-
-#if 0
-/* this is more aggressive but may cause other problems? */
-#define ZCACHE_GFP_MASK	(GFP_ATOMIC | __GFP_NORETRY | __GFP_NOWARN)
+#include "tmem.h"
+#include "zcache.h"
+#include "zbud.h"
+#include "ramster.h"
+#ifdef CONFIG_RAMSTER
+static int ramster_enabled;
 #else
-#define ZCACHE_GFP_MASK \
-	(__GFP_FS | __GFP_NORETRY | __GFP_NOWARN | __GFP_NOMEMALLOC)
+#define ramster_enabled 0
 #endif
 
-#define MAX_POOLS_PER_CLIENT 16
-
-#define MAX_CLIENTS 16
-#define LOCAL_CLIENT ((uint16_t)-1)
-
-MODULE_LICENSE("GPL");
-
-struct zcache_client {
-	struct tmem_pool *tmem_pools[MAX_POOLS_PER_CLIENT];
-	struct xv_pool *xvpool;
-	bool allocated;
-	atomic_t refcount;
-};
-
-static struct zcache_client zcache_host;
-static struct zcache_client zcache_clients[MAX_CLIENTS];
-
-static inline uint16_t get_client_id_from_client(struct zcache_client *cli)
+#ifndef __PG_WAS_ACTIVE
+static inline bool PageWasActive(struct page *page)
 {
-	BUG_ON(cli == NULL);
-	if (cli == &zcache_host)
-		return LOCAL_CLIENT;
-	return cli - &zcache_clients[0];
+	return true;
 }
 
-static inline bool is_local_client(struct zcache_client *cli)
+static inline void SetPageWasActive(struct page *page)
 {
-	return cli == &zcache_host;
-}
-
-/**********
- * Compression buddies ("zbud") provides for packing two (or, possibly
- * in the future, more) compressed ephemeral pages into a single "raw"
- * (physical) page and tracking them with data structures so that
- * the raw pages can be easily reclaimed.
- *
- * A zbud page ("zbpg") is an aligned page containing a list_head,
- * a lock, and two "zbud headers".  The remainder of the physical
- * page is divided up into aligned 64-byte "chunks" which contain
- * the compressed data for zero, one, or two zbuds.  Each zbpg
- * resides on: (1) an "unused list" if it has no zbuds; (2) a
- * "buddied" list if it is fully populated  with two zbuds; or
- * (3) one of PAGE_SIZE/64 "unbuddied" lists indexed by how many chunks
- * the one unbuddied zbud uses.  The data inside a zbpg cannot be
- * read or written unless the zbpg's lock is held.
- */
-
-struct zbud_page {
-	struct list_head bud_list;
-	spinlock_t lock;
-	struct zbud_hdr buddy[ZBUD_MAX_BUDS];
-	DECL_SENTINEL
-	/* followed by NUM_CHUNK aligned CHUNK_SIZE-byte chunks */
-};
-
-#define CHUNK_SHIFT	6
-#define CHUNK_SIZE	(1 << CHUNK_SHIFT)
-#define CHUNK_MASK	(~(CHUNK_SIZE-1))
-#define NCHUNKS		(((PAGE_SIZE - sizeof(struct zbud_page)) & \
-				CHUNK_MASK) >> CHUNK_SHIFT)
-#define MAX_CHUNK	(NCHUNKS-1)
-
-static struct {
-	struct list_head list;
-	unsigned count;
-} zbud_unbuddied[NCHUNKS];
-/* list N contains pages with N chunks USED and NCHUNKS-N unused */
-/* element 0 is never used but optimizing that isn't worth it */
-static unsigned long zbud_cumul_chunk_counts[NCHUNKS];
-
-struct list_head zbud_buddied_list;
-static unsigned long zcache_zbud_buddied_count;
-
-/* protects the buddied list and all unbuddied lists */
-static DEFINE_SPINLOCK(zbud_budlists_spinlock);
-
-static atomic_t zcache_zbud_curr_raw_pages;
-static atomic_t zcache_zbud_curr_zpages;
-static unsigned long zcache_zbud_curr_zbytes;
-static unsigned long zcache_zbud_cumul_zpages;
-static unsigned long zcache_zbud_cumul_zbytes;
-static unsigned long zcache_compress_poor;
-static unsigned long zcache_policy_percent_exceeded;
-static unsigned long zcache_mean_compress_poor;
-
-/*
- * RAMster counters
- * - Remote pages are pages with a local pampd but the data is remote
- * - Foreign pages are pages stored locally but belonging to another node
- */
-static atomic_t ramster_remote_pers_pages = ATOMIC_INIT(0);
-static unsigned long ramster_pers_remotify_enable;
-static unsigned long ramster_eph_remotify_enable;
-static unsigned long ramster_eph_pages_remoted;
-static unsigned long ramster_eph_pages_remote_failed;
-static unsigned long ramster_pers_pages_remoted;
-static unsigned long ramster_pers_pages_remote_failed;
-static unsigned long ramster_pers_pages_remote_nomem;
-static unsigned long ramster_remote_objects_flushed;
-static unsigned long ramster_remote_object_flushes_failed;
-static unsigned long ramster_remote_pages_flushed;
-static unsigned long ramster_remote_page_flushes_failed;
-static unsigned long ramster_remote_eph_pages_succ_get;
-static unsigned long ramster_remote_pers_pages_succ_get;
-static unsigned long ramster_remote_eph_pages_unsucc_get;
-static unsigned long ramster_remote_pers_pages_unsucc_get;
-static atomic_t ramster_curr_flnode_count = ATOMIC_INIT(0);
-static unsigned long ramster_curr_flnode_count_max;
-static atomic_t ramster_foreign_eph_pampd_count = ATOMIC_INIT(0);
-static unsigned long ramster_foreign_eph_pampd_count_max;
-static atomic_t ramster_foreign_pers_pampd_count = ATOMIC_INIT(0);
-static unsigned long ramster_foreign_pers_pampd_count_max;
-
-/* forward references */
-static void *zcache_get_free_page(void);
-static void zcache_free_page(void *p);
-
-/*
- * zbud helper functions
- */
-
-static inline unsigned zbud_max_buddy_size(void)
-{
-	return MAX_CHUNK << CHUNK_SHIFT;
 }
+#endif
 
-static inline unsigned zbud_size_to_chunks(unsigned size)
+#ifdef FRONTSWAP_HAS_EXCLUSIVE_GETS
+static bool frontswap_has_exclusive_gets __read_mostly = true;
+#else
+static bool frontswap_has_exclusive_gets __read_mostly;
+static inline void frontswap_tmem_exclusive_gets(bool b)
 {
-	BUG_ON(size == 0 || size > zbud_max_buddy_size());
-	return (size + CHUNK_SIZE - 1) >> CHUNK_SHIFT;
-}
-
-static inline int zbud_budnum(struct zbud_hdr *zh)
-{
-	unsigned offset = (unsigned long)zh & (PAGE_SIZE - 1);
-	struct zbud_page *zbpg = NULL;
-	unsigned budnum = -1U;
-	int i;
-
-	for (i = 0; i < ZBUD_MAX_BUDS; i++)
-		if (offset == offsetof(typeof(*zbpg), buddy[i])) {
-			budnum = i;
-			break;
-		}
-	BUG_ON(budnum == -1U);
-	return budnum;
-}
-
-static char *zbud_data(struct zbud_hdr *zh, unsigned size)
-{
-	struct zbud_page *zbpg;
-	char *p;
-	unsigned budnum;
-
-	ASSERT_SENTINEL(zh, ZBH);
-	budnum = zbud_budnum(zh);
-	BUG_ON(size == 0 || size > zbud_max_buddy_size());
-	zbpg = container_of(zh, struct zbud_page, buddy[budnum]);
-	ASSERT_SPINLOCK(&zbpg->lock);
-	p = (char *)zbpg;
-	if (budnum == 0)
-		p += ((sizeof(struct zbud_page) + CHUNK_SIZE - 1) &
-							CHUNK_MASK);
-	else if (budnum == 1)
-		p += PAGE_SIZE - ((size + CHUNK_SIZE - 1) & CHUNK_MASK);
-	return p;
-}
-
-static void zbud_copy_from_pampd(char *data, size_t *size, struct zbud_hdr *zh)
-{
-	struct zbud_page *zbpg;
-	char *p;
-	unsigned budnum;
-
-	ASSERT_SENTINEL(zh, ZBH);
-	budnum = zbud_budnum(zh);
-	zbpg = container_of(zh, struct zbud_page, buddy[budnum]);
-	spin_lock(&zbpg->lock);
-	BUG_ON(zh->size > *size);
-	p = (char *)zbpg;
-	if (budnum == 0)
-		p += ((sizeof(struct zbud_page) + CHUNK_SIZE - 1) &
-							CHUNK_MASK);
-	else if (budnum == 1)
-		p += PAGE_SIZE - ((zh->size + CHUNK_SIZE - 1) & CHUNK_MASK);
-	/* client should be filled in by caller */
-	memcpy(data, p, zh->size);
-	*size = zh->size;
-	spin_unlock(&zbpg->lock);
-}
-
-/*
- * zbud raw page management
- */
-
-static struct zbud_page *zbud_alloc_raw_page(void)
-{
-	struct zbud_page *zbpg = NULL;
-	struct zbud_hdr *zh0, *zh1;
-		zbpg = zcache_get_free_page();
-	if (likely(zbpg != NULL)) {
-		INIT_LIST_HEAD(&zbpg->bud_list);
-		zh0 = &zbpg->buddy[0]; zh1 = &zbpg->buddy[1];
-		spin_lock_init(&zbpg->lock);
-		atomic_inc(&zcache_zbud_curr_raw_pages);
-		INIT_LIST_HEAD(&zbpg->bud_list);
-		SET_SENTINEL(zbpg, ZBPG);
-		zh0->size = 0; zh1->size = 0;
-		tmem_oid_set_invalid(&zh0->oid);
-		tmem_oid_set_invalid(&zh1->oid);
-	}
-	return zbpg;
 }
+#endif
 
-static void zbud_free_raw_page(struct zbud_page *zbpg)
-{
-	struct zbud_hdr *zh0 = &zbpg->buddy[0], *zh1 = &zbpg->buddy[1];
+static int zcache_enabled __read_mostly;
+static int disable_cleancache __read_mostly;
+static int disable_frontswap __read_mostly;
+static int disable_frontswap_ignore_nonactive __read_mostly;
+static int disable_cleancache_ignore_nonactive __read_mostly;
+static char *namestr __read_mostly = "zcache";
 
-	ASSERT_SENTINEL(zbpg, ZBPG);
-	BUG_ON(!list_empty(&zbpg->bud_list));
-	ASSERT_SPINLOCK(&zbpg->lock);
-	BUG_ON(zh0->size != 0 || tmem_oid_valid(&zh0->oid));
-	BUG_ON(zh1->size != 0 || tmem_oid_valid(&zh1->oid));
-	INVERT_SENTINEL(zbpg, ZBPG);
-	spin_unlock(&zbpg->lock);
-	atomic_dec(&zcache_zbud_curr_raw_pages);
-	zcache_free_page(zbpg);
-}
+#define ZCACHE_GFP_MASK \
+	(__GFP_FS | __GFP_NORETRY | __GFP_NOWARN | __GFP_NOMEMALLOC)
 
-/*
- * core zbud handling routines
- */
+MODULE_LICENSE("GPL");
 
-static unsigned zbud_free(struct zbud_hdr *zh)
-{
-	unsigned size;
-
-	ASSERT_SENTINEL(zh, ZBH);
-	BUG_ON(!tmem_oid_valid(&zh->oid));
-	size = zh->size;
-	BUG_ON(zh->size == 0 || zh->size > zbud_max_buddy_size());
-	zh->size = 0;
-	tmem_oid_set_invalid(&zh->oid);
-	INVERT_SENTINEL(zh, ZBH);
-	zcache_zbud_curr_zbytes -= size;
-	atomic_dec(&zcache_zbud_curr_zpages);
-	return size;
-}
-
-static void zbud_free_and_delist(struct zbud_hdr *zh)
-{
-	unsigned chunks;
-	struct zbud_hdr *zh_other;
-	unsigned budnum = zbud_budnum(zh), size;
-	struct zbud_page *zbpg =
-		container_of(zh, struct zbud_page, buddy[budnum]);
-
-	/* FIXME, should be BUG_ON, pool destruction path doesn't disable
-	 * interrupts tmem_destroy_pool()->tmem_pampd_destroy_all_in_obj()->
-	 * tmem_objnode_node_destroy()-> zcache_pampd_free() */
-	WARN_ON(!irqs_disabled());
-	spin_lock(&zbpg->lock);
-	if (list_empty(&zbpg->bud_list)) {
-		/* ignore zombie page... see zbud_evict_pages() */
-		spin_unlock(&zbpg->lock);
-		return;
-	}
-	size = zbud_free(zh);
-	ASSERT_SPINLOCK(&zbpg->lock);
-	zh_other = &zbpg->buddy[(budnum == 0) ? 1 : 0];
-	if (zh_other->size == 0) { /* was unbuddied: unlist and free */
-		chunks = zbud_size_to_chunks(size) ;
-		spin_lock(&zbud_budlists_spinlock);
-		BUG_ON(list_empty(&zbud_unbuddied[chunks].list));
-		list_del_init(&zbpg->bud_list);
-		zbud_unbuddied[chunks].count--;
-		spin_unlock(&zbud_budlists_spinlock);
-		zbud_free_raw_page(zbpg);
-	} else { /* was buddied: move remaining buddy to unbuddied list */
-		chunks = zbud_size_to_chunks(zh_other->size) ;
-		spin_lock(&zbud_budlists_spinlock);
-		list_del_init(&zbpg->bud_list);
-		zcache_zbud_buddied_count--;
-		list_add_tail(&zbpg->bud_list, &zbud_unbuddied[chunks].list);
-		zbud_unbuddied[chunks].count++;
-		spin_unlock(&zbud_budlists_spinlock);
-		spin_unlock(&zbpg->lock);
-	}
-}
+/* crypto API for zcache  */
+#define ZCACHE_COMP_NAME_SZ CRYPTO_MAX_ALG_NAME
+static char zcache_comp_name[ZCACHE_COMP_NAME_SZ] __read_mostly;
+static struct crypto_comp * __percpu *zcache_comp_pcpu_tfms __read_mostly;
 
-static struct zbud_hdr *zbud_create(uint16_t client_id, uint16_t pool_id,
-					struct tmem_oid *oid,
-					uint32_t index, struct page *page,
-					void *cdata, unsigned size)
-{
-	struct zbud_hdr *zh0, *zh1, *zh = NULL;
-	struct zbud_page *zbpg = NULL, *ztmp;
-	unsigned nchunks;
-	char *to;
-	int i, found_good_buddy = 0;
-
-	nchunks = zbud_size_to_chunks(size) ;
-	for (i = MAX_CHUNK - nchunks + 1; i > 0; i--) {
-		spin_lock(&zbud_budlists_spinlock);
-		if (!list_empty(&zbud_unbuddied[i].list)) {
-			list_for_each_entry_safe(zbpg, ztmp,
-				    &zbud_unbuddied[i].list, bud_list) {
-				if (spin_trylock(&zbpg->lock)) {
-					found_good_buddy = i;
-					goto found_unbuddied;
-				}
-			}
-		}
-		spin_unlock(&zbud_budlists_spinlock);
-	}
-	/* didn't find a good buddy, try allocating a new page */
-	zbpg = zbud_alloc_raw_page();
-	if (unlikely(zbpg == NULL))
-		goto out;
-	/* ok, have a page, now compress the data before taking locks */
-	spin_lock(&zbud_budlists_spinlock);
-	spin_lock(&zbpg->lock);
-	list_add_tail(&zbpg->bud_list, &zbud_unbuddied[nchunks].list);
-	zbud_unbuddied[nchunks].count++;
-	zh = &zbpg->buddy[0];
-	goto init_zh;
-
-found_unbuddied:
-	ASSERT_SPINLOCK(&zbpg->lock);
-	zh0 = &zbpg->buddy[0]; zh1 = &zbpg->buddy[1];
-	BUG_ON(!((zh0->size == 0) ^ (zh1->size == 0)));
-	if (zh0->size != 0) { /* buddy0 in use, buddy1 is vacant */
-		ASSERT_SENTINEL(zh0, ZBH);
-		zh = zh1;
-	} else if (zh1->size != 0) { /* buddy1 in use, buddy0 is vacant */
-		ASSERT_SENTINEL(zh1, ZBH);
-		zh = zh0;
-	} else
-		BUG();
-	list_del_init(&zbpg->bud_list);
-	zbud_unbuddied[found_good_buddy].count--;
-	list_add_tail(&zbpg->bud_list, &zbud_buddied_list);
-	zcache_zbud_buddied_count++;
-
-init_zh:
-	SET_SENTINEL(zh, ZBH);
-	zh->size = size;
-	zh->index = index;
-	zh->oid = *oid;
-	zh->pool_id = pool_id;
-	zh->client_id = client_id;
-	to = zbud_data(zh, size);
-	memcpy(to, cdata, size);
-	spin_unlock(&zbpg->lock);
-	spin_unlock(&zbud_budlists_spinlock);
-	zbud_cumul_chunk_counts[nchunks]++;
-	atomic_inc(&zcache_zbud_curr_zpages);
-	zcache_zbud_cumul_zpages++;
-	zcache_zbud_curr_zbytes += size;
-	zcache_zbud_cumul_zbytes += size;
-out:
-	return zh;
-}
+enum comp_op {
+	ZCACHE_COMPOP_COMPRESS,
+	ZCACHE_COMPOP_DECOMPRESS
+};
 
-static int zbud_decompress(struct page *page, struct zbud_hdr *zh)
+static inline int zcache_comp_op(enum comp_op op,
+				const u8 *src, unsigned int slen,
+				u8 *dst, unsigned int *dlen)
 {
-	struct zbud_page *zbpg;
-	unsigned budnum = zbud_budnum(zh);
-	size_t out_len = PAGE_SIZE;
-	char *to_va, *from_va;
-	unsigned size;
-	int ret = 0;
+	struct crypto_comp *tfm;
+	int ret = -1;
 
-	zbpg = container_of(zh, struct zbud_page, buddy[budnum]);
-	spin_lock(&zbpg->lock);
-	if (list_empty(&zbpg->bud_list)) {
-		/* ignore zombie page... see zbud_evict_pages() */
+	BUG_ON(!zcache_comp_pcpu_tfms);
+	tfm = *per_cpu_ptr(zcache_comp_pcpu_tfms, get_cpu());
+	BUG_ON(!tfm);
+	switch (op) {
+	case ZCACHE_COMPOP_COMPRESS:
+		ret = crypto_comp_compress(tfm, src, slen, dst, dlen);
+		break;
+	case ZCACHE_COMPOP_DECOMPRESS:
+		ret = crypto_comp_decompress(tfm, src, slen, dst, dlen);
+		break;
+	default:
 		ret = -EINVAL;
-		goto out;
 	}
-	ASSERT_SENTINEL(zh, ZBH);
-	BUG_ON(zh->size == 0 || zh->size > zbud_max_buddy_size());
-	to_va = kmap_atomic(page);
-	size = zh->size;
-	from_va = zbud_data(zh, size);
-	ret = lzo1x_decompress_safe(from_va, size, to_va, &out_len);
-	BUG_ON(ret != LZO_E_OK);
-	BUG_ON(out_len != PAGE_SIZE);
-	kunmap_atomic(to_va);
-out:
-	spin_unlock(&zbpg->lock);
+	put_cpu();
 	return ret;
 }
 
 /*
- * The following routines handle shrinking of ephemeral pages by evicting
- * pages "least valuable" first.
- */
-
-static unsigned long zcache_evicted_raw_pages;
-static unsigned long zcache_evicted_buddied_pages;
-static unsigned long zcache_evicted_unbuddied_pages;
-
-static struct tmem_pool *zcache_get_pool_by_id(uint16_t cli_id,
-						uint16_t poolid);
-static void zcache_put_pool(struct tmem_pool *pool);
-
-/*
- * Flush and free all zbuds in a zbpg, then free the pageframe
- */
-static void zbud_evict_zbpg(struct zbud_page *zbpg)
-{
-	struct zbud_hdr *zh;
-	int i, j;
-	uint32_t pool_id[ZBUD_MAX_BUDS], client_id[ZBUD_MAX_BUDS];
-	uint32_t index[ZBUD_MAX_BUDS];
-	struct tmem_oid oid[ZBUD_MAX_BUDS];
-	struct tmem_pool *pool;
-	unsigned long flags;
-
-	ASSERT_SPINLOCK(&zbpg->lock);
-	for (i = 0, j = 0; i < ZBUD_MAX_BUDS; i++) {
-		zh = &zbpg->buddy[i];
-		if (zh->size) {
-			client_id[j] = zh->client_id;
-			pool_id[j] = zh->pool_id;
-			oid[j] = zh->oid;
-			index[j] = zh->index;
-			j++;
-		}
-	}
-	spin_unlock(&zbpg->lock);
-	for (i = 0; i < j; i++) {
-		pool = zcache_get_pool_by_id(client_id[i], pool_id[i]);
-		BUG_ON(pool == NULL);
-		local_irq_save(flags);
-		/* these flushes should dispose of any local storage */
-		tmem_flush_page(pool, &oid[i], index[i]);
-		local_irq_restore(flags);
-		zcache_put_pool(pool);
-	}
-}
-
-/*
- * Free nr pages.  This code is funky because we want to hold the locks
- * protecting various lists for as short a time as possible, and in some
- * circumstances the list may change asynchronously when the list lock is
- * not held.  In some cases we also trylock not only to avoid waiting on a
- * page in use by another cpu, but also to avoid potential deadlock due to
- * lock inversion.
- */
-static void zbud_evict_pages(int nr)
-{
-	struct zbud_page *zbpg;
-	int i, newly_unused_pages = 0;
-
-
-	/* now try freeing unbuddied pages, starting with least space avail */
-	for (i = 0; i < MAX_CHUNK; i++) {
-retry_unbud_list_i:
-		spin_lock_bh(&zbud_budlists_spinlock);
-		if (list_empty(&zbud_unbuddied[i].list)) {
-			spin_unlock_bh(&zbud_budlists_spinlock);
-			continue;
-		}
-		list_for_each_entry(zbpg, &zbud_unbuddied[i].list, bud_list) {
-			if (unlikely(!spin_trylock(&zbpg->lock)))
-				continue;
-			zbud_unbuddied[i].count--;
-			spin_unlock(&zbud_budlists_spinlock);
-			zcache_evicted_unbuddied_pages++;
-			/* want budlists unlocked when doing zbpg eviction */
-			zbud_evict_zbpg(zbpg);
-			newly_unused_pages++;
-			local_bh_enable();
-			if (--nr <= 0)
-				goto evict_unused;
-			goto retry_unbud_list_i;
-		}
-		spin_unlock_bh(&zbud_budlists_spinlock);
-	}
-
-	/* as a last resort, free buddied pages */
-retry_bud_list:
-	spin_lock_bh(&zbud_budlists_spinlock);
-	if (list_empty(&zbud_buddied_list)) {
-		spin_unlock_bh(&zbud_budlists_spinlock);
-		goto evict_unused;
-	}
-	list_for_each_entry(zbpg, &zbud_buddied_list, bud_list) {
-		if (unlikely(!spin_trylock(&zbpg->lock)))
-			continue;
-		zcache_zbud_buddied_count--;
-		spin_unlock(&zbud_budlists_spinlock);
-		zcache_evicted_buddied_pages++;
-		/* want budlists unlocked when doing zbpg eviction */
-		zbud_evict_zbpg(zbpg);
-		newly_unused_pages++;
-		local_bh_enable();
-		if (--nr <= 0)
-			goto evict_unused;
-		goto retry_bud_list;
-	}
-	spin_unlock_bh(&zbud_budlists_spinlock);
-
-evict_unused:
-	return;
-}
-
-static DEFINE_PER_CPU(unsigned char *, zcache_remoteputmem);
-
-static int zbud_remotify_zbud(struct tmem_xhandle *xh, char *data,
-				size_t size)
-{
-	struct tmem_pool *pool;
-	int i, remotenode, ret = -1;
-	unsigned char cksum, *p;
-	unsigned long flags;
-
-	for (p = data, cksum = 0, i = 0; i < size; i++)
-		cksum += *p;
-	ret = ramster_remote_put(xh, data, size, true, &remotenode);
-	if (ret == 0) {
-		/* data was successfully remoted so change the local version
-		 * to point to the remote node where it landed */
-		pool = zcache_get_pool_by_id(LOCAL_CLIENT, xh->pool_id);
-		BUG_ON(pool == NULL);
-		local_irq_save(flags);
-		/* tmem_replace will also free up any local space */
-		(void)tmem_replace(pool, &xh->oid, xh->index,
-			pampd_make_remote(remotenode, size, cksum));
-		local_irq_restore(flags);
-		zcache_put_pool(pool);
-		ramster_eph_pages_remoted++;
-		ret = 0;
-	} else
-		ramster_eph_pages_remote_failed++;
-	return ret;
-}
-
-static int zbud_remotify_zbpg(struct zbud_page *zbpg)
-{
-	struct zbud_hdr *zh1, *zh2 = NULL;
-	struct tmem_xhandle xh1, xh2 = { 0 };
-	char *data1 = NULL, *data2 = NULL;
-	size_t size1 = 0, size2 = 0;
-	int ret = 0;
-	unsigned char *tmpmem = __get_cpu_var(zcache_remoteputmem);
-
-	ASSERT_SPINLOCK(&zbpg->lock);
-	if (zbpg->buddy[0].size == 0)
-		zh1 = &zbpg->buddy[1];
-	else if (zbpg->buddy[1].size == 0)
-		zh1 = &zbpg->buddy[0];
-	else {
-		zh1 = &zbpg->buddy[0];
-		zh2 = &zbpg->buddy[1];
-	}
-	/* don't remotify pages that are already remotified */
-	if (zh1->client_id != LOCAL_CLIENT)
-		zh1 = NULL;
-	if ((zh2 != NULL) && (zh2->client_id != LOCAL_CLIENT))
-		zh2 = NULL;
-
-	/* copy the data and metadata so can release lock */
-	if (zh1 != NULL) {
-		xh1.client_id = zh1->client_id;
-		xh1.pool_id = zh1->pool_id;
-		xh1.oid = zh1->oid;
-		xh1.index = zh1->index;
-		size1 = zh1->size;
-		data1 = zbud_data(zh1, size1);
-		memcpy(tmpmem, zbud_data(zh1, size1), size1);
-		data1 = tmpmem;
-		tmpmem += size1;
-	}
-	if (zh2 != NULL) {
-		xh2.client_id = zh2->client_id;
-		xh2.pool_id = zh2->pool_id;
-		xh2.oid = zh2->oid;
-		xh2.index = zh2->index;
-		size2 = zh2->size;
-		memcpy(tmpmem, zbud_data(zh2, size2), size2);
-		data2 = tmpmem;
-	}
-	spin_unlock(&zbpg->lock);
-	preempt_enable();
-
-	/* OK, no locks held anymore, remotify one or both zbuds */
-	if (zh1 != NULL)
-		ret = zbud_remotify_zbud(&xh1, data1, size1);
-	if (zh2 != NULL)
-		ret |= zbud_remotify_zbud(&xh2, data2, size2);
-	return ret;
-}
-
-void zbud_remotify_pages(int nr)
-{
-	struct zbud_page *zbpg;
-	int i, ret;
-
-	/*
-	 * for now just try remotifying unbuddied pages, starting with
-	 * least space avail
-	 */
-	for (i = 0; i < MAX_CHUNK; i++) {
-retry_unbud_list_i:
-		preempt_disable();  /* enable in zbud_remotify_zbpg */
-		spin_lock_bh(&zbud_budlists_spinlock);
-		if (list_empty(&zbud_unbuddied[i].list)) {
-			spin_unlock_bh(&zbud_budlists_spinlock);
-			preempt_enable();
-			continue; /* next i in for loop */
-		}
-		list_for_each_entry(zbpg, &zbud_unbuddied[i].list, bud_list) {
-			if (unlikely(!spin_trylock(&zbpg->lock)))
-				continue; /* next list_for_each_entry */
-			zbud_unbuddied[i].count--;
-			/* want budlists unlocked when doing zbpg remotify */
-			spin_unlock_bh(&zbud_budlists_spinlock);
-			ret = zbud_remotify_zbpg(zbpg);
-			/* preemption is re-enabled in zbud_remotify_zbpg */
-			if (ret == 0) {
-				if (--nr <= 0)
-					goto out;
-				goto retry_unbud_list_i;
-			}
-			/* if fail to remotify any page, quit */
-			pr_err("TESTING zbud_remotify_pages failed on page,"
-				" trying to re-add\n");
-			spin_lock_bh(&zbud_budlists_spinlock);
-			spin_lock(&zbpg->lock);
-			list_add_tail(&zbpg->bud_list, &zbud_unbuddied[i].list);
-			zbud_unbuddied[i].count++;
-			spin_unlock(&zbpg->lock);
-			spin_unlock_bh(&zbud_budlists_spinlock);
-			pr_err("TESTING zbud_remotify_pages failed on page,"
-				" finished re-add\n");
-			goto out;
-		}
-		spin_unlock_bh(&zbud_budlists_spinlock);
-		preempt_enable();
-	}
-
-next_buddied_zbpg:
-	preempt_disable();  /* enable in zbud_remotify_zbpg */
-	spin_lock_bh(&zbud_budlists_spinlock);
-	if (list_empty(&zbud_buddied_list))
-		goto unlock_out;
-	list_for_each_entry(zbpg, &zbud_buddied_list, bud_list) {
-		if (unlikely(!spin_trylock(&zbpg->lock)))
-			continue; /* next list_for_each_entry */
-		zcache_zbud_buddied_count--;
-		/* want budlists unlocked when doing zbpg remotify */
-		spin_unlock_bh(&zbud_budlists_spinlock);
-		ret = zbud_remotify_zbpg(zbpg);
-		/* preemption is re-enabled in zbud_remotify_zbpg */
-		if (ret == 0) {
-			if (--nr <= 0)
-				goto out;
-			goto next_buddied_zbpg;
-		}
-		/* if fail to remotify any page, quit */
-		pr_err("TESTING zbud_remotify_pages failed on BUDDIED page,"
-			" trying to re-add\n");
-		spin_lock_bh(&zbud_budlists_spinlock);
-		spin_lock(&zbpg->lock);
-		list_add_tail(&zbpg->bud_list, &zbud_buddied_list);
-		zcache_zbud_buddied_count++;
-		spin_unlock(&zbpg->lock);
-		spin_unlock_bh(&zbud_budlists_spinlock);
-		pr_err("TESTING zbud_remotify_pages failed on BUDDIED page,"
-			" finished re-add\n");
-		goto out;
-	}
-unlock_out:
-	spin_unlock_bh(&zbud_budlists_spinlock);
-	preempt_enable();
-out:
-	return;
-}
-
-/* the "flush list" asynchronously collects pages to remotely flush */
-#define FLUSH_ENTIRE_OBJECT ((uint32_t)-1)
-static void ramster_flnode_free(struct flushlist_node *,
-				struct tmem_pool *);
-
-static void zcache_remote_flush_page(struct flushlist_node *flnode)
-{
-	struct tmem_xhandle *xh;
-	int remotenode, ret;
-
-	preempt_disable();
-	xh = &flnode->xh;
-	remotenode = flnode->xh.client_id;
-	ret = ramster_remote_flush(xh, remotenode);
-	if (ret >= 0)
-		ramster_remote_pages_flushed++;
-	else
-		ramster_remote_page_flushes_failed++;
-	preempt_enable_no_resched();
-	ramster_flnode_free(flnode, NULL);
-}
-
-static void zcache_remote_flush_object(struct flushlist_node *flnode)
-{
-	struct tmem_xhandle *xh;
-	int remotenode, ret;
-
-	preempt_disable();
-	xh = &flnode->xh;
-	remotenode = flnode->xh.client_id;
-	ret = ramster_remote_flush_object(xh, remotenode);
-	if (ret >= 0)
-		ramster_remote_objects_flushed++;
-	else
-		ramster_remote_object_flushes_failed++;
-	preempt_enable_no_resched();
-	ramster_flnode_free(flnode, NULL);
-}
-
-static void zcache_remote_eph_put(struct zbud_hdr *zbud)
-{
-	/* FIXME */
-}
-
-static void zcache_remote_pers_put(struct zv_hdr *zv)
-{
-	struct tmem_xhandle xh;
-	uint16_t size;
-	bool ephemeral;
-	int remotenode, ret = -1;
-	char *data;
-	struct tmem_pool *pool;
-	unsigned long flags;
-	unsigned char cksum;
-	char *p;
-	int i;
-	unsigned char *tmpmem = __get_cpu_var(zcache_remoteputmem);
-
-	ASSERT_SENTINEL(zv, ZVH);
-	BUG_ON(zv->client_id != LOCAL_CLIENT);
-	local_bh_disable();
-	xh.client_id = zv->client_id;
-	xh.pool_id = zv->pool_id;
-	xh.oid = zv->oid;
-	xh.index = zv->index;
-	size = xv_get_object_size(zv) - sizeof(*zv);
-	BUG_ON(size == 0 || size > zv_max_page_size);
-	data = (char *)zv + sizeof(*zv);
-	for (p = data, cksum = 0, i = 0; i < size; i++)
-		cksum += *p;
-	memcpy(tmpmem, data, size);
-	data = tmpmem;
-	pool = zcache_get_pool_by_id(zv->client_id, zv->pool_id);
-	ephemeral = is_ephemeral(pool);
-	zcache_put_pool(pool);
-	/* now OK to release lock set in caller */
-	spin_unlock(&zcache_rem_op_list_lock);
-	local_bh_enable();
-	preempt_disable();
-	ret = ramster_remote_put(&xh, data, size, ephemeral, &remotenode);
-	preempt_enable_no_resched();
-	if (ret != 0) {
-		/*
-		 * This is some form of a memory leak... if the remote put
-		 * fails, there will never be another attempt to remotify
-		 * this page.  But since we've dropped the zv pointer,
-		 * the page may have been freed or the data replaced
-		 * so we can't just "put it back" in the remote op list.
-		 * Even if we could, not sure where to put it in the list
-		 * because there may be flushes that must be strictly
-		 * ordered vs the put.  So leave this as a FIXME for now.
-		 * But count them so we know if it becomes a problem.
-		 */
-		ramster_pers_pages_remote_failed++;
-		goto out;
-	} else
-		atomic_inc(&ramster_remote_pers_pages);
-	ramster_pers_pages_remoted++;
-	/*
-	 * data was successfully remoted so change the local version to
-	 * point to the remote node where it landed
-	 */
-	local_bh_disable();
-	pool = zcache_get_pool_by_id(LOCAL_CLIENT, xh.pool_id);
-	local_irq_save(flags);
-	(void)tmem_replace(pool, &xh.oid, xh.index,
-			pampd_make_remote(remotenode, size, cksum));
-	local_irq_restore(flags);
-	zcache_put_pool(pool);
-	local_bh_enable();
-out:
-	return;
-}
-
-static void zcache_do_remotify_ops(int nr)
-{
-	struct ramster_remotify_hdr *rem_op;
-	union remotify_list_node *u;
-
-	while (1) {
-		if (!nr)
-			goto out;
-		spin_lock(&zcache_rem_op_list_lock);
-		if (list_empty(&zcache_rem_op_list)) {
-			spin_unlock(&zcache_rem_op_list_lock);
-			goto out;
-		}
-		rem_op = list_first_entry(&zcache_rem_op_list,
-				struct ramster_remotify_hdr, list);
-		list_del_init(&rem_op->list);
-		if (rem_op->op != RAMSTER_REMOTIFY_PERS_PUT)
-			spin_unlock(&zcache_rem_op_list_lock);
-		u = (union remotify_list_node *)rem_op;
-		switch (rem_op->op) {
-		case RAMSTER_REMOTIFY_EPH_PUT:
-BUG();
-			zcache_remote_eph_put((struct zbud_hdr *)rem_op);
-			break;
-		case RAMSTER_REMOTIFY_PERS_PUT:
-			zcache_remote_pers_put((struct zv_hdr *)rem_op);
-			break;
-		case RAMSTER_REMOTIFY_FLUSH_PAGE:
-			zcache_remote_flush_page((struct flushlist_node *)u);
-			break;
-		case RAMSTER_REMOTIFY_FLUSH_OBJ:
-			zcache_remote_flush_object((struct flushlist_node *)u);
-			break;
-		default:
-			BUG();
-		}
-	}
-out:
-	return;
-}
-
-/*
- * Communicate interface revision with userspace
- */
-#include "cluster/ramster_nodemanager.h"
-static unsigned long ramster_interface_revision  = R2NM_API_VERSION;
-
-/*
- * For now, just push over a few pages every few seconds to
- * ensure that it basically works
- */
-static struct workqueue_struct *ramster_remotify_workqueue;
-static void ramster_remotify_process(struct work_struct *work);
-static DECLARE_DELAYED_WORK(ramster_remotify_worker,
-		ramster_remotify_process);
-
-static void ramster_remotify_queue_delayed_work(unsigned long delay)
-{
-	if (!queue_delayed_work(ramster_remotify_workqueue,
-				&ramster_remotify_worker, delay))
-		pr_err("ramster_remotify: bad workqueue\n");
-}
-
-
-static int use_frontswap;
-static int use_cleancache;
-static int ramster_remote_target_nodenum = -1;
-static void ramster_remotify_process(struct work_struct *work)
-{
-	static bool remotify_in_progress;
-
-	BUG_ON(irqs_disabled());
-	if (remotify_in_progress)
-		ramster_remotify_queue_delayed_work(HZ);
-	else if (ramster_remote_target_nodenum != -1) {
-		remotify_in_progress = true;
-#ifdef CONFIG_CLEANCACHE
-	if (use_cleancache && ramster_eph_remotify_enable)
-		zbud_remotify_pages(5000); /* FIXME is this a good number? */
-#endif
-#ifdef CONFIG_FRONTSWAP
-	if (use_frontswap && ramster_pers_remotify_enable)
-		zcache_do_remotify_ops(500); /* FIXME is this a good number? */
-#endif
-		remotify_in_progress = false;
-		ramster_remotify_queue_delayed_work(HZ);
-	}
-}
-
-static void ramster_remotify_init(void)
-{
-	unsigned long n = 60UL;
-	ramster_remotify_workqueue =
-		create_singlethread_workqueue("ramster_remotify");
-	ramster_remotify_queue_delayed_work(n * HZ);
-}
-
-
-static void zbud_init(void)
-{
-	int i;
-
-	INIT_LIST_HEAD(&zbud_buddied_list);
-	zcache_zbud_buddied_count = 0;
-	for (i = 0; i < NCHUNKS; i++) {
-		INIT_LIST_HEAD(&zbud_unbuddied[i].list);
-		zbud_unbuddied[i].count = 0;
-	}
-}
-
-#ifdef CONFIG_SYSFS
-/*
- * These sysfs routines show a nice distribution of how many zbpg's are
- * currently (and have ever been placed) in each unbuddied list.  It's fun
- * to watch but can probably go away before final merge.
+ * policy parameters
  */
-static int zbud_show_unbuddied_list_counts(char *buf)
-{
-	int i;
-	char *p = buf;
-
-	for (i = 0; i < NCHUNKS; i++)
-		p += sprintf(p, "%u ", zbud_unbuddied[i].count);
-	return p - buf;
-}
-
-static int zbud_show_cumul_chunk_counts(char *buf)
-{
-	unsigned long i, chunks = 0, total_chunks = 0, sum_total_chunks = 0;
-	unsigned long total_chunks_lte_21 = 0, total_chunks_lte_32 = 0;
-	unsigned long total_chunks_lte_42 = 0;
-	char *p = buf;
-
-	for (i = 0; i < NCHUNKS; i++) {
-		p += sprintf(p, "%lu ", zbud_cumul_chunk_counts[i]);
-		chunks += zbud_cumul_chunk_counts[i];
-		total_chunks += zbud_cumul_chunk_counts[i];
-		sum_total_chunks += i * zbud_cumul_chunk_counts[i];
-		if (i == 21)
-			total_chunks_lte_21 = total_chunks;
-		if (i == 32)
-			total_chunks_lte_32 = total_chunks;
-		if (i == 42)
-			total_chunks_lte_42 = total_chunks;
-	}
-	p += sprintf(p, "<=21:%lu <=32:%lu <=42:%lu, mean:%lu\n",
-		total_chunks_lte_21, total_chunks_lte_32, total_chunks_lte_42,
-		chunks == 0 ? 0 : sum_total_chunks / chunks);
-	return p - buf;
-}
-#endif
 
-/**********
- * This "zv" PAM implementation combines the TLSF-based xvMalloc
- * with lzo1x compression to maximize the amount of data that can
- * be packed into a physical page.
- *
- * Zv represents a PAM page with the index and object (plus a "size" value
- * necessary for decompression) immediately preceding the compressed data.
- */
-
-/* rudimentary policy limits */
-/* total number of persistent pages may not exceed this percentage */
-static unsigned int zv_page_count_policy_percent = 75;
 /*
  * byte count defining poor compression; pages with greater zsize will be
  * rejected
  */
-static unsigned int zv_max_zsize = (PAGE_SIZE / 8) * 7;
+static unsigned int zbud_max_zsize __read_mostly = (PAGE_SIZE / 8) * 7;
 /*
  * byte count defining poor *mean* compression; pages with greater zsize
  * will be rejected until sufficient better-compressed pages are accepted
  * driving the mean below this threshold
  */
-static unsigned int zv_max_mean_zsize = (PAGE_SIZE / 8) * 5;
-
-static atomic_t zv_curr_dist_counts[NCHUNKS];
-static atomic_t zv_cumul_dist_counts[NCHUNKS];
+static unsigned int zbud_max_mean_zsize __read_mostly = (PAGE_SIZE / 8) * 5;
 
-
-static struct zv_hdr *zv_create(struct zcache_client *cli, uint32_t pool_id,
-				struct tmem_oid *oid, uint32_t index,
-				void *cdata, unsigned clen)
-{
-	struct page *page;
-	struct zv_hdr *zv = NULL;
-	uint32_t offset;
-	int alloc_size = clen + sizeof(struct zv_hdr);
-	int chunks = (alloc_size + (CHUNK_SIZE - 1)) >> CHUNK_SHIFT;
-	int ret;
-
-	BUG_ON(!irqs_disabled());
-	BUG_ON(chunks >= NCHUNKS);
-	ret = xv_malloc(cli->xvpool, clen + sizeof(struct zv_hdr),
-			&page, &offset, ZCACHE_GFP_MASK);
-	if (unlikely(ret))
-		goto out;
-	atomic_inc(&zv_curr_dist_counts[chunks]);
-	atomic_inc(&zv_cumul_dist_counts[chunks]);
-	zv = kmap_atomic(page) + offset;
-	zv->index = index;
-	zv->oid = *oid;
-	zv->pool_id = pool_id;
-	SET_SENTINEL(zv, ZVH);
-	INIT_LIST_HEAD(&zv->rem_op.list);
-	zv->client_id = get_client_id_from_client(cli);
-	zv->rem_op.op = RAMSTER_REMOTIFY_PERS_PUT;
-	if (zv->client_id == LOCAL_CLIENT) {
-		spin_lock(&zcache_rem_op_list_lock);
-		list_add_tail(&zv->rem_op.list, &zcache_rem_op_list);
-		spin_unlock(&zcache_rem_op_list_lock);
-	}
-	memcpy((char *)zv + sizeof(struct zv_hdr), cdata, clen);
-	kunmap_atomic(zv);
-out:
-	return zv;
-}
-
-/* similar to zv_create, but just reserve space, no data yet */
-static struct zv_hdr *zv_alloc(struct tmem_pool *pool,
-				struct tmem_oid *oid, uint32_t index,
-				unsigned clen)
-{
-	struct zcache_client *cli = pool->client;
-	struct page *page;
-	struct zv_hdr *zv = NULL;
-	uint32_t offset;
-	int ret;
-
-	BUG_ON(!irqs_disabled());
-	BUG_ON(!is_local_client(pool->client));
-	ret = xv_malloc(cli->xvpool, clen + sizeof(struct zv_hdr),
-			&page, &offset, ZCACHE_GFP_MASK);
-	if (unlikely(ret))
-		goto out;
-	zv = kmap_atomic(page) + offset;
-	SET_SENTINEL(zv, ZVH);
-	INIT_LIST_HEAD(&zv->rem_op.list);
-	zv->client_id = LOCAL_CLIENT;
-	zv->rem_op.op = RAMSTER_INTRANSIT_PERS;
-	zv->index = index;
-	zv->oid = *oid;
-	zv->pool_id = pool->pool_id;
-	kunmap_atomic(zv);
-out:
-	return zv;
-}
-
-static void zv_free(struct xv_pool *xvpool, struct zv_hdr *zv)
-{
-	unsigned long flags;
-	struct page *page;
-	uint32_t offset;
-	uint16_t size = xv_get_object_size(zv);
-	int chunks = (size + (CHUNK_SIZE - 1)) >> CHUNK_SHIFT;
-
-	ASSERT_SENTINEL(zv, ZVH);
-	BUG_ON(chunks >= NCHUNKS);
-	atomic_dec(&zv_curr_dist_counts[chunks]);
-	size -= sizeof(*zv);
-	spin_lock(&zcache_rem_op_list_lock);
-	size = xv_get_object_size(zv) - sizeof(*zv);
-	BUG_ON(size == 0);
-	INVERT_SENTINEL(zv, ZVH);
-	if (!list_empty(&zv->rem_op.list))
-		list_del_init(&zv->rem_op.list);
-	spin_unlock(&zcache_rem_op_list_lock);
-	page = virt_to_page(zv);
-	offset = (unsigned long)zv & ~PAGE_MASK;
-	local_irq_save(flags);
-	xv_free(xvpool, page, offset);
-	local_irq_restore(flags);
-}
-
-static void zv_decompress(struct page *page, struct zv_hdr *zv)
-{
-	size_t clen = PAGE_SIZE;
-	char *to_va;
-	unsigned size;
-	int ret;
-
-	ASSERT_SENTINEL(zv, ZVH);
-	size = xv_get_object_size(zv) - sizeof(*zv);
-	BUG_ON(size == 0);
-	to_va = kmap_atomic(page);
-	ret = lzo1x_decompress_safe((char *)zv + sizeof(*zv),
-					size, to_va, &clen);
-	kunmap_atomic(to_va);
-	BUG_ON(ret != LZO_E_OK);
-	BUG_ON(clen != PAGE_SIZE);
-}
-
-static void zv_copy_from_pampd(char *data, size_t *bufsize, struct zv_hdr *zv)
-{
-	unsigned size;
-
-	ASSERT_SENTINEL(zv, ZVH);
-	size = xv_get_object_size(zv) - sizeof(*zv);
-	BUG_ON(size == 0 || size > zv_max_page_size);
-	BUG_ON(size > *bufsize);
-	memcpy(data, (char *)zv + sizeof(*zv), size);
-	*bufsize = size;
-}
-
-static void zv_copy_to_pampd(struct zv_hdr *zv, char *data, size_t size)
-{
-	unsigned zv_size;
-
-	ASSERT_SENTINEL(zv, ZVH);
-	zv_size = xv_get_object_size(zv) - sizeof(*zv);
-	BUG_ON(zv_size != size);
-	BUG_ON(zv_size == 0 || zv_size > zv_max_page_size);
-	memcpy((char *)zv + sizeof(*zv), data, size);
-}
-
-#ifdef CONFIG_SYSFS
 /*
- * show a distribution of compression stats for zv pages.
+ * for now, used named slabs so can easily track usage; later can
+ * either just use kmalloc, or perhaps add a slab-like allocator
+ * to more carefully manage total memory utilization
  */
+static struct kmem_cache *zcache_objnode_cache;
+static struct kmem_cache *zcache_obj_cache;
 
-static int zv_curr_dist_counts_show(char *buf)
-{
-	unsigned long i, n, chunks = 0, sum_total_chunks = 0;
-	char *p = buf;
-
-	for (i = 0; i < NCHUNKS; i++) {
-		n = atomic_read(&zv_curr_dist_counts[i]);
-		p += sprintf(p, "%lu ", n);
-		chunks += n;
-		sum_total_chunks += i * n;
-	}
-	p += sprintf(p, "mean:%lu\n",
-		chunks == 0 ? 0 : sum_total_chunks / chunks);
-	return p - buf;
-}
-
-static int zv_cumul_dist_counts_show(char *buf)
-{
-	unsigned long i, n, chunks = 0, sum_total_chunks = 0;
-	char *p = buf;
-
-	for (i = 0; i < NCHUNKS; i++) {
-		n = atomic_read(&zv_cumul_dist_counts[i]);
-		p += sprintf(p, "%lu ", n);
-		chunks += n;
-		sum_total_chunks += i * n;
-	}
-	p += sprintf(p, "mean:%lu\n",
-		chunks == 0 ? 0 : sum_total_chunks / chunks);
-	return p - buf;
-}
+static DEFINE_PER_CPU(struct zcache_preload, zcache_preloads) = { 0, };
 
-/*
- * setting zv_max_zsize via sysfs causes all persistent (e.g. swap)
- * pages that don't compress to less than this value (including metadata
- * overhead) to be rejected.  We don't allow the value to get too close
- * to PAGE_SIZE.
- */
-static ssize_t zv_max_zsize_show(struct kobject *kobj,
-				    struct kobj_attribute *attr,
-				    char *buf)
-{
-	return sprintf(buf, "%u\n", zv_max_zsize);
+/* we try to keep these statistics SMP-consistent */
+static long zcache_obj_count;
+static atomic_t zcache_obj_atomic = ATOMIC_INIT(0);
+static long zcache_obj_count_max;
+static long zcache_objnode_count;
+static atomic_t zcache_objnode_atomic = ATOMIC_INIT(0);
+static long zcache_objnode_count_max;
+static u64 zcache_eph_zbytes;
+static atomic_long_t zcache_eph_zbytes_atomic = ATOMIC_INIT(0);
+static u64 zcache_eph_zbytes_max;
+static u64 zcache_pers_zbytes;
+static atomic_long_t zcache_pers_zbytes_atomic = ATOMIC_INIT(0);
+static u64 zcache_pers_zbytes_max;
+static long zcache_eph_pageframes;
+static atomic_t zcache_eph_pageframes_atomic = ATOMIC_INIT(0);
+static long zcache_eph_pageframes_max;
+static long zcache_pers_pageframes;
+static atomic_t zcache_pers_pageframes_atomic = ATOMIC_INIT(0);
+static long zcache_pers_pageframes_max;
+static long zcache_pageframes_alloced;
+static atomic_t zcache_pageframes_alloced_atomic = ATOMIC_INIT(0);
+static long zcache_pageframes_freed;
+static atomic_t zcache_pageframes_freed_atomic = ATOMIC_INIT(0);
+static long zcache_eph_zpages;
+static atomic_t zcache_eph_zpages_atomic = ATOMIC_INIT(0);
+static long zcache_eph_zpages_max;
+static long zcache_pers_zpages;
+static atomic_t zcache_pers_zpages_atomic = ATOMIC_INIT(0);
+static long zcache_pers_zpages_max;
+
+/* but for the rest of these, counting races are ok */
+static unsigned long zcache_flush_total;
+static unsigned long zcache_flush_found;
+static unsigned long zcache_flobj_total;
+static unsigned long zcache_flobj_found;
+static unsigned long zcache_failed_eph_puts;
+static unsigned long zcache_failed_pers_puts;
+static unsigned long zcache_failed_getfreepages;
+static unsigned long zcache_failed_alloc;
+static unsigned long zcache_put_to_flush;
+static unsigned long zcache_compress_poor;
+static unsigned long zcache_mean_compress_poor;
+static unsigned long zcache_eph_ate_tail;
+static unsigned long zcache_eph_ate_tail_failed;
+static unsigned long zcache_pers_ate_eph;
+static unsigned long zcache_pers_ate_eph_failed;
+static unsigned long zcache_evicted_eph_zpages;
+static unsigned long zcache_evicted_eph_pageframes;
+static unsigned long zcache_last_active_file_pageframes;
+static unsigned long zcache_last_inactive_file_pageframes;
+static unsigned long zcache_last_active_anon_pageframes;
+static unsigned long zcache_last_inactive_anon_pageframes;
+static unsigned long zcache_eph_nonactive_puts_ignored;
+static unsigned long zcache_pers_nonactive_puts_ignored;
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#define	zdfs	debugfs_create_size_t
+#define	zdfs64	debugfs_create_u64
+static int zcache_debugfs_init(void)
+{
+	struct dentry *root = debugfs_create_dir("zcache", NULL);
+	if (root == NULL)
+		return -ENXIO;
+
+	zdfs("obj_count", S_IRUGO, root, &zcache_obj_count);
+	zdfs("obj_count_max", S_IRUGO, root, &zcache_obj_count_max);
+	zdfs("objnode_count", S_IRUGO, root, &zcache_objnode_count);
+	zdfs("objnode_count_max", S_IRUGO, root, &zcache_objnode_count_max);
+	zdfs("flush_total", S_IRUGO, root, &zcache_flush_total);
+	zdfs("flush_found", S_IRUGO, root, &zcache_flush_found);
+	zdfs("flobj_total", S_IRUGO, root, &zcache_flobj_total);
+	zdfs("flobj_found", S_IRUGO, root, &zcache_flobj_found);
+	zdfs("failed_eph_puts", S_IRUGO, root, &zcache_failed_eph_puts);
+	zdfs("failed_pers_puts", S_IRUGO, root, &zcache_failed_pers_puts);
+	zdfs("failed_get_free_pages", S_IRUGO, root,
+				&zcache_failed_getfreepages);
+	zdfs("failed_alloc", S_IRUGO, root, &zcache_failed_alloc);
+	zdfs("put_to_flush", S_IRUGO, root, &zcache_put_to_flush);
+	zdfs("compress_poor", S_IRUGO, root, &zcache_compress_poor);
+	zdfs("mean_compress_poor", S_IRUGO, root, &zcache_mean_compress_poor);
+	zdfs("eph_ate_tail", S_IRUGO, root, &zcache_eph_ate_tail);
+	zdfs("eph_ate_tail_failed", S_IRUGO, root, &zcache_eph_ate_tail_failed);
+	zdfs("pers_ate_eph", S_IRUGO, root, &zcache_pers_ate_eph);
+	zdfs("pers_ate_eph_failed", S_IRUGO, root, &zcache_pers_ate_eph_failed);
+	zdfs("evicted_eph_zpages", S_IRUGO, root, &zcache_evicted_eph_zpages);
+	zdfs("evicted_eph_pageframes", S_IRUGO, root,
+				&zcache_evicted_eph_pageframes);
+	zdfs("eph_pageframes", S_IRUGO, root, &zcache_eph_pageframes);
+	zdfs("eph_pageframes_max", S_IRUGO, root, &zcache_eph_pageframes_max);
+	zdfs("pers_pageframes", S_IRUGO, root, &zcache_pers_pageframes);
+	zdfs("pers_pageframes_max", S_IRUGO, root, &zcache_pers_pageframes_max);
+	zdfs("eph_zpages", S_IRUGO, root, &zcache_eph_zpages);
+	zdfs("eph_zpages_max", S_IRUGO, root, &zcache_eph_zpages_max);
+	zdfs("pers_zpages", S_IRUGO, root, &zcache_pers_zpages);
+	zdfs("pers_zpages_max", S_IRUGO, root, &zcache_pers_zpages_max);
+	zdfs("last_active_file_pageframes", S_IRUGO, root,
+				&zcache_last_active_file_pageframes);
+	zdfs("last_inactive_file_pageframes", S_IRUGO, root,
+				&zcache_last_inactive_file_pageframes);
+	zdfs("last_active_anon_pageframes", S_IRUGO, root,
+				&zcache_last_active_anon_pageframes);
+	zdfs("last_inactive_anon_pageframes", S_IRUGO, root,
+				&zcache_last_inactive_anon_pageframes);
+	zdfs("eph_nonactive_puts_ignored", S_IRUGO, root,
+				&zcache_eph_nonactive_puts_ignored);
+	zdfs("pers_nonactive_puts_ignored", S_IRUGO, root,
+				&zcache_pers_nonactive_puts_ignored);
+	zdfs64("eph_zbytes", S_IRUGO, root, &zcache_eph_zbytes);
+	zdfs64("eph_zbytes_max", S_IRUGO, root, &zcache_eph_zbytes_max);
+	zdfs64("pers_zbytes", S_IRUGO, root, &zcache_pers_zbytes);
+	zdfs64("pers_zbytes_max", S_IRUGO, root, &zcache_pers_zbytes_max);
+	return 0;
 }
+#undef	zdebugfs
+#undef	zdfs64
+#endif
 
-static ssize_t zv_max_zsize_store(struct kobject *kobj,
-				    struct kobj_attribute *attr,
-				    const char *buf, size_t count)
-{
-	unsigned long val;
-	int err;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
-	err = kstrtoul(buf, 10, &val);
-	if (err || (val == 0) || (val > (PAGE_SIZE / 8) * 7))
-		return -EINVAL;
-	zv_max_zsize = val;
-	return count;
+#define ZCACHE_DEBUG
+#ifdef ZCACHE_DEBUG
+/* developers can call this in case of ooms, e.g. to find memory leaks */
+void zcache_dump(void)
+{
+	pr_info("zcache: obj_count=%lu\n", zcache_obj_count);
+	pr_info("zcache: obj_count_max=%lu\n", zcache_obj_count_max);
+	pr_info("zcache: objnode_count=%lu\n", zcache_objnode_count);
+	pr_info("zcache: objnode_count_max=%lu\n", zcache_objnode_count_max);
+	pr_info("zcache: flush_total=%lu\n", zcache_flush_total);
+	pr_info("zcache: flush_found=%lu\n", zcache_flush_found);
+	pr_info("zcache: flobj_total=%lu\n", zcache_flobj_total);
+	pr_info("zcache: flobj_found=%lu\n", zcache_flobj_found);
+	pr_info("zcache: failed_eph_puts=%lu\n", zcache_failed_eph_puts);
+	pr_info("zcache: failed_pers_puts=%lu\n", zcache_failed_pers_puts);
+	pr_info("zcache: failed_get_free_pages=%lu\n",
+				zcache_failed_getfreepages);
+	pr_info("zcache: failed_alloc=%lu\n", zcache_failed_alloc);
+	pr_info("zcache: put_to_flush=%lu\n", zcache_put_to_flush);
+	pr_info("zcache: compress_poor=%lu\n", zcache_compress_poor);
+	pr_info("zcache: mean_compress_poor=%lu\n",
+				zcache_mean_compress_poor);
+	pr_info("zcache: eph_ate_tail=%lu\n", zcache_eph_ate_tail);
+	pr_info("zcache: eph_ate_tail_failed=%lu\n",
+				zcache_eph_ate_tail_failed);
+	pr_info("zcache: pers_ate_eph=%lu\n", zcache_pers_ate_eph);
+	pr_info("zcache: pers_ate_eph_failed=%lu\n",
+				zcache_pers_ate_eph_failed);
+	pr_info("zcache: evicted_eph_zpages=%lu\n", zcache_evicted_eph_zpages);
+	pr_info("zcache: evicted_eph_pageframes=%lu\n",
+				zcache_evicted_eph_pageframes);
+	pr_info("zcache: eph_pageframes=%lu\n", zcache_eph_pageframes);
+	pr_info("zcache: eph_pageframes_max=%lu\n", zcache_eph_pageframes_max);
+	pr_info("zcache: pers_pageframes=%lu\n", zcache_pers_pageframes);
+	pr_info("zcache: pers_pageframes_max=%lu\n",
+				zcache_pers_pageframes_max);
+	pr_info("zcache: eph_zpages=%lu\n", zcache_eph_zpages);
+	pr_info("zcache: eph_zpages_max=%lu\n", zcache_eph_zpages_max);
+	pr_info("zcache: pers_zpages=%lu\n", zcache_pers_zpages);
+	pr_info("zcache: pers_zpages_max=%lu\n", zcache_pers_zpages_max);
+	pr_info("zcache: eph_zbytes=%llu\n",
+				(unsigned long long)zcache_eph_zbytes);
+	pr_info("zcache: eph_zbytes_max=%llu\n",
+				(unsigned long long)zcache_eph_zbytes_max);
+	pr_info("zcache: pers_zbytes=%llu\n",
+				(unsigned long long)zcache_pers_zbytes);
+	pr_info("zcache: pers_zbytes_max=%llu\n",
+			(unsigned long long)zcache_pers_zbytes_max);
 }
+#endif
 
 /*
- * setting zv_max_mean_zsize via sysfs causes all persistent (e.g. swap)
- * pages that don't compress to less than this value (including metadata
- * overhead) to be rejected UNLESS the mean compression is also smaller
- * than this value.  In other words, we are load-balancing-by-zsize the
- * accepted pages.  Again, we don't allow the value to get too close
- * to PAGE_SIZE.
+ * zcache core code starts here
  */
-static ssize_t zv_max_mean_zsize_show(struct kobject *kobj,
-				    struct kobj_attribute *attr,
-				    char *buf)
-{
-	return sprintf(buf, "%u\n", zv_max_mean_zsize);
-}
-
-static ssize_t zv_max_mean_zsize_store(struct kobject *kobj,
-				    struct kobj_attribute *attr,
-				    const char *buf, size_t count)
-{
-	unsigned long val;
-	int err;
 
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
-	err = kstrtoul(buf, 10, &val);
-	if (err || (val == 0) || (val > (PAGE_SIZE / 8) * 7))
-		return -EINVAL;
-	zv_max_mean_zsize = val;
-	return count;
-}
+static struct zcache_client zcache_host;
+static struct zcache_client zcache_clients[MAX_CLIENTS];
 
-/*
- * setting zv_page_count_policy_percent via sysfs sets an upper bound of
- * persistent (e.g. swap) pages that will be retained according to:
- *     (zv_page_count_policy_percent * totalram_pages) / 100)
- * when that limit is reached, further puts will be rejected (until
- * some pages have been flushed).  Note that, due to compression,
- * this number may exceed 100; it defaults to 75 and we set an
- * arbitrary limit of 150.  A poor choice will almost certainly result
- * in OOM's, so this value should only be changed prudently.
- */
-static ssize_t zv_page_count_policy_percent_show(struct kobject *kobj,
-						 struct kobj_attribute *attr,
-						 char *buf)
+static inline bool is_local_client(struct zcache_client *cli)
 {
-	return sprintf(buf, "%u\n", zv_page_count_policy_percent);
+	return cli == &zcache_host;
 }
 
-static ssize_t zv_page_count_policy_percent_store(struct kobject *kobj,
-						  struct kobj_attribute *attr,
-						  const char *buf, size_t count)
+static struct zcache_client *zcache_get_client_by_id(uint16_t cli_id)
 {
-	unsigned long val;
-	int err;
+	struct zcache_client *cli = &zcache_host;
 
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
-	err = kstrtoul(buf, 10, &val);
-	if (err || (val == 0) || (val > 150))
-		return -EINVAL;
-	zv_page_count_policy_percent = val;
-	return count;
+	if (cli_id != LOCAL_CLIENT) {
+		if (cli_id >= MAX_CLIENTS)
+			goto out;
+		cli = &zcache_clients[cli_id];
+	}
+out:
+	return cli;
 }
 
-static struct kobj_attribute zcache_zv_max_zsize_attr = {
-		.attr = { .name = "zv_max_zsize", .mode = 0644 },
-		.show = zv_max_zsize_show,
-		.store = zv_max_zsize_store,
-};
-
-static struct kobj_attribute zcache_zv_max_mean_zsize_attr = {
-		.attr = { .name = "zv_max_mean_zsize", .mode = 0644 },
-		.show = zv_max_mean_zsize_show,
-		.store = zv_max_mean_zsize_store,
-};
-
-static struct kobj_attribute zcache_zv_page_count_policy_percent_attr = {
-		.attr = { .name = "zv_page_count_policy_percent",
-			  .mode = 0644 },
-		.show = zv_page_count_policy_percent_show,
-		.store = zv_page_count_policy_percent_store,
-};
-#endif
-
-/*
- * zcache core code starts here
- */
-
-/* useful stats not collected by cleancache or frontswap */
-static unsigned long zcache_flush_total;
-static unsigned long zcache_flush_found;
-static unsigned long zcache_flobj_total;
-static unsigned long zcache_flobj_found;
-static unsigned long zcache_failed_eph_puts;
-static unsigned long zcache_nonactive_puts;
-static unsigned long zcache_failed_pers_puts;
-
 /*
  * Tmem operations assume the poolid implies the invoking client.
  * Zcache only has one client (the kernel itself): LOCAL_CLIENT.
@@ -1398,21 +328,16 @@ static unsigned long zcache_failed_pers_puts;
  * of zcache would have one client per guest and each client might
  * have a poolid==N.
  */
-static struct tmem_pool *zcache_get_pool_by_id(uint16_t cli_id, uint16_t poolid)
+struct tmem_pool *zcache_get_pool_by_id(uint16_t cli_id, uint16_t poolid)
 {
 	struct tmem_pool *pool = NULL;
 	struct zcache_client *cli = NULL;
 
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else {
-		if (cli_id >= MAX_CLIENTS)
-			goto out;
-		cli = &zcache_clients[cli_id];
-		if (cli == NULL)
-			goto out;
+	cli = zcache_get_client_by_id(cli_id);
+	if (cli == NULL)
+		goto out;
+	if (!is_local_client(cli))
 		atomic_inc(&cli->refcount);
-	}
 	if (poolid < MAX_POOLS_PER_CLIENT) {
 		pool = cli->tmem_pools[poolid];
 		if (pool != NULL)
@@ -1422,7 +347,7 @@ out:
 	return pool;
 }
 
-static void zcache_put_pool(struct tmem_pool *pool)
+void zcache_put_pool(struct tmem_pool *pool)
 {
 	struct zcache_client *cli = NULL;
 
@@ -1430,173 +355,26 @@ static void zcache_put_pool(struct tmem_pool *pool)
 		BUG();
 	cli = pool->client;
 	atomic_dec(&pool->refcount);
-	atomic_dec(&cli->refcount);
+	if (!is_local_client(cli))
+		atomic_dec(&cli->refcount);
 }
 
 int zcache_new_client(uint16_t cli_id)
 {
-	struct zcache_client *cli = NULL;
+	struct zcache_client *cli;
 	int ret = -1;
 
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else if ((unsigned int)cli_id < MAX_CLIENTS)
-		cli = &zcache_clients[cli_id];
+	cli = zcache_get_client_by_id(cli_id);
 	if (cli == NULL)
 		goto out;
 	if (cli->allocated)
 		goto out;
 	cli->allocated = 1;
-#ifdef CONFIG_FRONTSWAP
-	cli->xvpool = xv_create_pool();
-	if (cli->xvpool == NULL)
-		goto out;
-#endif
-	ret = 0;
-out:
-	return ret;
-}
-
-/* counters for debugging */
-static unsigned long zcache_failed_get_free_pages;
-static unsigned long zcache_failed_alloc;
-static unsigned long zcache_put_to_flush;
-
-/*
- * for now, used named slabs so can easily track usage; later can
- * either just use kmalloc, or perhaps add a slab-like allocator
- * to more carefully manage total memory utilization
- */
-static struct kmem_cache *zcache_objnode_cache;
-static struct kmem_cache *zcache_obj_cache;
-static struct kmem_cache *ramster_flnode_cache;
-static atomic_t zcache_curr_obj_count = ATOMIC_INIT(0);
-static unsigned long zcache_curr_obj_count_max;
-static atomic_t zcache_curr_objnode_count = ATOMIC_INIT(0);
-static unsigned long zcache_curr_objnode_count_max;
-
-/*
- * to avoid memory allocation recursion (e.g. due to direct reclaim), we
- * preload all necessary data structures so the hostops callbacks never
- * actually do a malloc
- */
-struct zcache_preload {
-	void *page;
-	struct tmem_obj *obj;
-	int nr;
-	struct tmem_objnode *objnodes[OBJNODE_TREE_MAX_PATH];
-	struct flushlist_node *flnode;
-};
-static DEFINE_PER_CPU(struct zcache_preload, zcache_preloads) = { 0, };
-
-static int zcache_do_preload(struct tmem_pool *pool)
-{
-	struct zcache_preload *kp;
-	struct tmem_objnode *objnode;
-	struct tmem_obj *obj;
-	struct flushlist_node *flnode;
-	void *page;
-	int ret = -ENOMEM;
-
-	if (unlikely(zcache_objnode_cache == NULL))
-		goto out;
-	if (unlikely(zcache_obj_cache == NULL))
-		goto out;
-	preempt_disable();
-	kp = &__get_cpu_var(zcache_preloads);
-	while (kp->nr < ARRAY_SIZE(kp->objnodes)) {
-		preempt_enable_no_resched();
-		objnode = kmem_cache_alloc(zcache_objnode_cache,
-				ZCACHE_GFP_MASK);
-		if (unlikely(objnode == NULL)) {
-			zcache_failed_alloc++;
-			goto out;
-		}
-		preempt_disable();
-		kp = &__get_cpu_var(zcache_preloads);
-		if (kp->nr < ARRAY_SIZE(kp->objnodes))
-			kp->objnodes[kp->nr++] = objnode;
-		else
-			kmem_cache_free(zcache_objnode_cache, objnode);
-	}
-	preempt_enable_no_resched();
-	obj = kmem_cache_alloc(zcache_obj_cache, ZCACHE_GFP_MASK);
-	if (unlikely(obj == NULL)) {
-		zcache_failed_alloc++;
-		goto out;
-	}
-	flnode = kmem_cache_alloc(ramster_flnode_cache, ZCACHE_GFP_MASK);
-	if (unlikely(flnode == NULL)) {
-		zcache_failed_alloc++;
-		goto out;
-	}
-	if (is_ephemeral(pool)) {
-		page = (void *)__get_free_page(ZCACHE_GFP_MASK);
-		if (unlikely(page == NULL)) {
-			zcache_failed_get_free_pages++;
-			kmem_cache_free(zcache_obj_cache, obj);
-			kmem_cache_free(ramster_flnode_cache, flnode);
-			goto out;
-		}
-	}
-	preempt_disable();
-	kp = &__get_cpu_var(zcache_preloads);
-	if (kp->obj == NULL)
-		kp->obj = obj;
-	else
-		kmem_cache_free(zcache_obj_cache, obj);
-	if (kp->flnode == NULL)
-		kp->flnode = flnode;
-	else
-		kmem_cache_free(ramster_flnode_cache, flnode);
-	if (is_ephemeral(pool)) {
-		if (kp->page == NULL)
-			kp->page = page;
-		else
-			free_page((unsigned long)page);
-	}
 	ret = 0;
 out:
 	return ret;
 }
 
-static int ramster_do_preload_flnode_only(struct tmem_pool *pool)
-{
-	struct zcache_preload *kp;
-	struct flushlist_node *flnode;
-	int ret = -ENOMEM;
-
-	BUG_ON(!irqs_disabled());
-	if (unlikely(ramster_flnode_cache == NULL))
-		BUG();
-	kp = &__get_cpu_var(zcache_preloads);
-	flnode = kmem_cache_alloc(ramster_flnode_cache, GFP_ATOMIC);
-	if (unlikely(flnode == NULL) && kp->flnode == NULL)
-		BUG();  /* FIXME handle more gracefully, but how??? */
-	else if (kp->flnode == NULL)
-		kp->flnode = flnode;
-	else
-		kmem_cache_free(ramster_flnode_cache, flnode);
-	return ret;
-}
-
-static void *zcache_get_free_page(void)
-{
-	struct zcache_preload *kp;
-	void *page;
-
-	kp = &__get_cpu_var(zcache_preloads);
-	page = kp->page;
-	BUG_ON(page == NULL);
-	kp->page = NULL;
-	return page;
-}
-
-static void zcache_free_page(void *p)
-{
-	free_page((unsigned long)p);
-}
-
 /*
  * zcache implementation for tmem host ops
  */
@@ -1604,78 +382,56 @@ static void zcache_free_page(void *p)
 static struct tmem_objnode *zcache_objnode_alloc(struct tmem_pool *pool)
 {
 	struct tmem_objnode *objnode = NULL;
-	unsigned long count;
 	struct zcache_preload *kp;
+	int i;
 
 	kp = &__get_cpu_var(zcache_preloads);
-	if (kp->nr <= 0)
-		goto out;
-	objnode = kp->objnodes[kp->nr - 1];
+	for (i = 0; i < ARRAY_SIZE(kp->objnodes); i++) {
+		objnode = kp->objnodes[i];
+		if (objnode != NULL) {
+			kp->objnodes[i] = NULL;
+			break;
+		}
+	}
 	BUG_ON(objnode == NULL);
-	kp->objnodes[kp->nr - 1] = NULL;
-	kp->nr--;
-	count = atomic_inc_return(&zcache_curr_objnode_count);
-	if (count > zcache_curr_objnode_count_max)
-		zcache_curr_objnode_count_max = count;
-out:
+	zcache_objnode_count = atomic_inc_return(&zcache_objnode_atomic);
+	if (zcache_objnode_count > zcache_objnode_count_max)
+		zcache_objnode_count_max = zcache_objnode_count;
 	return objnode;
 }
 
 static void zcache_objnode_free(struct tmem_objnode *objnode,
 					struct tmem_pool *pool)
 {
-	atomic_dec(&zcache_curr_objnode_count);
-	BUG_ON(atomic_read(&zcache_curr_objnode_count) < 0);
+	zcache_objnode_count =
+		atomic_dec_return(&zcache_objnode_atomic);
+	BUG_ON(zcache_objnode_count < 0);
 	kmem_cache_free(zcache_objnode_cache, objnode);
 }
 
 static struct tmem_obj *zcache_obj_alloc(struct tmem_pool *pool)
 {
 	struct tmem_obj *obj = NULL;
-	unsigned long count;
 	struct zcache_preload *kp;
 
 	kp = &__get_cpu_var(zcache_preloads);
 	obj = kp->obj;
 	BUG_ON(obj == NULL);
 	kp->obj = NULL;
-	count = atomic_inc_return(&zcache_curr_obj_count);
-	if (count > zcache_curr_obj_count_max)
-		zcache_curr_obj_count_max = count;
+	zcache_obj_count = atomic_inc_return(&zcache_obj_atomic);
+	if (zcache_obj_count > zcache_obj_count_max)
+		zcache_obj_count_max = zcache_obj_count;
 	return obj;
 }
 
 static void zcache_obj_free(struct tmem_obj *obj, struct tmem_pool *pool)
 {
-	atomic_dec(&zcache_curr_obj_count);
-	BUG_ON(atomic_read(&zcache_curr_obj_count) < 0);
+	zcache_obj_count =
+		atomic_dec_return(&zcache_obj_atomic);
+	BUG_ON(zcache_obj_count < 0);
 	kmem_cache_free(zcache_obj_cache, obj);
 }
 
-static struct flushlist_node *ramster_flnode_alloc(struct tmem_pool *pool)
-{
-	struct flushlist_node *flnode = NULL;
-	struct zcache_preload *kp;
-	int count;
-
-	kp = &__get_cpu_var(zcache_preloads);
-	flnode = kp->flnode;
-	BUG_ON(flnode == NULL);
-	kp->flnode = NULL;
-	count = atomic_inc_return(&ramster_curr_flnode_count);
-	if (count > ramster_curr_flnode_count_max)
-		ramster_curr_flnode_count_max = count;
-	return flnode;
-}
-
-static void ramster_flnode_free(struct flushlist_node *flnode,
-				struct tmem_pool *pool)
-{
-	atomic_dec(&ramster_curr_flnode_count);
-	BUG_ON(atomic_read(&ramster_curr_flnode_count) < 0);
-	kmem_cache_free(ramster_flnode_cache, flnode);
-}
-
 static struct tmem_hostops zcache_hostops = {
 	.obj_alloc = zcache_obj_alloc,
 	.obj_free = zcache_obj_free,
@@ -1683,220 +439,363 @@ static struct tmem_hostops zcache_hostops = {
 	.objnode_free = zcache_objnode_free,
 };
 
-/*
- * zcache implementations for PAM page descriptor ops
- */
+static struct page *zcache_alloc_page(void)
+{
+	struct page *page = alloc_page(ZCACHE_GFP_MASK);
 
+	if (page != NULL)
+		zcache_pageframes_alloced =
+			atomic_inc_return(&zcache_pageframes_alloced_atomic);
+	return page;
+}
 
-static inline void dec_and_check(atomic_t *pvar)
+#ifdef FRONTSWAP_HAS_UNUSE
+static void zcache_unacct_page(void)
 {
-	atomic_dec(pvar);
-	/* later when all accounting is fixed, make this a BUG */
-	WARN_ON_ONCE(atomic_read(pvar) < 0);
+	zcache_pageframes_freed =
+		atomic_inc_return(&zcache_pageframes_freed_atomic);
 }
+#endif
+
+static void zcache_free_page(struct page *page)
+{
+	long curr_pageframes;
+	static long max_pageframes, min_pageframes;
 
-static atomic_t zcache_curr_eph_pampd_count = ATOMIC_INIT(0);
-static unsigned long zcache_curr_eph_pampd_count_max;
-static atomic_t zcache_curr_pers_pampd_count = ATOMIC_INIT(0);
-static unsigned long zcache_curr_pers_pampd_count_max;
+	if (page == NULL)
+		BUG();
+	__free_page(page);
+	zcache_pageframes_freed =
+		atomic_inc_return(&zcache_pageframes_freed_atomic);
+	curr_pageframes = zcache_pageframes_alloced -
+			atomic_read(&zcache_pageframes_freed_atomic) -
+			atomic_read(&zcache_eph_pageframes_atomic) -
+			atomic_read(&zcache_pers_pageframes_atomic);
+	if (curr_pageframes > max_pageframes)
+		max_pageframes = curr_pageframes;
+	if (curr_pageframes < min_pageframes)
+		min_pageframes = curr_pageframes;
+#ifdef ZCACHE_DEBUG
+	if (curr_pageframes > 2L || curr_pageframes < -2L) {
+		/* pr_info here */
+	}
+#endif
+}
+
+/*
+ * zcache implementations for PAM page descriptor ops
+ */
 
 /* forward reference */
-static int zcache_compress(struct page *from, void **out_va, size_t *out_len);
+static void zcache_compress(struct page *from,
+				void **out_va, unsigned *out_len);
 
-static int zcache_pampd_eph_create(char *data, size_t size, bool raw,
-				struct tmem_pool *pool, struct tmem_oid *oid,
-				uint32_t index, void **pampd)
+static struct page *zcache_evict_eph_pageframe(void);
+
+static void *zcache_pampd_eph_create(char *data, size_t size, bool raw,
+					struct tmem_handle *th)
 {
-	int ret = -1;
-	void *cdata = data;
-	size_t clen = size;
-	struct zcache_client *cli = pool->client;
-	uint16_t client_id = get_client_id_from_client(cli);
-	struct page *page = NULL;
-	unsigned long count;
+	void *pampd = NULL, *cdata = data;
+	unsigned clen = size;
+	struct page *page = (struct page *)(data), *newpage;
 
 	if (!raw) {
-		page = virt_to_page(data);
-		ret = zcache_compress(page, &cdata, &clen);
-		if (ret == 0)
-			goto out;
-		if (clen == 0 || clen > zbud_max_buddy_size()) {
+		zcache_compress(page, &cdata, &clen);
+		if (clen > zbud_max_buddy_size()) {
 			zcache_compress_poor++;
 			goto out;
 		}
+	} else {
+		BUG_ON(clen > zbud_max_buddy_size());
 	}
-	*pampd = (void *)zbud_create(client_id, pool->pool_id, oid,
-					index, page, cdata, clen);
-	if (*pampd == NULL) {
-		ret = -ENOMEM;
+
+	/* look for space via an existing match first */
+	pampd = (void *)zbud_match_prep(th, true, cdata, clen);
+	if (pampd != NULL)
+		goto got_pampd;
+
+	/* no match, now we need to find (or free up) a full page */
+	newpage = zcache_alloc_page();
+	if (newpage != NULL)
+		goto create_in_new_page;
+
+	zcache_failed_getfreepages++;
+	/* can't allocate a page, evict an ephemeral page via LRU */
+	newpage = zcache_evict_eph_pageframe();
+	if (newpage == NULL) {
+		zcache_eph_ate_tail_failed++;
 		goto out;
 	}
-	ret = 0;
-	count = atomic_inc_return(&zcache_curr_eph_pampd_count);
-	if (count > zcache_curr_eph_pampd_count_max)
-		zcache_curr_eph_pampd_count_max = count;
-	if (client_id != LOCAL_CLIENT) {
-		count = atomic_inc_return(&ramster_foreign_eph_pampd_count);
-		if (count > ramster_foreign_eph_pampd_count_max)
-			ramster_foreign_eph_pampd_count_max = count;
-	}
+	zcache_eph_ate_tail++;
+
+create_in_new_page:
+	pampd = (void *)zbud_create_prep(th, true, cdata, clen, newpage);
+	BUG_ON(pampd == NULL);
+	zcache_eph_pageframes =
+		atomic_inc_return(&zcache_eph_pageframes_atomic);
+	if (zcache_eph_pageframes > zcache_eph_pageframes_max)
+		zcache_eph_pageframes_max = zcache_eph_pageframes;
+
+got_pampd:
+	zcache_eph_zbytes =
+		atomic_long_add_return(clen, &zcache_eph_zbytes_atomic);
+	if (zcache_eph_zbytes > zcache_eph_zbytes_max)
+		zcache_eph_zbytes_max = zcache_eph_zbytes;
+	zcache_eph_zpages = atomic_inc_return(&zcache_eph_zpages_atomic);
+	if (zcache_eph_zpages > zcache_eph_zpages_max)
+		zcache_eph_zpages_max = zcache_eph_zpages;
+	if (ramster_enabled && raw)
+		ramster_count_foreign_pages(true, 1);
 out:
-	return ret;
+	return pampd;
 }
 
-static int zcache_pampd_pers_create(char *data, size_t size, bool raw,
-				struct tmem_pool *pool, struct tmem_oid *oid,
-				uint32_t index, void **pampd)
+static void *zcache_pampd_pers_create(char *data, size_t size, bool raw,
+					struct tmem_handle *th)
 {
-	int ret = -1;
-	void *cdata = data;
-	size_t clen = size;
-	struct zcache_client *cli = pool->client;
-	struct page *page;
-	unsigned long count;
-	unsigned long zv_mean_zsize;
-	struct zv_hdr *zv;
-	long curr_pers_pampd_count;
-	u64 total_zsize;
-#ifdef RAMSTER_TESTING
-	static bool pampd_neg_warned;
-#endif
+	void *pampd = NULL, *cdata = data;
+	unsigned clen = size;
+	struct page *page = (struct page *)(data), *newpage;
+	unsigned long zbud_mean_zsize;
+	unsigned long curr_pers_zpages, total_zsize;
 
-	curr_pers_pampd_count = atomic_read(&zcache_curr_pers_pampd_count) -
-			atomic_read(&ramster_remote_pers_pages);
-#ifdef RAMSTER_TESTING
-	/* should always be positive, but warn if accounting is off */
-	if (!pampd_neg_warned) {
-		pr_warn("ramster: bad accounting for curr_pers_pampd_count\n");
-		pampd_neg_warned = true;
+	if (data == NULL) {
+		BUG_ON(!ramster_enabled);
+		goto create_pampd;
 	}
-#endif
-	if (curr_pers_pampd_count >
-		    (zv_page_count_policy_percent * totalram_pages) / 100) {
-		zcache_policy_percent_exceeded++;
-		goto out;
-	}
-	if (raw)
-		goto ok_to_create;
-	page = virt_to_page(data);
-	if (zcache_compress(page, &cdata, &clen) == 0)
-		goto out;
+	curr_pers_zpages = zcache_pers_zpages;
+/* FIXME CONFIG_RAMSTER... subtract atomic remote_pers_pages here? */
+	if (!raw)
+		zcache_compress(page, &cdata, &clen);
 	/* reject if compression is too poor */
-	if (clen > zv_max_zsize) {
+	if (clen > zbud_max_zsize) {
 		zcache_compress_poor++;
 		goto out;
 	}
 	/* reject if mean compression is too poor */
-	if ((clen > zv_max_mean_zsize) && (curr_pers_pampd_count > 0)) {
-		total_zsize = xv_get_total_size_bytes(cli->xvpool);
-		zv_mean_zsize = div_u64(total_zsize, curr_pers_pampd_count);
-		if (zv_mean_zsize > zv_max_mean_zsize) {
+	if ((clen > zbud_max_mean_zsize) && (curr_pers_zpages > 0)) {
+		total_zsize = zcache_pers_zbytes;
+		if ((long)total_zsize < 0)
+			total_zsize = 0;
+		zbud_mean_zsize = div_u64(total_zsize,
+					curr_pers_zpages);
+		if (zbud_mean_zsize > zbud_max_mean_zsize) {
 			zcache_mean_compress_poor++;
 			goto out;
 		}
 	}
-ok_to_create:
-	*pampd = (void *)zv_create(cli, pool->pool_id, oid, index, cdata, clen);
-	if (*pampd == NULL) {
-		ret = -ENOMEM;
+
+create_pampd:
+	/* look for space via an existing match first */
+	pampd = (void *)zbud_match_prep(th, false, cdata, clen);
+	if (pampd != NULL)
+		goto got_pampd;
+
+	/* no match, now we need to find (or free up) a full page */
+	newpage = zcache_alloc_page();
+	if (newpage != NULL)
+		goto create_in_new_page;
+	/*
+	 * FIXME do the following only if eph is oversized?
+	 * if (zcache_eph_pageframes >
+	 * (global_page_state(NR_LRU_BASE + LRU_ACTIVE_FILE) +
+	 * global_page_state(NR_LRU_BASE + LRU_INACTIVE_FILE)))
+	 */
+	zcache_failed_getfreepages++;
+	/* can't allocate a page, evict an ephemeral page via LRU */
+	newpage = zcache_evict_eph_pageframe();
+	if (newpage == NULL) {
+		zcache_pers_ate_eph_failed++;
 		goto out;
 	}
-	ret = 0;
-	count = atomic_inc_return(&zcache_curr_pers_pampd_count);
-	if (count > zcache_curr_pers_pampd_count_max)
-		zcache_curr_pers_pampd_count_max = count;
-	if (is_local_client(cli))
-		goto out;
-	zv = *(struct zv_hdr **)pampd;
-	count = atomic_inc_return(&ramster_foreign_pers_pampd_count);
-	if (count > ramster_foreign_pers_pampd_count_max)
-		ramster_foreign_pers_pampd_count_max = count;
+	zcache_pers_ate_eph++;
+
+create_in_new_page:
+	pampd = (void *)zbud_create_prep(th, false, cdata, clen, newpage);
+	BUG_ON(pampd == NULL);
+	zcache_pers_pageframes =
+		atomic_inc_return(&zcache_pers_pageframes_atomic);
+	if (zcache_pers_pageframes > zcache_pers_pageframes_max)
+		zcache_pers_pageframes_max = zcache_pers_pageframes;
+
+got_pampd:
+	zcache_pers_zpages = atomic_inc_return(&zcache_pers_zpages_atomic);
+	if (zcache_pers_zpages > zcache_pers_zpages_max)
+		zcache_pers_zpages_max = zcache_pers_zpages;
+	zcache_pers_zbytes =
+		atomic_long_add_return(clen, &zcache_pers_zbytes_atomic);
+	if (zcache_pers_zbytes > zcache_pers_zbytes_max)
+		zcache_pers_zbytes_max = zcache_pers_zbytes;
+	if (ramster_enabled && raw)
+		ramster_count_foreign_pages(false, 1);
 out:
-	return ret;
+	return pampd;
 }
 
-static void *zcache_pampd_create(char *data, size_t size, bool raw, int eph,
-				struct tmem_pool *pool, struct tmem_oid *oid,
-				uint32_t index)
+/*
+ * This is called directly from zcache_put_page to pre-allocate space
+ * to store a zpage.
+ */
+void *zcache_pampd_create(char *data, unsigned int size, bool raw,
+					int eph, struct tmem_handle *th)
 {
 	void *pampd = NULL;
-	int ret;
-	bool ephemeral;
+	struct zcache_preload *kp;
+	struct tmem_objnode *objnode;
+	struct tmem_obj *obj;
+	int i;
 
-	BUG_ON(preemptible());
-	ephemeral = (eph == 1) || ((eph == 0) && is_ephemeral(pool));
-	if (ephemeral)
-		ret = zcache_pampd_eph_create(data, size, raw, pool,
-						oid, index, &pampd);
+	BUG_ON(!irqs_disabled());
+	/* pre-allocate per-cpu metadata */
+	BUG_ON(zcache_objnode_cache == NULL);
+	BUG_ON(zcache_obj_cache == NULL);
+	kp = &__get_cpu_var(zcache_preloads);
+	for (i = 0; i < ARRAY_SIZE(kp->objnodes); i++) {
+		objnode = kp->objnodes[i];
+		if (objnode == NULL) {
+			objnode = kmem_cache_alloc(zcache_objnode_cache,
+							ZCACHE_GFP_MASK);
+			if (unlikely(objnode == NULL)) {
+				zcache_failed_alloc++;
+				goto out;
+			}
+			kp->objnodes[i] = objnode;
+		}
+	}
+	if (kp->obj == NULL) {
+		obj = kmem_cache_alloc(zcache_obj_cache, ZCACHE_GFP_MASK);
+		kp->obj = obj;
+	}
+	if (unlikely(kp->obj == NULL)) {
+		zcache_failed_alloc++;
+		goto out;
+	}
+	/*
+	 * ok, have all the metadata pre-allocated, now do the data
+	 * but since how we allocate the data is dependent on ephemeral
+	 * or persistent, we split the call here to different sub-functions
+	 */
+	if (eph)
+		pampd = zcache_pampd_eph_create(data, size, raw, th);
 	else
-		ret = zcache_pampd_pers_create(data, size, raw, pool,
-						oid, index, &pampd);
-	/* FIXME add some counters here for failed creates? */
+		pampd = zcache_pampd_pers_create(data, size, raw, th);
+out:
 	return pampd;
 }
 
 /*
+ * This is a pamops called via tmem_put and is necessary to "finish"
+ * a pampd creation.
+ */
+void zcache_pampd_create_finish(void *pampd, bool eph)
+{
+	zbud_create_finish((struct zbudref *)pampd, eph);
+}
+
+/*
+ * This is passed as a function parameter to zbud_decompress so that
+ * zbud need not be familiar with the details of crypto. It assumes that
+ * the bytes from_va and to_va through from_va+size-1 and to_va+size-1 are
+ * kmapped.  It must be successful, else there is a logic bug somewhere.
+ */
+static void zcache_decompress(char *from_va, unsigned int size, char *to_va)
+{
+	int ret;
+	unsigned int outlen = PAGE_SIZE;
+
+	ret = zcache_comp_op(ZCACHE_COMPOP_DECOMPRESS, from_va, size,
+				to_va, &outlen);
+	BUG_ON(ret);
+	BUG_ON(outlen != PAGE_SIZE);
+}
+
+/*
+ * Decompress from the kernel va to a pageframe
+ */
+void zcache_decompress_to_page(char *from_va, unsigned int size,
+					struct page *to_page)
+{
+	char *to_va = kmap_atomic(to_page);
+	zcache_decompress(from_va, size, to_va);
+	kunmap_atomic(to_va);
+}
+
+/*
  * fill the pageframe corresponding to the struct page with the data
  * from the passed pampd
  */
-static int zcache_pampd_get_data(char *data, size_t *bufsize, bool raw,
+static int zcache_pampd_get_data(char *data, size_t *sizep, bool raw,
 					void *pampd, struct tmem_pool *pool,
 					struct tmem_oid *oid, uint32_t index)
 {
-	int ret = 0;
+	int ret;
+	bool eph = !is_persistent(pool);
 
 	BUG_ON(preemptible());
-	BUG_ON(is_ephemeral(pool)); /* Fix later for shared pools? */
+	BUG_ON(eph);	/* fix later if shared pools get implemented */
 	BUG_ON(pampd_is_remote(pampd));
 	if (raw)
-		zv_copy_from_pampd(data, bufsize, pampd);
-	else
-		zv_decompress(virt_to_page(data), pampd);
+		ret = zbud_copy_from_zbud(data, (struct zbudref *)pampd,
+						sizep, eph);
+	else {
+		ret = zbud_decompress((struct page *)(data),
+					(struct zbudref *)pampd, false,
+					zcache_decompress);
+		*sizep = PAGE_SIZE;
+	}
 	return ret;
 }
 
-static int zcache_pampd_get_data_and_free(char *data, size_t *bufsize, bool raw,
+/*
+ * fill the pageframe corresponding to the struct page with the data
+ * from the passed pampd
+ */
+static int zcache_pampd_get_data_and_free(char *data, size_t *sizep, bool raw,
 					void *pampd, struct tmem_pool *pool,
 					struct tmem_oid *oid, uint32_t index)
 {
-	int ret = 0;
-	unsigned long flags;
-	struct zcache_client *cli = pool->client;
+	int ret;
+	bool eph = !is_persistent(pool);
+	struct page *page = NULL;
+	unsigned int zsize, zpages;
 
 	BUG_ON(preemptible());
 	BUG_ON(pampd_is_remote(pampd));
-	if (is_ephemeral(pool)) {
-		local_irq_save(flags);
-		if (raw)
-			zbud_copy_from_pampd(data, bufsize, pampd);
-		else
-			ret = zbud_decompress(virt_to_page(data), pampd);
-		zbud_free_and_delist((struct zbud_hdr *)pampd);
-		local_irq_restore(flags);
-		if (!is_local_client(cli))
-			dec_and_check(&ramster_foreign_eph_pampd_count);
-		dec_and_check(&zcache_curr_eph_pampd_count);
+	if (raw)
+		ret = zbud_copy_from_zbud(data, (struct zbudref *)pampd,
+						sizep, eph);
+	else {
+		ret = zbud_decompress((struct page *)(data),
+					(struct zbudref *)pampd, eph,
+					zcache_decompress);
+		*sizep = PAGE_SIZE;
+	}
+	page = zbud_free_and_delist((struct zbudref *)pampd, eph,
+					&zsize, &zpages);
+	if (eph) {
+		if (page)
+			zcache_eph_pageframes =
+			    atomic_dec_return(&zcache_eph_pageframes_atomic);
+		zcache_eph_zpages =
+		    atomic_sub_return(zpages, &zcache_eph_zpages_atomic);
+		zcache_eph_zbytes =
+		    atomic_long_sub_return(zsize, &zcache_eph_zbytes_atomic);
 	} else {
-		if (is_local_client(cli))
-			BUG();
-		if (raw)
-			zv_copy_from_pampd(data, bufsize, pampd);
-		else
-			zv_decompress(virt_to_page(data), pampd);
-		zv_free(cli->xvpool, pampd);
-		if (!is_local_client(cli))
-			dec_and_check(&ramster_foreign_pers_pampd_count);
-		dec_and_check(&zcache_curr_pers_pampd_count);
-		ret = 0;
-	}
+		if (page)
+			zcache_pers_pageframes =
+			    atomic_dec_return(&zcache_pers_pageframes_atomic);
+		zcache_pers_zpages =
+		    atomic_sub_return(zpages, &zcache_pers_zpages_atomic);
+		zcache_pers_zbytes =
+		    atomic_long_sub_return(zsize, &zcache_pers_zbytes_atomic);
+	}
+	if (!is_local_client(pool->client))
+		ramster_count_foreign_pages(eph, -1);
+	if (page)
+		zcache_free_page(page);
 	return ret;
 }
 
-static bool zcache_pampd_is_remote(void *pampd)
-{
-	return pampd_is_remote(pampd);
-}
-
 /*
  * free the pampd and remove it from any zcache lists
  * pampd must no longer be pointed to from any tmem data structures!
@@ -1904,362 +803,134 @@ static bool zcache_pampd_is_remote(void *pampd)
 static void zcache_pampd_free(void *pampd, struct tmem_pool *pool,
 			      struct tmem_oid *oid, uint32_t index, bool acct)
 {
-	struct zcache_client *cli = pool->client;
-	bool eph = is_ephemeral(pool);
-	struct zv_hdr *zv;
+	struct page *page = NULL;
+	unsigned int zsize, zpages;
 
 	BUG_ON(preemptible());
 	if (pampd_is_remote(pampd)) {
-		WARN_ON(acct == false);
-		if (oid == NULL) {
-			/*
-			 * a NULL oid means to ignore this pampd free
-			 * as the remote freeing will be handled elsewhere
-			 */
-		} else if (eph) {
-			/* FIXME remote flush optional but probably good idea */
-			/* FIXME get these working properly again */
-			dec_and_check(&zcache_curr_eph_pampd_count);
-		} else if (pampd_is_intransit(pampd)) {
-			/* did a pers remote get_and_free, so just free local */
-			pampd = pampd_mask_intransit_and_remote(pampd);
-			goto local_pers;
-		} else {
-			struct flushlist_node *flnode =
-				ramster_flnode_alloc(pool);
-
-			flnode->xh.client_id = pampd_remote_node(pampd);
-			flnode->xh.pool_id = pool->pool_id;
-			flnode->xh.oid = *oid;
-			flnode->xh.index = index;
-			flnode->rem_op.op = RAMSTER_REMOTIFY_FLUSH_PAGE;
-			spin_lock(&zcache_rem_op_list_lock);
-			list_add(&flnode->rem_op.list, &zcache_rem_op_list);
-			spin_unlock(&zcache_rem_op_list_lock);
-			dec_and_check(&zcache_curr_pers_pampd_count);
-			dec_and_check(&ramster_remote_pers_pages);
-		}
-	} else if (eph) {
-		zbud_free_and_delist((struct zbud_hdr *)pampd);
-		if (!is_local_client(pool->client))
-			dec_and_check(&ramster_foreign_eph_pampd_count);
-		if (acct)
-			/* FIXME get these working properly again */
-			dec_and_check(&zcache_curr_eph_pampd_count);
-	} else {
-local_pers:
-		zv = (struct zv_hdr *)pampd;
-		if (!is_local_client(pool->client))
-			dec_and_check(&ramster_foreign_pers_pampd_count);
-		zv_free(cli->xvpool, zv);
-		if (acct)
-			/* FIXME get these working properly again */
-			dec_and_check(&zcache_curr_pers_pampd_count);
+		BUG_ON(!ramster_enabled);
+		pampd = ramster_pampd_free(pampd, pool, oid, index, acct);
+		if (pampd == NULL)
+			return;
 	}
-}
-
-static void zcache_pampd_free_obj(struct tmem_pool *pool,
-					struct tmem_obj *obj)
-{
-	struct flushlist_node *flnode;
-
-	BUG_ON(preemptible());
-	if (obj->extra == NULL)
-		return;
-	BUG_ON(!pampd_is_remote(obj->extra));
-	flnode = ramster_flnode_alloc(pool);
-	flnode->xh.client_id = pampd_remote_node(obj->extra);
-	flnode->xh.pool_id = pool->pool_id;
-	flnode->xh.oid = obj->oid;
-	flnode->xh.index = FLUSH_ENTIRE_OBJECT;
-	flnode->rem_op.op = RAMSTER_REMOTIFY_FLUSH_OBJ;
-	spin_lock(&zcache_rem_op_list_lock);
-	list_add(&flnode->rem_op.list, &zcache_rem_op_list);
-	spin_unlock(&zcache_rem_op_list_lock);
-}
-
-void zcache_pampd_new_obj(struct tmem_obj *obj)
-{
-	obj->extra = NULL;
-}
-
-int zcache_pampd_replace_in_obj(void *new_pampd, struct tmem_obj *obj)
-{
-	int ret = -1;
-
-	if (new_pampd != NULL) {
-		if (obj->extra == NULL)
-			obj->extra = new_pampd;
-		/* enforce that all remote pages in an object reside
-		 * in the same node! */
-		else if (pampd_remote_node(new_pampd) !=
-				pampd_remote_node((void *)(obj->extra)))
-			BUG();
-		ret = 0;
-	}
-	return ret;
-}
-
-/*
- * Called by the message handler after a (still compressed) page has been
- * fetched from the remote machine in response to an "is_remote" tmem_get
- * or persistent tmem_localify.  For a tmem_get, "extra" is the address of
- * the page that is to be filled to successfully resolve the tmem_get; for
- * a (persistent) tmem_localify, "extra" is NULL (as the data is placed only
- * in the local zcache).  "data" points to "size" bytes of (compressed) data
- * passed in the message.  In the case of a persistent remote get, if
- * pre-allocation was successful (see zcache_repatriate_preload), the page
- * is placed into both local zcache and at "extra".
- */
-int zcache_localify(int pool_id, struct tmem_oid *oidp,
-			uint32_t index, char *data, size_t size,
-			void *extra)
-{
-	int ret = -ENOENT;
-	unsigned long flags;
-	struct tmem_pool *pool;
-	bool ephemeral, delete = false;
-	size_t clen = PAGE_SIZE;
-	void *pampd, *saved_hb;
-	struct tmem_obj *obj;
-
-	pool = zcache_get_pool_by_id(LOCAL_CLIENT, pool_id);
-	if (unlikely(pool == NULL))
-		/* pool doesn't exist anymore */
-		goto out;
-	ephemeral = is_ephemeral(pool);
-	local_irq_save(flags);  /* FIXME: maybe only disable softirqs? */
-	pampd = tmem_localify_get_pampd(pool, oidp, index, &obj, &saved_hb);
-	if (pampd == NULL) {
-		/* hmmm... must have been a flush while waiting */
-#ifdef RAMSTER_TESTING
-		pr_err("UNTESTED pampd==NULL in zcache_localify\n");
-#endif
-		if (ephemeral)
-			ramster_remote_eph_pages_unsucc_get++;
-		else
-			ramster_remote_pers_pages_unsucc_get++;
-		obj = NULL;
-		goto finish;
-	} else if (unlikely(!pampd_is_remote(pampd))) {
-		/* hmmm... must have been a dup put while waiting */
-#ifdef RAMSTER_TESTING
-		pr_err("UNTESTED dup while waiting in zcache_localify\n");
-#endif
-		if (ephemeral)
-			ramster_remote_eph_pages_unsucc_get++;
-		else
-			ramster_remote_pers_pages_unsucc_get++;
-		obj = NULL;
-		pampd = NULL;
-		ret = -EEXIST;
-		goto finish;
-	} else if (size == 0) {
-		/* no remote data, delete the local is_remote pampd */
-		pampd = NULL;
-		if (ephemeral)
-			ramster_remote_eph_pages_unsucc_get++;
-		else
-			BUG();
-		delete = true;
-		goto finish;
-	}
-	if (!ephemeral && pampd_is_intransit(pampd)) {
-		/* localify to zcache */
-		pampd = pampd_mask_intransit_and_remote(pampd);
-		zv_copy_to_pampd(pampd, data, size);
+	if (is_ephemeral(pool)) {
+		page = zbud_free_and_delist((struct zbudref *)pampd,
+						true, &zsize, &zpages);
+		if (page)
+			zcache_eph_pageframes =
+			    atomic_dec_return(&zcache_eph_pageframes_atomic);
+		zcache_eph_zpages =
+		    atomic_sub_return(zpages, &zcache_eph_zpages_atomic);
+		zcache_eph_zbytes =
+		    atomic_long_sub_return(zsize, &zcache_eph_zbytes_atomic);
+		/* FIXME CONFIG_RAMSTER... check acct parameter? */
 	} else {
-		pampd = NULL;
-		obj = NULL;
-	}
-	if (extra != NULL) {
-		/* decompress direct-to-memory to complete remotify */
-		ret = lzo1x_decompress_safe((char *)data, size,
-						(char *)extra, &clen);
-		BUG_ON(ret != LZO_E_OK);
-		BUG_ON(clen != PAGE_SIZE);
-	}
-	if (ephemeral)
-		ramster_remote_eph_pages_succ_get++;
-	else
-		ramster_remote_pers_pages_succ_get++;
-	ret = 0;
-finish:
-	tmem_localify_finish(obj, index, pampd, saved_hb, delete);
-	zcache_put_pool(pool);
-	local_irq_restore(flags);
-out:
-	return ret;
-}
-
-/*
- * Called on a remote persistent tmem_get to attempt to preallocate
- * local storage for the data contained in the remote persistent page.
- * If successfully preallocated, returns the pampd, marked as remote and
- * in_transit.  Else returns NULL.  Note that the appropriate tmem data
- * structure must be locked.
- */
-static void *zcache_pampd_repatriate_preload(void *pampd,
-						struct tmem_pool *pool,
-						struct tmem_oid *oid,
-						uint32_t index,
-						bool *intransit)
-{
-	int clen = pampd_remote_size(pampd);
-	void *ret_pampd = NULL;
-	unsigned long flags;
-
-	if (!pampd_is_remote(pampd))
-		BUG();
-	if (is_ephemeral(pool))
-		BUG();
-	if (pampd_is_intransit(pampd)) {
-		/*
-		 * to avoid multiple allocations (and maybe a memory leak)
-		 * don't preallocate if already in the process of being
-		 * repatriated
-		 */
-		*intransit = true;
-		goto out;
-	}
-	*intransit = false;
-	local_irq_save(flags);
-	ret_pampd = (void *)zv_alloc(pool, oid, index, clen);
-	if (ret_pampd != NULL) {
-		/*
-		 *  a pampd is marked intransit if it is remote and space has
-		 *  been allocated for it locally (note, only happens for
-		 *  persistent pages, in which case the remote copy is freed)
-		 */
-		ret_pampd = pampd_mark_intransit(ret_pampd);
-		dec_and_check(&ramster_remote_pers_pages);
-	} else
-		ramster_pers_pages_remote_nomem++;
-	local_irq_restore(flags);
-out:
-	return ret_pampd;
-}
-
-/*
- * Called on a remote tmem_get to invoke a message to fetch the page.
- * Might sleep so no tmem locks can be held.  "extra" is passed
- * all the way through the round-trip messaging to zcache_localify.
- */
-static int zcache_pampd_repatriate(void *fake_pampd, void *real_pampd,
-				   struct tmem_pool *pool,
-				   struct tmem_oid *oid, uint32_t index,
-				   bool free, void *extra)
-{
-	struct tmem_xhandle xh;
-	int ret;
-
-	if (pampd_is_intransit(real_pampd))
-		/* have local space pre-reserved, so free remote copy */
-		free = true;
-	xh = tmem_xhandle_fill(LOCAL_CLIENT, pool, oid, index);
-	/* unreliable request/response for now */
-	ret = ramster_remote_async_get(&xh, free,
-					pampd_remote_node(fake_pampd),
-					pampd_remote_size(fake_pampd),
-					pampd_remote_cksum(fake_pampd),
-					extra);
-#ifdef RAMSTER_TESTING
-	if (ret != 0 && ret != -ENOENT)
-		pr_err("TESTING zcache_pampd_repatriate returns, ret=%d\n",
-			ret);
-#endif
-	return ret;
+		page = zbud_free_and_delist((struct zbudref *)pampd,
+						false, &zsize, &zpages);
+		if (page)
+			zcache_pers_pageframes =
+			    atomic_dec_return(&zcache_pers_pageframes_atomic);
+		zcache_pers_zpages =
+		     atomic_sub_return(zpages, &zcache_pers_zpages_atomic);
+		zcache_pers_zbytes =
+		    atomic_long_sub_return(zsize, &zcache_pers_zbytes_atomic);
+	}
+	if (!is_local_client(pool->client))
+		ramster_count_foreign_pages(is_ephemeral(pool), -1);
+	if (page)
+		zcache_free_page(page);
 }
 
 static struct tmem_pamops zcache_pamops = {
-	.create = zcache_pampd_create,
+	.create_finish = zcache_pampd_create_finish,
 	.get_data = zcache_pampd_get_data,
-	.free = zcache_pampd_free,
 	.get_data_and_free = zcache_pampd_get_data_and_free,
-	.free_obj = zcache_pampd_free_obj,
-	.is_remote = zcache_pampd_is_remote,
-	.repatriate_preload = zcache_pampd_repatriate_preload,
-	.repatriate = zcache_pampd_repatriate,
-	.new_obj = zcache_pampd_new_obj,
-	.replace_in_obj = zcache_pampd_replace_in_obj,
+	.free = zcache_pampd_free,
 };
 
 /*
  * zcache compression/decompression and related per-cpu stuff
  */
 
-#define LZO_WORKMEM_BYTES LZO1X_1_MEM_COMPRESS
-#define LZO_DSTMEM_PAGE_ORDER 1
-static DEFINE_PER_CPU(unsigned char *, zcache_workmem);
 static DEFINE_PER_CPU(unsigned char *, zcache_dstmem);
+#define ZCACHE_DSTMEM_ORDER 1
 
-static int zcache_compress(struct page *from, void **out_va, size_t *out_len)
+static void zcache_compress(struct page *from, void **out_va, unsigned *out_len)
 {
-	int ret = 0;
+	int ret;
 	unsigned char *dmem = __get_cpu_var(zcache_dstmem);
-	unsigned char *wmem = __get_cpu_var(zcache_workmem);
 	char *from_va;
 
 	BUG_ON(!irqs_disabled());
-	if (unlikely(dmem == NULL || wmem == NULL))
-		goto out;  /* no buffer, so can't compress */
+	/* no buffer or no compressor so can't compress */
+	BUG_ON(dmem == NULL);
+	*out_len = PAGE_SIZE << ZCACHE_DSTMEM_ORDER;
 	from_va = kmap_atomic(from);
 	mb();
-	ret = lzo1x_1_compress(from_va, PAGE_SIZE, dmem, out_len, wmem);
-	BUG_ON(ret != LZO_E_OK);
+	ret = zcache_comp_op(ZCACHE_COMPOP_COMPRESS, from_va, PAGE_SIZE, dmem,
+				out_len);
+	BUG_ON(ret);
 	*out_va = dmem;
 	kunmap_atomic(from_va);
-	ret = 1;
-out:
-	return ret;
 }
 
+static int zcache_comp_cpu_up(int cpu)
+{
+	struct crypto_comp *tfm;
+
+	tfm = crypto_alloc_comp(zcache_comp_name, 0, 0);
+	if (IS_ERR(tfm))
+		return NOTIFY_BAD;
+	*per_cpu_ptr(zcache_comp_pcpu_tfms, cpu) = tfm;
+	return NOTIFY_OK;
+}
+
+static void zcache_comp_cpu_down(int cpu)
+{
+	struct crypto_comp *tfm;
+
+	tfm = *per_cpu_ptr(zcache_comp_pcpu_tfms, cpu);
+	crypto_free_comp(tfm);
+	*per_cpu_ptr(zcache_comp_pcpu_tfms, cpu) = NULL;
+}
 
 static int zcache_cpu_notifier(struct notifier_block *nb,
 				unsigned long action, void *pcpu)
 {
-	int cpu = (long)pcpu;
+	int ret, i, cpu = (long)pcpu;
 	struct zcache_preload *kp;
 
 	switch (action) {
 	case CPU_UP_PREPARE:
+		ret = zcache_comp_cpu_up(cpu);
+		if (ret != NOTIFY_OK) {
+			pr_err("%s: can't allocate compressor xform\n",
+				namestr);
+			return ret;
+		}
 		per_cpu(zcache_dstmem, cpu) = (void *)__get_free_pages(
-			GFP_KERNEL | __GFP_REPEAT,
-			LZO_DSTMEM_PAGE_ORDER),
-		per_cpu(zcache_workmem, cpu) =
-			kzalloc(LZO1X_MEM_COMPRESS,
-				GFP_KERNEL | __GFP_REPEAT);
-		per_cpu(zcache_remoteputmem, cpu) =
-			kzalloc(PAGE_SIZE, GFP_KERNEL | __GFP_REPEAT);
+			GFP_KERNEL | __GFP_REPEAT, ZCACHE_DSTMEM_ORDER);
+		if (ramster_enabled)
+			ramster_cpu_up(cpu);
 		break;
 	case CPU_DEAD:
 	case CPU_UP_CANCELED:
-		kfree(per_cpu(zcache_remoteputmem, cpu));
-		per_cpu(zcache_remoteputmem, cpu) = NULL;
+		zcache_comp_cpu_down(cpu);
 		free_pages((unsigned long)per_cpu(zcache_dstmem, cpu),
-				LZO_DSTMEM_PAGE_ORDER);
+			ZCACHE_DSTMEM_ORDER);
 		per_cpu(zcache_dstmem, cpu) = NULL;
-		kfree(per_cpu(zcache_workmem, cpu));
-		per_cpu(zcache_workmem, cpu) = NULL;
 		kp = &per_cpu(zcache_preloads, cpu);
-		while (kp->nr) {
-			kmem_cache_free(zcache_objnode_cache,
-					kp->objnodes[kp->nr - 1]);
-			kp->objnodes[kp->nr - 1] = NULL;
-			kp->nr--;
+		for (i = 0; i < ARRAY_SIZE(kp->objnodes); i++) {
+			if (kp->objnodes[i])
+				kmem_cache_free(zcache_objnode_cache,
+						kp->objnodes[i]);
 		}
 		if (kp->obj) {
 			kmem_cache_free(zcache_obj_cache, kp->obj);
 			kp->obj = NULL;
 		}
-		if (kp->flnode) {
-			kmem_cache_free(ramster_flnode_cache, kp->flnode);
-			kp->flnode = NULL;
-		}
-		if (kp->page) {
-			free_page((unsigned long)kp->page);
-			kp->page = NULL;
-		}
+		if (ramster_enabled)
+			ramster_cpu_down(cpu);
 		break;
 	default:
 		break;
@@ -2271,314 +942,104 @@ static struct notifier_block zcache_cpu_notifier_block = {
 	.notifier_call = zcache_cpu_notifier
 };
 
-#ifdef CONFIG_SYSFS
-#define ZCACHE_SYSFS_RO(_name) \
-	static ssize_t zcache_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-		return sprintf(buf, "%lu\n", zcache_##_name); \
-	} \
-	static struct kobj_attribute zcache_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0444 }, \
-		.show = zcache_##_name##_show, \
-	}
-
-#define ZCACHE_SYSFS_RO_ATOMIC(_name) \
-	static ssize_t zcache_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-	    return sprintf(buf, "%d\n", atomic_read(&zcache_##_name)); \
-	} \
-	static struct kobj_attribute zcache_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0444 }, \
-		.show = zcache_##_name##_show, \
-	}
-
-#define ZCACHE_SYSFS_RO_CUSTOM(_name, _func) \
-	static ssize_t zcache_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-	    return _func(buf); \
-	} \
-	static struct kobj_attribute zcache_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0444 }, \
-		.show = zcache_##_name##_show, \
-	}
-
-ZCACHE_SYSFS_RO(curr_obj_count_max);
-ZCACHE_SYSFS_RO(curr_objnode_count_max);
-ZCACHE_SYSFS_RO(flush_total);
-ZCACHE_SYSFS_RO(flush_found);
-ZCACHE_SYSFS_RO(flobj_total);
-ZCACHE_SYSFS_RO(flobj_found);
-ZCACHE_SYSFS_RO(failed_eph_puts);
-ZCACHE_SYSFS_RO(nonactive_puts);
-ZCACHE_SYSFS_RO(failed_pers_puts);
-ZCACHE_SYSFS_RO(zbud_curr_zbytes);
-ZCACHE_SYSFS_RO(zbud_cumul_zpages);
-ZCACHE_SYSFS_RO(zbud_cumul_zbytes);
-ZCACHE_SYSFS_RO(zbud_buddied_count);
-ZCACHE_SYSFS_RO(evicted_raw_pages);
-ZCACHE_SYSFS_RO(evicted_unbuddied_pages);
-ZCACHE_SYSFS_RO(evicted_buddied_pages);
-ZCACHE_SYSFS_RO(failed_get_free_pages);
-ZCACHE_SYSFS_RO(failed_alloc);
-ZCACHE_SYSFS_RO(put_to_flush);
-ZCACHE_SYSFS_RO(compress_poor);
-ZCACHE_SYSFS_RO(mean_compress_poor);
-ZCACHE_SYSFS_RO(policy_percent_exceeded);
-ZCACHE_SYSFS_RO_ATOMIC(zbud_curr_raw_pages);
-ZCACHE_SYSFS_RO_ATOMIC(zbud_curr_zpages);
-ZCACHE_SYSFS_RO_ATOMIC(curr_obj_count);
-ZCACHE_SYSFS_RO_ATOMIC(curr_objnode_count);
-ZCACHE_SYSFS_RO_CUSTOM(zbud_unbuddied_list_counts,
-			zbud_show_unbuddied_list_counts);
-ZCACHE_SYSFS_RO_CUSTOM(zbud_cumul_chunk_counts,
-			zbud_show_cumul_chunk_counts);
-ZCACHE_SYSFS_RO_CUSTOM(zv_curr_dist_counts,
-			zv_curr_dist_counts_show);
-ZCACHE_SYSFS_RO_CUSTOM(zv_cumul_dist_counts,
-			zv_cumul_dist_counts_show);
-
-static struct attribute *zcache_attrs[] = {
-	&zcache_curr_obj_count_attr.attr,
-	&zcache_curr_obj_count_max_attr.attr,
-	&zcache_curr_objnode_count_attr.attr,
-	&zcache_curr_objnode_count_max_attr.attr,
-	&zcache_flush_total_attr.attr,
-	&zcache_flobj_total_attr.attr,
-	&zcache_flush_found_attr.attr,
-	&zcache_flobj_found_attr.attr,
-	&zcache_failed_eph_puts_attr.attr,
-	&zcache_nonactive_puts_attr.attr,
-	&zcache_failed_pers_puts_attr.attr,
-	&zcache_policy_percent_exceeded_attr.attr,
-	&zcache_compress_poor_attr.attr,
-	&zcache_mean_compress_poor_attr.attr,
-	&zcache_zbud_curr_raw_pages_attr.attr,
-	&zcache_zbud_curr_zpages_attr.attr,
-	&zcache_zbud_curr_zbytes_attr.attr,
-	&zcache_zbud_cumul_zpages_attr.attr,
-	&zcache_zbud_cumul_zbytes_attr.attr,
-	&zcache_zbud_buddied_count_attr.attr,
-	&zcache_evicted_raw_pages_attr.attr,
-	&zcache_evicted_unbuddied_pages_attr.attr,
-	&zcache_evicted_buddied_pages_attr.attr,
-	&zcache_failed_get_free_pages_attr.attr,
-	&zcache_failed_alloc_attr.attr,
-	&zcache_put_to_flush_attr.attr,
-	&zcache_zbud_unbuddied_list_counts_attr.attr,
-	&zcache_zbud_cumul_chunk_counts_attr.attr,
-	&zcache_zv_curr_dist_counts_attr.attr,
-	&zcache_zv_cumul_dist_counts_attr.attr,
-	&zcache_zv_max_zsize_attr.attr,
-	&zcache_zv_max_mean_zsize_attr.attr,
-	&zcache_zv_page_count_policy_percent_attr.attr,
-	NULL,
-};
-
-static struct attribute_group zcache_attr_group = {
-	.attrs = zcache_attrs,
-	.name = "zcache",
-};
-
-#define RAMSTER_SYSFS_RO(_name) \
-	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-		return sprintf(buf, "%lu\n", ramster_##_name); \
-	} \
-	static struct kobj_attribute ramster_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0444 }, \
-		.show = ramster_##_name##_show, \
-	}
-
-#define RAMSTER_SYSFS_RW(_name) \
-	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-		return sprintf(buf, "%lu\n", ramster_##_name); \
-	} \
-	static ssize_t ramster_##_name##_store(struct kobject *kobj, \
-		struct kobj_attribute *attr, const char *buf, size_t count) \
-	{ \
-		int err; \
-		unsigned long enable; \
-		err = kstrtoul(buf, 10, &enable); \
-		if (err) \
-			return -EINVAL; \
-		ramster_##_name = enable; \
-		return count; \
-	} \
-	static struct kobj_attribute ramster_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0644 }, \
-		.show = ramster_##_name##_show, \
-		.store = ramster_##_name##_store, \
-	}
-
-#define RAMSTER_SYSFS_RO_ATOMIC(_name) \
-	static ssize_t ramster_##_name##_show(struct kobject *kobj, \
-				struct kobj_attribute *attr, char *buf) \
-	{ \
-	    return sprintf(buf, "%d\n", atomic_read(&ramster_##_name)); \
-	} \
-	static struct kobj_attribute ramster_##_name##_attr = { \
-		.attr = { .name = __stringify(_name), .mode = 0444 }, \
-		.show = ramster_##_name##_show, \
-	}
-
-RAMSTER_SYSFS_RO(interface_revision);
-RAMSTER_SYSFS_RO_ATOMIC(remote_pers_pages);
-RAMSTER_SYSFS_RW(pers_remotify_enable);
-RAMSTER_SYSFS_RW(eph_remotify_enable);
-RAMSTER_SYSFS_RO(eph_pages_remoted);
-RAMSTER_SYSFS_RO(eph_pages_remote_failed);
-RAMSTER_SYSFS_RO(pers_pages_remoted);
-RAMSTER_SYSFS_RO(pers_pages_remote_failed);
-RAMSTER_SYSFS_RO(pers_pages_remote_nomem);
-RAMSTER_SYSFS_RO(remote_pages_flushed);
-RAMSTER_SYSFS_RO(remote_page_flushes_failed);
-RAMSTER_SYSFS_RO(remote_objects_flushed);
-RAMSTER_SYSFS_RO(remote_object_flushes_failed);
-RAMSTER_SYSFS_RO(remote_eph_pages_succ_get);
-RAMSTER_SYSFS_RO(remote_eph_pages_unsucc_get);
-RAMSTER_SYSFS_RO(remote_pers_pages_succ_get);
-RAMSTER_SYSFS_RO(remote_pers_pages_unsucc_get);
-RAMSTER_SYSFS_RO_ATOMIC(foreign_eph_pampd_count);
-RAMSTER_SYSFS_RO(foreign_eph_pampd_count_max);
-RAMSTER_SYSFS_RO_ATOMIC(foreign_pers_pampd_count);
-RAMSTER_SYSFS_RO(foreign_pers_pampd_count_max);
-RAMSTER_SYSFS_RO_ATOMIC(curr_flnode_count);
-RAMSTER_SYSFS_RO(curr_flnode_count_max);
-
-#define MANUAL_NODES 8
-static bool ramster_nodes_manual_up[MANUAL_NODES];
-static ssize_t ramster_manual_node_up_show(struct kobject *kobj,
-				struct kobj_attribute *attr, char *buf)
-{
-	int i;
-	char *p = buf;
-	for (i = 0; i < MANUAL_NODES; i++)
-		if (ramster_nodes_manual_up[i])
-			p += sprintf(p, "%d ", i);
-	p += sprintf(p, "\n");
-	return p - buf;
-}
+/*
+ * The following code interacts with the zbud eviction and zbud
+ * zombify code to access LRU pages
+ */
 
-static ssize_t ramster_manual_node_up_store(struct kobject *kobj,
-		struct kobj_attribute *attr, const char *buf, size_t count)
+static struct page *zcache_evict_eph_pageframe(void)
 {
-	int err;
-	unsigned long node_num;
+	struct page *page;
+	unsigned int zsize = 0, zpages = 0;
 
-	err = kstrtoul(buf, 10, &node_num);
-	if (err) {
-		pr_err("ramster: bad strtoul?\n");
-		return -EINVAL;
-	}
-	if (node_num >= MANUAL_NODES) {
-		pr_err("ramster: bad node_num=%lu?\n", node_num);
-		return -EINVAL;
-	}
-	if (ramster_nodes_manual_up[node_num]) {
-		pr_err("ramster: node %d already up, ignoring\n",
-							(int)node_num);
-	} else {
-		ramster_nodes_manual_up[node_num] = true;
-		r2net_hb_node_up_manual((int)node_num);
-	}
-	return count;
+	page = zbud_evict_pageframe_lru(&zsize, &zpages);
+	if (page == NULL)
+		goto out;
+	zcache_eph_zbytes = atomic_long_sub_return(zsize,
+					&zcache_eph_zbytes_atomic);
+	zcache_eph_zpages = atomic_sub_return(zpages,
+					&zcache_eph_zpages_atomic);
+	zcache_evicted_eph_zpages++;
+	zcache_eph_pageframes =
+		atomic_dec_return(&zcache_eph_pageframes_atomic);
+	zcache_evicted_eph_pageframes++;
+out:
+	return page;
 }
 
-static struct kobj_attribute ramster_manual_node_up_attr = {
-	.attr = { .name = "manual_node_up", .mode = 0644 },
-	.show = ramster_manual_node_up_show,
-	.store = ramster_manual_node_up_store,
-};
+#ifdef FRONTSWAP_HAS_UNUSE
+static void unswiz(struct tmem_oid oid, u32 index,
+				unsigned *type, pgoff_t *offset);
 
-static ssize_t ramster_remote_target_nodenum_show(struct kobject *kobj,
-				struct kobj_attribute *attr, char *buf)
+/*
+ *  Choose an LRU persistent pageframe and attempt to "unuse" it by
+ *  calling frontswap_unuse on both zpages.
+ *
+ *  This is work-in-progress.
+ */
+
+static int zcache_frontswap_unuse(void)
 {
-	if (ramster_remote_target_nodenum == -1UL)
-		return sprintf(buf, "unset\n");
-	else
-		return sprintf(buf, "%d\n", ramster_remote_target_nodenum);
-}
-
-static ssize_t ramster_remote_target_nodenum_store(struct kobject *kobj,
-		struct kobj_attribute *attr, const char *buf, size_t count)
-{
-	int err;
-	unsigned long node_num;
-
-	err = kstrtoul(buf, 10, &node_num);
-	if (err) {
-		pr_err("ramster: bad strtoul?\n");
-		return -EINVAL;
-	} else if (node_num == -1UL) {
-		pr_err("ramster: disabling all remotification, "
-			"data may still reside on remote nodes however\n");
-		return -EINVAL;
-	} else if (node_num >= MANUAL_NODES) {
-		pr_err("ramster: bad node_num=%lu?\n", node_num);
-		return -EINVAL;
-	} else if (!ramster_nodes_manual_up[node_num]) {
-		pr_err("ramster: node %d not up, ignoring setting "
-			"of remotification target\n", (int)node_num);
-	} else if (r2net_remote_target_node_set((int)node_num) >= 0) {
-		pr_info("ramster: node %d set as remotification target\n",
-				(int)node_num);
-		ramster_remote_target_nodenum = (int)node_num;
-	} else {
-		pr_err("ramster: bad num to node node_num=%d?\n",
-				(int)node_num);
-		return -EINVAL;
+	struct tmem_handle th[2];
+	int ret = -ENOMEM;
+	int nzbuds, unuse_ret;
+	unsigned type;
+	struct page *newpage1 = NULL, *newpage2 = NULL;
+	struct page *evictpage1 = NULL, *evictpage2 = NULL;
+	pgoff_t offset;
+
+	newpage1 = alloc_page(ZCACHE_GFP_MASK);
+	newpage2 = alloc_page(ZCACHE_GFP_MASK);
+	if (newpage1 == NULL)
+		evictpage1 = zcache_evict_eph_pageframe();
+	if (newpage2 == NULL)
+		evictpage2 = zcache_evict_eph_pageframe();
+	if (evictpage1 == NULL || evictpage2 == NULL)
+		goto free_and_out;
+	/* ok, we have two pages pre-allocated */
+	nzbuds = zbud_make_zombie_lru(&th[0], NULL, NULL, false);
+	if (nzbuds == 0) {
+		ret = -ENOENT;
+		goto free_and_out;
+	}
+	unswiz(th[0].oid, th[0].index, &type, &offset);
+	unuse_ret = frontswap_unuse(type, offset,
+				newpage1 != NULL ? newpage1 : evictpage1,
+				ZCACHE_GFP_MASK);
+	if (unuse_ret != 0)
+		goto free_and_out;
+	else if (evictpage1 != NULL)
+		zcache_unacct_page();
+	newpage1 = NULL;
+	evictpage1 = NULL;
+	if (nzbuds == 2) {
+		unswiz(th[1].oid, th[1].index, &type, &offset);
+		unuse_ret = frontswap_unuse(type, offset,
+				newpage2 != NULL ? newpage2 : evictpage2,
+				ZCACHE_GFP_MASK);
+		if (unuse_ret != 0) {
+			goto free_and_out;
+		} else if (evictpage2 != NULL) {
+			zcache_unacct_page();
+		}
 	}
-	return count;
+	ret = 0;
+	goto out;
+
+free_and_out:
+	if (newpage1 != NULL)
+		__free_page(newpage1);
+	if (newpage2 != NULL)
+		__free_page(newpage2);
+	if (evictpage1 != NULL)
+		zcache_free_page(evictpage1);
+	if (evictpage2 != NULL)
+		zcache_free_page(evictpage2);
+out:
+	return ret;
 }
+#endif
 
-static struct kobj_attribute ramster_remote_target_nodenum_attr = {
-	.attr = { .name = "remote_target_nodenum", .mode = 0644 },
-	.show = ramster_remote_target_nodenum_show,
-	.store = ramster_remote_target_nodenum_store,
-};
-
-
-static struct attribute *ramster_attrs[] = {
-	&ramster_interface_revision_attr.attr,
-	&ramster_pers_remotify_enable_attr.attr,
-	&ramster_eph_remotify_enable_attr.attr,
-	&ramster_remote_pers_pages_attr.attr,
-	&ramster_eph_pages_remoted_attr.attr,
-	&ramster_eph_pages_remote_failed_attr.attr,
-	&ramster_pers_pages_remoted_attr.attr,
-	&ramster_pers_pages_remote_failed_attr.attr,
-	&ramster_pers_pages_remote_nomem_attr.attr,
-	&ramster_remote_pages_flushed_attr.attr,
-	&ramster_remote_page_flushes_failed_attr.attr,
-	&ramster_remote_objects_flushed_attr.attr,
-	&ramster_remote_object_flushes_failed_attr.attr,
-	&ramster_remote_eph_pages_succ_get_attr.attr,
-	&ramster_remote_eph_pages_unsucc_get_attr.attr,
-	&ramster_remote_pers_pages_succ_get_attr.attr,
-	&ramster_remote_pers_pages_unsucc_get_attr.attr,
-	&ramster_foreign_eph_pampd_count_attr.attr,
-	&ramster_foreign_eph_pampd_count_max_attr.attr,
-	&ramster_foreign_pers_pampd_count_attr.attr,
-	&ramster_foreign_pers_pampd_count_max_attr.attr,
-	&ramster_curr_flnode_count_attr.attr,
-	&ramster_curr_flnode_count_max_attr.attr,
-	&ramster_manual_node_up_attr.attr,
-	&ramster_remote_target_nodenum_attr.attr,
-	NULL,
-};
-
-static struct attribute_group ramster_attr_group = {
-	.attrs = ramster_attrs,
-	.name = "ramster",
-};
-
-#endif /* CONFIG_SYSFS */
 /*
  * When zcache is disabled ("frozen"), pools can be created and destroyed,
  * but all puts (and thus all other operations that require memory allocation)
@@ -2589,23 +1050,74 @@ static struct attribute_group ramster_attr_group = {
 static bool zcache_freeze;
 
 /*
- * zcache shrinker interface (only useful for ephemeral pages, so zbud only)
+ * This zcache shrinker interface reduces the number of ephemeral pageframes
+ * used by zcache to approximately the same as the total number of LRU_FILE
+ * pageframes in use.
  */
 static int shrink_zcache_memory(struct shrinker *shrink,
 				struct shrink_control *sc)
 {
+	static bool in_progress;
 	int ret = -1;
 	int nr = sc->nr_to_scan;
-	gfp_t gfp_mask = sc->gfp_mask;
+	int nr_evict = 0;
+	int nr_unuse = 0;
+	struct page *page;
+#ifdef FRONTSWAP_HAS_UNUSE
+	int unuse_ret;
+#endif
 
-	if (nr >= 0) {
-		if (!(gfp_mask & __GFP_FS))
-			/* does this case really need to be skipped? */
-			goto out;
-		zbud_evict_pages(nr);
+	if (nr <= 0)
+		goto skip_evict;
+
+	/* don't allow more than one eviction thread at a time */
+	if (in_progress)
+		goto skip_evict;
+
+	in_progress = true;
+
+	/* we are going to ignore nr, and target a different value */
+	zcache_last_active_file_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_ACTIVE_FILE);
+	zcache_last_inactive_file_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_INACTIVE_FILE);
+	nr_evict = zcache_eph_pageframes - zcache_last_active_file_pageframes +
+		zcache_last_inactive_file_pageframes;
+	while (nr_evict-- > 0) {
+		page = zcache_evict_eph_pageframe();
+		if (page == NULL)
+			break;
+		zcache_free_page(page);
+	}
+
+	zcache_last_active_anon_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_ACTIVE_ANON);
+	zcache_last_inactive_anon_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_INACTIVE_ANON);
+	nr_unuse = zcache_pers_pageframes - zcache_last_active_anon_pageframes +
+		zcache_last_inactive_anon_pageframes;
+#ifdef FRONTSWAP_HAS_UNUSE
+	/* rate limit for testing */
+	if (nr_unuse > 32)
+		nr_unuse = 32;
+	while (nr_unuse-- > 0) {
+		unuse_ret = zcache_frontswap_unuse();
+		if (unuse_ret == -ENOMEM)
+			break;
 	}
-	ret = (int)atomic_read(&zcache_zbud_curr_raw_pages);
-out:
+#endif
+	in_progress = false;
+
+skip_evict:
+	/* resample: has changed, but maybe not all the way yet */
+	zcache_last_active_file_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_ACTIVE_FILE);
+	zcache_last_inactive_file_pageframes =
+		global_page_state(NR_LRU_BASE + LRU_INACTIVE_FILE);
+	ret = zcache_eph_pageframes - zcache_last_active_file_pageframes +
+		zcache_last_inactive_file_pageframes;
+	if (ret < 0)
+		ret = 0;
 	return ret;
 }
 
@@ -2618,30 +1130,46 @@ static struct shrinker zcache_shrinker = {
  * zcache shims between cleancache/frontswap ops and tmem
  */
 
-int zcache_put(int cli_id, int pool_id, struct tmem_oid *oidp,
-			uint32_t index, char *data, size_t size,
-			bool raw, int ephemeral)
+/* FIXME rename these core routines to zcache_tmemput etc? */
+int zcache_put_page(int cli_id, int pool_id, struct tmem_oid *oidp,
+				uint32_t index, void *page,
+				unsigned int size, bool raw, int ephemeral)
 {
 	struct tmem_pool *pool;
+	struct tmem_handle th;
 	int ret = -1;
+	void *pampd = NULL;
 
 	BUG_ON(!irqs_disabled());
 	pool = zcache_get_pool_by_id(cli_id, pool_id);
 	if (unlikely(pool == NULL))
 		goto out;
-	if (!zcache_freeze && zcache_do_preload(pool) == 0) {
-		/* preload does preempt_disable on success */
-		ret = tmem_put(pool, oidp, index, data, size, raw, ephemeral);
-		if (ret < 0) {
-			if (is_ephemeral(pool))
+	if (!zcache_freeze) {
+		ret = 0;
+		th.client_id = cli_id;
+		th.pool_id = pool_id;
+		th.oid = *oidp;
+		th.index = index;
+		pampd = zcache_pampd_create((char *)page, size, raw,
+				ephemeral, &th);
+		if (pampd == NULL) {
+			ret = -ENOMEM;
+			if (ephemeral)
 				zcache_failed_eph_puts++;
 			else
 				zcache_failed_pers_puts++;
+		} else {
+			if (ramster_enabled)
+				ramster_do_preload_flnode(pool);
+			ret = tmem_put(pool, oidp, index, 0, pampd);
+			if (ret < 0)
+				BUG();
 		}
 		zcache_put_pool(pool);
-		preempt_enable_no_resched();
 	} else {
 		zcache_put_to_flush++;
+		if (ramster_enabled)
+			ramster_do_preload_flnode(pool);
 		if (atomic_read(&pool->obj_count) > 0)
 			/* the put fails whether the flush succeeds or not */
 			(void)tmem_flush_page(pool, oidp, index);
@@ -2651,9 +1179,9 @@ out:
 	return ret;
 }
 
-int zcache_get(int cli_id, int pool_id, struct tmem_oid *oidp,
-			uint32_t index, char *data, size_t *sizep,
-			bool raw, int get_and_free)
+int zcache_get_page(int cli_id, int pool_id, struct tmem_oid *oidp,
+				uint32_t index, void *page,
+				size_t *sizep, bool raw, int get_and_free)
 {
 	struct tmem_pool *pool;
 	int ret = -1;
@@ -2667,22 +1195,21 @@ int zcache_get(int cli_id, int pool_id, struct tmem_oid *oidp,
 	eph = is_ephemeral(pool);
 	if (likely(pool != NULL)) {
 		if (atomic_read(&pool->obj_count) > 0)
-			ret = tmem_get(pool, oidp, index, data, sizep,
-					raw, get_and_free);
+			ret = tmem_get(pool, oidp, index, (char *)(page),
+					sizep, raw, get_and_free);
 		zcache_put_pool(pool);
 	}
-	WARN_ONCE((!eph && (ret != 0)), "zcache_get fails on persistent pool, "
-			  "bad things are very likely to happen soon\n");
+	WARN_ONCE((!is_ephemeral(pool) && (ret != 0)),
+			"zcache_get fails on persistent pool, "
+			"bad things are very likely to happen soon\n");
 #ifdef RAMSTER_TESTING
 	if (ret != 0 && ret != -1 && !(ret == -EINVAL && is_ephemeral(pool)))
 		pr_err("TESTING zcache_get tmem_get returns ret=%d\n", ret);
 #endif
-	if (ret == -EAGAIN)
-		BUG(); /* FIXME... don't need this anymore??? let's ensure */
 	return ret;
 }
 
-int zcache_flush(int cli_id, int pool_id,
+int zcache_flush_page(int cli_id, int pool_id,
 				struct tmem_oid *oidp, uint32_t index)
 {
 	struct tmem_pool *pool;
@@ -2692,7 +1219,8 @@ int zcache_flush(int cli_id, int pool_id,
 	local_irq_save(flags);
 	zcache_flush_total++;
 	pool = zcache_get_pool_by_id(cli_id, pool_id);
-	ramster_do_preload_flnode_only(pool);
+	if (ramster_enabled)
+		ramster_do_preload_flnode(pool);
 	if (likely(pool != NULL)) {
 		if (atomic_read(&pool->obj_count) > 0)
 			ret = tmem_flush_page(pool, oidp, index);
@@ -2704,7 +1232,8 @@ int zcache_flush(int cli_id, int pool_id,
 	return ret;
 }
 
-int zcache_flush_object(int cli_id, int pool_id, struct tmem_oid *oidp)
+int zcache_flush_object(int cli_id, int pool_id,
+				struct tmem_oid *oidp)
 {
 	struct tmem_pool *pool;
 	int ret = -1;
@@ -2713,7 +1242,8 @@ int zcache_flush_object(int cli_id, int pool_id, struct tmem_oid *oidp)
 	local_irq_save(flags);
 	zcache_flobj_total++;
 	pool = zcache_get_pool_by_id(cli_id, pool_id);
-	ramster_do_preload_flnode_only(pool);
+	if (ramster_enabled)
+		ramster_do_preload_flnode(pool);
 	if (likely(pool != NULL)) {
 		if (atomic_read(&pool->obj_count) > 0)
 			ret = tmem_flush_object(pool, oidp);
@@ -2725,7 +1255,7 @@ int zcache_flush_object(int cli_id, int pool_id, struct tmem_oid *oidp)
 	return ret;
 }
 
-int zcache_client_destroy_pool(int cli_id, int pool_id)
+static int zcache_client_destroy_pool(int cli_id, int pool_id)
 {
 	struct tmem_pool *pool = NULL;
 	struct zcache_client *cli = NULL;
@@ -2752,16 +1282,15 @@ int zcache_client_destroy_pool(int cli_id, int pool_id)
 	ret = tmem_destroy_pool(pool);
 	local_bh_enable();
 	kfree(pool);
-	pr_info("ramster: destroyed pool id=%d cli_id=%d\n", pool_id, cli_id);
+	if (cli_id == LOCAL_CLIENT)
+		pr_info("%s: destroyed local pool id=%d\n", namestr, pool_id);
+	else
+		pr_info("%s: destroyed pool id=%d, client=%d\n",
+				namestr, pool_id, cli_id);
 out:
 	return ret;
 }
 
-static int zcache_destroy_pool(int pool_id)
-{
-	return zcache_client_destroy_pool(LOCAL_CLIENT, pool_id);
-}
-
 int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 {
 	int poolid = -1;
@@ -2777,7 +1306,7 @@ int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 	atomic_inc(&cli->refcount);
 	pool = kmalloc(sizeof(struct tmem_pool), GFP_ATOMIC);
 	if (pool == NULL) {
-		pr_info("ramster: pool creation failed: out of memory\n");
+		pr_info("%s: pool creation failed: out of memory\n", namestr);
 		goto out;
 	}
 
@@ -2785,7 +1314,7 @@ int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 		if (cli->tmem_pools[poolid] == NULL)
 			break;
 	if (poolid >= MAX_POOLS_PER_CLIENT) {
-		pr_info("ramster: pool creation failed: max exceeded\n");
+		pr_info("%s: pool creation failed: max exceeded\n", namestr);
 		kfree(pool);
 		poolid = -1;
 		goto out;
@@ -2796,11 +1325,11 @@ int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 	tmem_new_pool(pool, flags);
 	cli->tmem_pools[poolid] = pool;
 	if (cli_id == LOCAL_CLIENT)
-		pr_info("ramster: created %s tmem pool, id=%d, local client\n",
+		pr_info("%s: created %s local tmem pool, id=%d\n", namestr,
 			flags & TMEM_POOL_PERSIST ? "persistent" : "ephemeral",
 			poolid);
 	else
-		pr_info("ramster: created %s tmem pool, id=%d, client=%d\n",
+		pr_info("%s: created %s tmem pool, id=%d, client=%d\n", namestr,
 			flags & TMEM_POOL_PERSIST ? "persistent" : "ephemeral",
 			poolid, cli_id);
 out:
@@ -2814,30 +1343,37 @@ static int zcache_local_new_pool(uint32_t flags)
 	return zcache_new_pool(LOCAL_CLIENT, flags);
 }
 
-int zcache_autocreate_pool(int cli_id, int pool_id, bool ephemeral)
+int zcache_autocreate_pool(unsigned int cli_id, unsigned int pool_id, bool eph)
 {
 	struct tmem_pool *pool;
-	struct zcache_client *cli = NULL;
-	uint32_t flags = ephemeral ? 0 : TMEM_POOL_PERSIST;
+	struct zcache_client *cli;
+	uint32_t flags = eph ? 0 : TMEM_POOL_PERSIST;
 	int ret = -1;
 
+	BUG_ON(!ramster_enabled);
 	if (cli_id == LOCAL_CLIENT)
 		goto out;
 	if (pool_id >= MAX_POOLS_PER_CLIENT)
 		goto out;
-	else if ((unsigned int)cli_id < MAX_CLIENTS)
-		cli = &zcache_clients[cli_id];
-	if ((ephemeral && !use_cleancache) || (!ephemeral && !use_frontswap))
-		BUG(); /* FIXME, handle more gracefully later */
+	if (cli_id >= MAX_CLIENTS)
+		goto out;
+
+	cli = &zcache_clients[cli_id];
+	if ((eph && disable_cleancache) || (!eph && disable_frontswap)) {
+		pr_err("zcache_autocreate_pool: pool type disabled\n");
+		goto out;
+	}
 	if (!cli->allocated) {
-		if (zcache_new_client(cli_id))
-			BUG(); /* FIXME, handle more gracefully later */
+		if (zcache_new_client(cli_id)) {
+			pr_err("zcache_autocreate_pool: can't create client\n");
+			goto out;
+		}
 		cli = &zcache_clients[cli_id];
 	}
 	atomic_inc(&cli->refcount);
 	pool = cli->tmem_pools[pool_id];
 	if (pool != NULL) {
-		if (pool->persistent && ephemeral) {
+		if (pool->persistent && eph) {
 			pr_err("zcache_autocreate_pool: type mismatch\n");
 			goto out;
 		}
@@ -2846,7 +1382,7 @@ int zcache_autocreate_pool(int cli_id, int pool_id, bool ephemeral)
 	}
 	pool = kmalloc(sizeof(struct tmem_pool), GFP_KERNEL);
 	if (pool == NULL) {
-		pr_info("ramster: pool creation failed: out of memory\n");
+		pr_info("%s: pool creation failed: out of memory\n", namestr);
 		goto out;
 	}
 	atomic_set(&pool->refcount, 0);
@@ -2854,14 +1390,11 @@ int zcache_autocreate_pool(int cli_id, int pool_id, bool ephemeral)
 	pool->pool_id = pool_id;
 	tmem_new_pool(pool, flags);
 	cli->tmem_pools[pool_id] = pool;
-	pr_info("ramster: AUTOcreated %s tmem poolid=%d, for remote client=%d\n",
-		flags & TMEM_POOL_PERSIST ? "persistent" : "ephemeral",
+	pr_info("%s: AUTOcreated %s tmem poolid=%d, for remote client=%d\n",
+		namestr, flags & TMEM_POOL_PERSIST ? "persistent" : "ephemeral",
 		pool_id, cli_id);
 	ret = 0;
 out:
-	if (cli == NULL)
-		BUG(); /* FIXME, handle more gracefully later */
-		/* pr_err("zcache_autocreate_pool: failed\n"); */
 	if (cli != NULL)
 		atomic_dec(&cli->refcount);
 	return ret;
@@ -2875,7 +1408,6 @@ out:
  * to translate in-kernel semantics to zcache semantics.
  */
 
-#ifdef CONFIG_CLEANCACHE
 static void zcache_cleancache_put_page(int pool_id,
 					struct cleancache_filekey key,
 					pgoff_t index, struct page *page)
@@ -2883,18 +1415,13 @@ static void zcache_cleancache_put_page(int pool_id,
 	u32 ind = (u32) index;
 	struct tmem_oid oid = *(struct tmem_oid *)&key;
 
-#ifdef __PG_WAS_ACTIVE
-	if (!PageWasActive(page)) {
-		zcache_nonactive_puts++;
+	if (!disable_cleancache_ignore_nonactive && !PageWasActive(page)) {
+		zcache_eph_nonactive_puts_ignored++;
 		return;
 	}
-#endif
-	if (likely(ind == index)) {
-		char *kva = page_address(page);
-
-		(void)zcache_put(LOCAL_CLIENT, pool_id, &oid, index,
-			kva, PAGE_SIZE, 0, 1);
-	}
+	if (likely(ind == index))
+		(void)zcache_put_page(LOCAL_CLIENT, pool_id, &oid, index,
+					page, PAGE_SIZE, false, 1);
 }
 
 static int zcache_cleancache_get_page(int pool_id,
@@ -2903,21 +1430,16 @@ static int zcache_cleancache_get_page(int pool_id,
 {
 	u32 ind = (u32) index;
 	struct tmem_oid oid = *(struct tmem_oid *)&key;
+	size_t size;
 	int ret = -1;
 
-	preempt_disable();
 	if (likely(ind == index)) {
-		char *kva = page_address(page);
-		size_t size = PAGE_SIZE;
-
-		ret = zcache_get(LOCAL_CLIENT, pool_id, &oid, index,
-			kva, &size, 0, 0);
-#ifdef __PG_WAS_ACTIVE
+		ret = zcache_get_page(LOCAL_CLIENT, pool_id, &oid, index,
+					page, &size, false, 0);
+		BUG_ON(ret >= 0 && size != PAGE_SIZE);
 		if (ret == 0)
 			SetPageWasActive(page);
-#endif
 	}
-	preempt_enable();
 	return ret;
 }
 
@@ -2929,7 +1451,7 @@ static void zcache_cleancache_flush_page(int pool_id,
 	struct tmem_oid oid = *(struct tmem_oid *)&key;
 
 	if (likely(ind == index))
-		(void)zcache_flush(LOCAL_CLIENT, pool_id, &oid, ind);
+		(void)zcache_flush_page(LOCAL_CLIENT, pool_id, &oid, ind);
 }
 
 static void zcache_cleancache_flush_inode(int pool_id,
@@ -2943,7 +1465,7 @@ static void zcache_cleancache_flush_inode(int pool_id,
 static void zcache_cleancache_flush_fs(int pool_id)
 {
 	if (pool_id >= 0)
-		(void)zcache_destroy_pool(pool_id);
+		(void)zcache_client_destroy_pool(LOCAL_CLIENT, pool_id);
 }
 
 static int zcache_cleancache_init_fs(size_t pagesize)
@@ -2980,15 +1502,15 @@ struct cleancache_ops zcache_cleancache_register_ops(void)
 
 	return old_ops;
 }
-#endif
 
-#ifdef CONFIG_FRONTSWAP
 /* a single tmem poolid is used for all frontswap "types" (swapfiles) */
-static int zcache_frontswap_poolid = -1;
+static int zcache_frontswap_poolid __read_mostly = -1;
 
 /*
  * Swizzling increases objects per swaptype, increasing tmem concurrency
  * for heavy swaploads.  Later, larger nr_cpus -> larger SWIZ_BITS
+ * Setting SWIZ_BITS to 27 basically reconstructs the swap entry from
+ * frontswap_get_page(), but has side-effects. Hence using 8.
  */
 #define SWIZ_BITS		8
 #define SWIZ_MASK		((1 << SWIZ_BITS) - 1)
@@ -3002,47 +1524,64 @@ static inline struct tmem_oid oswiz(unsigned type, u32 ind)
 	return oid;
 }
 
-static int zcache_frontswap_store(unsigned type, pgoff_t offset,
-				   struct page *page)
+#ifdef FRONTSWAP_HAS_UNUSE
+static void unswiz(struct tmem_oid oid, u32 index,
+				unsigned *type, pgoff_t *offset)
+{
+	*type = (unsigned)(oid.oid[0] >> SWIZ_BITS);
+	*offset = (pgoff_t)((index << SWIZ_BITS) |
+			(oid.oid[0] & SWIZ_MASK));
+}
+#endif
+
+static int zcache_frontswap_put_page(unsigned type, pgoff_t offset,
+					struct page *page)
 {
 	u64 ind64 = (u64)offset;
 	u32 ind = (u32)offset;
 	struct tmem_oid oid = oswiz(type, ind);
 	int ret = -1;
 	unsigned long flags;
-	char *kva;
 
 	BUG_ON(!PageLocked(page));
+	if (!disable_frontswap_ignore_nonactive && !PageWasActive(page)) {
+		zcache_pers_nonactive_puts_ignored++;
+		ret = -ERANGE;
+		goto out;
+	}
 	if (likely(ind64 == ind)) {
 		local_irq_save(flags);
-		kva = page_address(page);
-		ret = zcache_put(LOCAL_CLIENT, zcache_frontswap_poolid,
-				&oid, iswiz(ind), kva, PAGE_SIZE, 0, 0);
+		ret = zcache_put_page(LOCAL_CLIENT, zcache_frontswap_poolid,
+					&oid, iswiz(ind),
+					page, PAGE_SIZE, false, 0);
 		local_irq_restore(flags);
 	}
+out:
 	return ret;
 }
 
 /* returns 0 if the page was successfully gotten from frontswap, -1 if
  * was not present (should never happen!) */
-static int zcache_frontswap_load(unsigned type, pgoff_t offset,
-				   struct page *page)
+static int zcache_frontswap_get_page(unsigned type, pgoff_t offset,
+					struct page *page)
 {
 	u64 ind64 = (u64)offset;
 	u32 ind = (u32)offset;
 	struct tmem_oid oid = oswiz(type, ind);
-	int ret = -1;
+	size_t size;
+	int ret = -1, get_and_free;
 
-	preempt_disable(); /* FIXME, remove this? */
+	if (frontswap_has_exclusive_gets)
+		get_and_free = 1;
+	else
+		get_and_free = -1;
 	BUG_ON(!PageLocked(page));
 	if (likely(ind64 == ind)) {
-		char *kva = page_address(page);
-		size_t size = PAGE_SIZE;
-
-		ret = zcache_get(LOCAL_CLIENT, zcache_frontswap_poolid,
-					&oid, iswiz(ind), kva, &size, 0, -1);
+		ret = zcache_get_page(LOCAL_CLIENT, zcache_frontswap_poolid,
+					&oid, iswiz(ind),
+					page, &size, false, get_and_free);
+		BUG_ON(ret >= 0 && size != PAGE_SIZE);
 	}
-	preempt_enable(); /* FIXME, remove this? */
 	return ret;
 }
 
@@ -3054,7 +1593,7 @@ static void zcache_frontswap_flush_page(unsigned type, pgoff_t offset)
 	struct tmem_oid oid = oswiz(type, ind);
 
 	if (likely(ind64 == ind))
-		(void)zcache_flush(LOCAL_CLIENT, zcache_frontswap_poolid,
+		(void)zcache_flush_page(LOCAL_CLIENT, zcache_frontswap_poolid,
 					&oid, iswiz(ind));
 }
 
@@ -3076,12 +1615,12 @@ static void zcache_frontswap_init(unsigned ignored)
 	/* a single tmem poolid is used for all frontswap "types" (swapfiles) */
 	if (zcache_frontswap_poolid < 0)
 		zcache_frontswap_poolid =
-				zcache_local_new_pool(TMEM_POOL_PERSIST);
+			zcache_local_new_pool(TMEM_POOL_PERSIST);
 }
 
 static struct frontswap_ops zcache_frontswap_ops = {
-	.store = zcache_frontswap_store,
-	.load = zcache_frontswap_load,
+	.store = zcache_frontswap_put_page,
+	.load = zcache_frontswap_get_page,
 	.invalidate_page = zcache_frontswap_flush_page,
 	.invalidate_area = zcache_frontswap_flush_area,
 	.init = zcache_frontswap_init
@@ -3094,179 +1633,135 @@ struct frontswap_ops zcache_frontswap_register_ops(void)
 
 	return old_ops;
 }
-#endif
 
 /*
- * frontswap selfshrinking
- */
-
-#ifdef CONFIG_FRONTSWAP
-/* In HZ, controls frequency of worker invocation. */
-static unsigned int selfshrink_interval __read_mostly = 5;
-
-static void selfshrink_process(struct work_struct *work);
-static DECLARE_DELAYED_WORK(selfshrink_worker, selfshrink_process);
-
-/* Enable/disable with sysfs. */
-static bool frontswap_selfshrinking __read_mostly;
-
-/* Enable/disable with kernel boot option. */
-static bool use_frontswap_selfshrink __initdata = true;
-
-/*
- * The default values for the following parameters were deemed reasonable
- * by experimentation, may be workload-dependent, and can all be
- * adjusted via sysfs.
- */
-
-/* Control rate for frontswap shrinking. Higher hysteresis is slower. */
-static unsigned int frontswap_hysteresis __read_mostly = 20;
-
-/*
- * Number of selfshrink worker invocations to wait before observing that
- * frontswap selfshrinking should commence. Note that selfshrinking does
- * not use a separate worker thread.
+ * zcache initialization
+ * NOTE FOR NOW zcache or ramster MUST BE PROVIDED AS A KERNEL BOOT PARAMETER
+ * OR NOTHING HAPPENS!
  */
-static unsigned int frontswap_inertia __read_mostly = 3;
 
-/* Countdown to next invocation of frontswap_shrink() */
-static unsigned long frontswap_inertia_counter;
-
-/*
- * Invoked by the selfshrink worker thread, uses current number of pages
- * in frontswap (frontswap_curr_pages()), previous status, and control
- * values (hysteresis and inertia) to determine if frontswap should be
- * shrunk and what the new frontswap size should be.  Note that
- * frontswap_shrink is essentially a partial swapoff that immediately
- * transfers pages from the "swap device" (frontswap) back into kernel
- * RAM; despite the name, frontswap "shrinking" is very different from
- * the "shrinker" interface used by the kernel MM subsystem to reclaim
- * memory.
- */
-static void frontswap_selfshrink(void)
+static int __init enable_zcache(char *s)
 {
-	static unsigned long cur_frontswap_pages;
-	static unsigned long last_frontswap_pages;
-	static unsigned long tgt_frontswap_pages;
-
-	last_frontswap_pages = cur_frontswap_pages;
-	cur_frontswap_pages = frontswap_curr_pages();
-	if (!cur_frontswap_pages ||
-			(cur_frontswap_pages > last_frontswap_pages)) {
-		frontswap_inertia_counter = frontswap_inertia;
-		return;
-	}
-	if (frontswap_inertia_counter && --frontswap_inertia_counter)
-		return;
-	if (cur_frontswap_pages <= frontswap_hysteresis)
-		tgt_frontswap_pages = 0;
-	else
-		tgt_frontswap_pages = cur_frontswap_pages -
-			(cur_frontswap_pages / frontswap_hysteresis);
-	frontswap_shrink(tgt_frontswap_pages);
+	zcache_enabled = 1;
+	return 1;
 }
+__setup("zcache", enable_zcache);
 
-static int __init ramster_nofrontswap_selfshrink_setup(char *s)
+static int __init enable_ramster(char *s)
 {
-	use_frontswap_selfshrink = false;
+	zcache_enabled = 1;
+#ifdef CONFIG_RAMSTER
+	ramster_enabled = 1;
+#endif
 	return 1;
 }
+__setup("ramster", enable_ramster);
 
-__setup("noselfshrink", ramster_nofrontswap_selfshrink_setup);
+/* allow independent dynamic disabling of cleancache and frontswap */
 
-static void selfshrink_process(struct work_struct *work)
+static int __init no_cleancache(char *s)
 {
-	if (frontswap_selfshrinking && frontswap_enabled) {
-		frontswap_selfshrink();
-		schedule_delayed_work(&selfshrink_worker,
-			selfshrink_interval * HZ);
-	}
+	disable_cleancache = 1;
+	return 1;
 }
 
-static int ramster_enabled;
+__setup("nocleancache", no_cleancache);
 
-static int __init ramster_selfshrink_init(void)
+static int __init no_frontswap(char *s)
 {
-	frontswap_selfshrinking = ramster_enabled && use_frontswap_selfshrink;
-	if (frontswap_selfshrinking)
-		pr_info("ramster: Initializing frontswap "
-					"selfshrinking driver.\n");
-	else
-		return -ENODEV;
-
-	schedule_delayed_work(&selfshrink_worker, selfshrink_interval * HZ);
-
-	return 0;
+	disable_frontswap = 1;
+	return 1;
 }
 
-subsys_initcall(ramster_selfshrink_init);
-#endif
+__setup("nofrontswap", no_frontswap);
 
-/*
- * zcache initialization
- * NOTE FOR NOW ramster MUST BE PROVIDED AS A KERNEL BOOT PARAMETER OR
- * NOTHING HAPPENS!
- */
+static int __init no_frontswap_exclusive_gets(char *s)
+{
+	frontswap_has_exclusive_gets = false;
+	return 1;
+}
 
-static int ramster_enabled;
+__setup("nofrontswapexclusivegets", no_frontswap_exclusive_gets);
 
-static int __init enable_ramster(char *s)
+static int __init no_frontswap_ignore_nonactive(char *s)
 {
-	ramster_enabled = 1;
+	disable_frontswap_ignore_nonactive = 1;
 	return 1;
 }
-__setup("ramster", enable_ramster);
 
-/* allow independent dynamic disabling of cleancache and frontswap */
+__setup("nofrontswapignorenonactive", no_frontswap_ignore_nonactive);
 
-static int use_cleancache = 1;
-
-static int __init no_cleancache(char *s)
+static int __init no_cleancache_ignore_nonactive(char *s)
 {
-	pr_info("INIT no_cleancache called\n");
-	use_cleancache = 0;
+	disable_cleancache_ignore_nonactive = 1;
 	return 1;
 }
 
-/*
- * FIXME: need to guarantee this gets checked before zcache_init is called
- * What is the correct way to achieve this?
- */
-early_param("nocleancache", no_cleancache);
-
-static int use_frontswap = 1;
+__setup("nocleancacheignorenonactive", no_cleancache_ignore_nonactive);
 
-static int __init no_frontswap(char *s)
+static int __init enable_zcache_compressor(char *s)
 {
-	pr_info("INIT no_frontswap called\n");
-	use_frontswap = 0;
+	strncpy(zcache_comp_name, s, ZCACHE_COMP_NAME_SZ);
+	zcache_enabled = 1;
 	return 1;
 }
+__setup("zcache=", enable_zcache_compressor);
 
-__setup("nofrontswap", no_frontswap);
 
-static int __init zcache_init(void)
+static int __init zcache_comp_init(void)
 {
 	int ret = 0;
 
-#ifdef CONFIG_SYSFS
-	ret = sysfs_create_group(mm_kobj, &zcache_attr_group);
-	ret = sysfs_create_group(mm_kobj, &ramster_attr_group);
-	if (ret) {
-		pr_err("ramster: can't create sysfs\n");
+	/* check crypto algorithm */
+	if (*zcache_comp_name != '\0') {
+		ret = crypto_has_comp(zcache_comp_name, 0, 0);
+		if (!ret)
+			pr_info("zcache: %s not supported\n",
+					zcache_comp_name);
+	}
+	if (!ret)
+		strcpy(zcache_comp_name, "lzo");
+	ret = crypto_has_comp(zcache_comp_name, 0, 0);
+	if (!ret) {
+		ret = 1;
 		goto out;
 	}
-#endif /* CONFIG_SYSFS */
-#if defined(CONFIG_CLEANCACHE) || defined(CONFIG_FRONTSWAP)
+	pr_info("zcache: using %s compressor\n", zcache_comp_name);
+
+	/* alloc percpu transforms */
+	ret = 0;
+	zcache_comp_pcpu_tfms = alloc_percpu(struct crypto_comp *);
+	if (!zcache_comp_pcpu_tfms)
+		ret = 1;
+out:
+	return ret;
+}
+
+static int __init zcache_init(void)
+{
+	int ret = 0;
+
 	if (ramster_enabled) {
+		namestr = "ramster";
+		ramster_register_pamops(&zcache_pamops);
+	}
+#ifdef CONFIG_DEBUG_FS
+	zcache_debugfs_init();
+#endif
+	if (zcache_enabled) {
 		unsigned int cpu;
 
-		(void)r2net_register_handlers();
 		tmem_register_hostops(&zcache_hostops);
 		tmem_register_pamops(&zcache_pamops);
 		ret = register_cpu_notifier(&zcache_cpu_notifier_block);
 		if (ret) {
-			pr_err("ramster: can't register cpu notifier\n");
+			pr_err("%s: can't register cpu notifier\n", namestr);
+			goto out;
+		}
+		ret = zcache_comp_init();
+		if (ret) {
+			pr_err("%s: compressor initialization failed\n",
+				namestr);
 			goto out;
 		}
 		for_each_online_cpu(cpu) {
@@ -3279,42 +1774,47 @@ static int __init zcache_init(void)
 				sizeof(struct tmem_objnode), 0, 0, NULL);
 	zcache_obj_cache = kmem_cache_create("zcache_obj",
 				sizeof(struct tmem_obj), 0, 0, NULL);
-	ramster_flnode_cache = kmem_cache_create("ramster_flnode",
-				sizeof(struct flushlist_node), 0, 0, NULL);
-#endif
-#ifdef CONFIG_CLEANCACHE
-	pr_info("INIT ramster_enabled=%d use_cleancache=%d\n",
-					ramster_enabled, use_cleancache);
-	if (ramster_enabled && use_cleancache) {
+	ret = zcache_new_client(LOCAL_CLIENT);
+	if (ret) {
+		pr_err("%s: can't create client\n", namestr);
+		goto out;
+	}
+	zbud_init();
+	if (zcache_enabled && !disable_cleancache) {
 		struct cleancache_ops old_ops;
 
-		zbud_init();
 		register_shrinker(&zcache_shrinker);
 		old_ops = zcache_cleancache_register_ops();
-		pr_info("ramster: cleancache enabled using kernel "
-			"transcendent memory and compression buddies\n");
+		pr_info("%s: cleancache enabled using kernel transcendent "
+			"memory and compression buddies\n", namestr);
+#ifdef ZCACHE_DEBUG
+		pr_info("%s: cleancache: ignorenonactive = %d\n",
+			namestr, !disable_cleancache_ignore_nonactive);
+#endif
 		if (old_ops.init_fs != NULL)
-			pr_warning("ramster: cleancache_ops overridden");
+			pr_warn("%s: cleancache_ops overridden\n", namestr);
 	}
-#endif
-#ifdef CONFIG_FRONTSWAP
-	pr_info("INIT ramster_enabled=%d use_frontswap=%d\n",
-					ramster_enabled, use_frontswap);
-	if (ramster_enabled && use_frontswap) {
+	if (zcache_enabled && !disable_frontswap) {
 		struct frontswap_ops old_ops;
 
-		zcache_new_client(LOCAL_CLIENT);
 		old_ops = zcache_frontswap_register_ops();
-		pr_info("ramster: frontswap enabled using kernel "
-			"transcendent memory and xvmalloc\n");
+		if (frontswap_has_exclusive_gets)
+			frontswap_tmem_exclusive_gets(true);
+		pr_info("%s: frontswap enabled using kernel transcendent "
+			"memory and compression buddies\n", namestr);
+#ifdef ZCACHE_DEBUG
+		pr_info("%s: frontswap: excl gets = %d active only = %d\n",
+			namestr, frontswap_has_exclusive_gets,
+			!disable_frontswap_ignore_nonactive);
+#endif
 		if (old_ops.init != NULL)
-			pr_warning("ramster: frontswap_ops overridden");
+			pr_warn("%s: frontswap_ops overridden\n", namestr);
 	}
-	if (ramster_enabled && (use_frontswap || use_cleancache))
-		ramster_remotify_init();
-#endif
+	if (ramster_enabled)
+		ramster_init(!disable_cleancache, !disable_frontswap,
+				frontswap_has_exclusive_gets);
 out:
 	return ret;
 }
 
-module_init(zcache_init)
+late_initcall(zcache_init);
diff --git a/drivers/staging/ramster/zcache.h b/drivers/staging/ramster/zcache.h
index 250b121c22e5..81722b33b087 100644
--- a/drivers/staging/ramster/zcache.h
+++ b/drivers/staging/ramster/zcache.h
@@ -1,22 +1,53 @@
+
 /*
  * zcache.h
  *
- * External zcache functions
- *
- * Copyright (c) 2009-2012, Dan Magenheimer, Oracle Corp.
+ * Copyright (c) 2012, Dan Magenheimer, Oracle Corp.
  */
 
 #ifndef _ZCACHE_H_
 #define _ZCACHE_H_
 
-extern int zcache_put(int, int, struct tmem_oid *, uint32_t,
-			char *, size_t, bool, int);
-extern int zcache_autocreate_pool(int, int, bool);
-extern int zcache_get(int, int, struct tmem_oid *, uint32_t,
-			char *, size_t *, bool, int);
-extern int zcache_flush(int, int, struct tmem_oid *, uint32_t);
+struct zcache_preload {
+	struct tmem_obj *obj;
+	struct tmem_objnode *objnodes[OBJNODE_TREE_MAX_PATH];
+};
+
+struct tmem_pool;
+
+#define MAX_POOLS_PER_CLIENT 16
+
+#define MAX_CLIENTS 16
+#define LOCAL_CLIENT ((uint16_t)-1)
+
+struct zcache_client {
+	struct tmem_pool *tmem_pools[MAX_POOLS_PER_CLIENT];
+	bool allocated;
+	atomic_t refcount;
+};
+
+extern struct tmem_pool *zcache_get_pool_by_id(uint16_t cli_id,
+							uint16_t poolid);
+extern void zcache_put_pool(struct tmem_pool *pool);
+
+extern int zcache_put_page(int, int, struct tmem_oid *,
+				uint32_t, void *,
+				unsigned int, bool, int);
+extern int zcache_get_page(int, int, struct tmem_oid *, uint32_t,
+				void *, size_t *, bool, int);
+extern int zcache_flush_page(int, int, struct tmem_oid *, uint32_t);
 extern int zcache_flush_object(int, int, struct tmem_oid *);
-extern int zcache_localify(int, struct tmem_oid *, uint32_t,
-			char *, size_t, void *);
+extern void zcache_decompress_to_page(char *, unsigned int, struct page *);
+
+#ifdef CONFIG_RAMSTER
+extern void *zcache_pampd_create(char *, unsigned int, bool, int,
+				struct tmem_handle *);
+int zcache_autocreate_pool(unsigned int cli_id, unsigned int pool_id, bool eph);
+#endif
+
+#define MAX_POOLS_PER_CLIENT 16
+
+#define MAX_CLIENTS 16
+#define LOCAL_CLIENT ((uint16_t)-1)
 
-#endif /* _ZCACHE_H */
+#endif /* _ZCACHE_H_ */
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211.h b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
index b94c48b29302..5f5a30223d56 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
@@ -1094,7 +1094,7 @@ struct ieee80211_device {
 
 	int (*reset_port)(struct net_device *dev);
 
-	/* Softmac-generated frames (mamagement) are TXed via this
+	/* Softmac-generated frames (management) are TXed via this
 	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
 	 * not set. As some cards may have different HW queues that
 	 * one might want to use for data and management frames
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
index 8173240dcf7a..00f9af06aca5 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
@@ -21,6 +21,7 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <asm/uaccess.h>
+#include <linux/etherdevice.h>
 
 #include "dot11d.h"
 u8 rsn_authen_cipher_suite[16][4] = {
@@ -2110,13 +2111,7 @@ void ieee80211_rtl_stop_queue(struct ieee80211_device *ieee)
 inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
 {
 
-	get_random_bytes(ieee->current_network.bssid, ETH_ALEN);
-
-	/* an IBSS cell address must have the two less significant
-	 * bits of the first byte = 2
-	 */
-	ieee->current_network.bssid[0] &= ~0x01;
-	ieee->current_network.bssid[0] |= 0x02;
+	random_ether_addr(ieee->current_network.bssid);
 }
 
 /* called in user context only */
@@ -2808,9 +2803,7 @@ static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
 			       param->u.crypt.key_len);
 		return -EINVAL;
 	}
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= WEP_KEYS)
 			return -EINVAL;
 		crypt = &ieee->crypt[param->u.crypt.idx];
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
index 5d204906baf7..1ef8fd612732 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
@@ -14,6 +14,8 @@
  */
 
 
+#include <linux/etherdevice.h>
+
 #include "ieee80211.h"
 
 /* FIXME: add A freqs */
@@ -131,7 +133,6 @@ int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 {
 
 	int ret = 0;
-	u8 zero[] = {0,0,0,0,0,0};
 	unsigned long flags;
 
 	short ifup = ieee->proto_started;//dev->flags & IFF_UP;
@@ -161,7 +162,7 @@ int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
-	ieee->wap_set = memcmp(temp->sa_data, zero,ETH_ALEN)!=0;
+	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
 	//printk(" %x:%x:%x:%x:%x:%x\n", ieee->current_network.bssid[0],ieee->current_network.bssid[1],ieee->current_network.bssid[2],ieee->current_network.bssid[3],ieee->current_network.bssid[4],ieee->current_network.bssid[5]);
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
index fd22b75aea4f..20e5fb58f52f 100644
--- a/drivers/staging/rtl8187se/r8180_core.c
+++ b/drivers/staging/rtl8187se/r8180_core.c
@@ -2377,7 +2377,7 @@ void rtl8180_wmm_param_update(struct work_struct *work)
 				u8		u1bAIFS;
 				u32		u4bAcParam;
 				pAcParam = (PAC_PARAM)(&AcParam);
-				/* Retrieve paramters to update. */
+				/* Retrieve parameters to update. */
 				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G) ? 9 : 20) + aSifsTime;
 				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit))<<AC_PARAM_TXOP_LIMIT_OFFSET)|
 					      (((u32)(pAcParam->f.Ecw.f.ECWmax))<<AC_PARAM_ECW_MAX_OFFSET)|
@@ -2413,7 +2413,7 @@ void rtl8180_wmm_param_update(struct work_struct *work)
 			u8		u1bAIFS;
 			u32		u4bAcParam;
 
-			/* Retrieve paramters to update. */
+			/* Retrieve parameters to update. */
 			eACI = pAcParam->f.AciAifsn.f.ACI;
 			/* Mode G/A: slotTimeTimer = 9; Mode B: 20 */
 			u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G) ? 9 : 20) + aSifsTime;
diff --git a/drivers/staging/rtl8187se/r8180_hw.h b/drivers/staging/rtl8187se/r8180_hw.h
index 3fca144a56a4..533938123a97 100644
--- a/drivers/staging/rtl8187se/r8180_hw.h
+++ b/drivers/staging/rtl8187se/r8180_hw.h
@@ -554,11 +554,16 @@
 /* by amy for power save		*/
 /* by amy for antenna			*/
 #define EEPROM_SW_REVD_OFFSET 0x3f
-/*  BIT[8-9] is for SW Antenna Diversity. Only the value EEPROM_SW_AD_ENABLE means enable, other values are diable.					*/
+
+/*  BIT[8-9] is for SW Antenna Diversity. 
+ *  Only the value EEPROM_SW_AD_ENABLE means enable, other values are disable.
+ */
 #define EEPROM_SW_AD_MASK			0x0300
 #define EEPROM_SW_AD_ENABLE			0x0100
 
-/* BIT[10-11] determine if Antenna 1 is the Default Antenna. Only the value EEPROM_DEF_ANT_1 means TRUE, other values are FALSE.	*/
+/* BIT[10-11] determine if Antenna 1 is the Default Antenna.
+ * Only the value EEPROM_DEF_ANT_1 means TRUE, other values are FALSE.	
+ */
 #define EEPROM_DEF_ANT_MASK			0x0C00
 #define EEPROM_DEF_ANT_1			0x0400
 /*by amy for antenna																				*/
diff --git a/drivers/staging/rtl8187se/r8185b_init.c b/drivers/staging/rtl8187se/r8185b_init.c
index 914495783c06..bf343199bd21 100644
--- a/drivers/staging/rtl8187se/r8185b_init.c
+++ b/drivers/staging/rtl8187se/r8185b_init.c
@@ -1008,7 +1008,7 @@ void ActUpdateChannelAccessSetting(struct net_device *dev,
 				u8		u1bAIFS;
 				u32		u4bAcParam;
 
-				/*  Retrieve paramters to update. */
+				/*  Retrieve parameters to update. */
 				eACI = pAcParam->f.AciAifsn.f.ACI;
 				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * ChnlAccessSetting->SlotTimeTimer + aSifsTime;
 				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index b526fa428679..dd2a96bfcc0c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -265,10 +265,11 @@ bool init_firmware(struct net_device *dev)
 			case FW_SOURCE_IMG_FILE:
 			{
 				if (pfirmware->firmware_buf_size[init_step] == 0) {
-					const char *fw_name[3] = { "RTL8192E/boot.img",
-							   "RTL8192E/main.img",
-							   "RTL8192E/data.img"
-							 };
+					const char *fw_name[3] = {
+							RTL8192E_BOOT_IMG_FW,
+							RTL8192E_MAIN_IMG_FW,
+							RTL8192E_DATA_IMG_FW
+					};
 					const struct firmware	*fw_entry;
 					int rc;
 					rc = request_firmware(&fw_entry,
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index caa878833106..06d6abc8345c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -23,6 +23,10 @@
 
 #define GET_COMMAND_PACKET_FRAG_THRESHOLD(v)	(4*(v/4) - 8)
 
+#define RTL8192E_BOOT_IMG_FW	"RTL8192E/boot.img"
+#define RTL8192E_MAIN_IMG_FW	"RTL8192E/main.img"
+#define RTL8192E_DATA_IMG_FW	"RTL8192E/data.img"
+
 enum firmware_init_step {
 	FW_INIT_STEP0_BOOT = 0,
 	FW_INIT_STEP1_MAIN = 1,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 4f602b227b50..81134d312ee3 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -734,8 +734,7 @@ static void rtl8192_prepare_beacon(struct r8192_priv *priv)
 
 	ring = &priv->tx_ring[BEACON_QUEUE];
 	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
+	kfree_skb(pskb);
 
 	pnewskb = rtllib_get_beacon(priv->rtllib);
 	if (!pnewskb)
@@ -3125,6 +3124,9 @@ MODULE_DESCRIPTION("Linux driver for Realtek RTL819x WiFi cards");
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_VERSION(DRV_VERSION);
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(RTL8192E_BOOT_IMG_FW);
+MODULE_FIRMWARE(RTL8192E_MAIN_IMG_FW);
+MODULE_FIRMWARE(RTL8192E_DATA_IMG_FW);
 
 module_param(ifname, charp, S_IRUGO|S_IWUSR);
 module_param(hwwep, int, S_IRUGO|S_IWUSR);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index 481b1e4d4913..1853665764a0 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -202,7 +202,7 @@ static void dm_check_ac_dc_power(struct net_device *dev)
 
 	if (priv->ResetProgress == RESET_TYPE_SILENT) {
 		RT_TRACE((COMP_INIT | COMP_POWER | COMP_RF),
-			 "GPIOChangeRFWorkItemCallBack(): Silent Reseting!!!!!!!\n");
+			 "GPIOChangeRFWorkItemCallBack(): Silent Reset!!!!!!!\n");
 		return;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
index ddadcc3e4e7c..5abbee37cdca 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
@@ -31,12 +31,10 @@ static void rtl8192_parse_pci_configuration(struct pci_dev *pdev,
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
 
 	u8 tmp;
-	int pos;
-	u8 LinkCtrlReg;
+	u16 LinkCtrlReg;
 
-	pos = pci_find_capability(priv->pdev, PCI_CAP_ID_EXP);
-	pci_read_config_byte(priv->pdev, pos + PCI_EXP_LNKCTL, &LinkCtrlReg);
-	priv->NdisAdapter.LinkCtrlReg = LinkCtrlReg;
+	pcie_capability_read_word(priv->pdev, PCI_EXP_LNKCTL, &LinkCtrlReg);
+	priv->NdisAdapter.LinkCtrlReg = (u8)LinkCtrlReg;
 
 	RT_TRACE(COMP_INIT, "Link Control Register =%x\n",
 		 priv->NdisAdapter.LinkCtrlReg);
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index d7460ae3a765..9ac8d8ea4ae8 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -2397,12 +2397,12 @@ struct rtllib_device {
 				 struct rtllib_network *network, u16 type);
 	int (*is_qos_active)(struct net_device *dev, struct sk_buff *skb);
 
-	/* Softmac-generated frames (mamagement) are TXed via this
+	/* Softmac-generated frames (management) are TXed via this
 	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
 	 * not set. As some cards may have different HW queues that
 	 * one might want to use for data and management frames
 	 * the option to have two callbacks might be useful.
-	 * This fucntion can't sleep.
+	 * This function can't sleep.
 	 */
 	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
 			       struct net_device *dev);
@@ -2441,9 +2441,9 @@ struct rtllib_device {
 	 * it is called in a work_queue when switching to ad-hoc mode
 	 * or in behalf of iwlist scan when the card is associated
 	 * and root user ask for a scan.
-	 * the fucntion stop_scan should stop both the syncro and
+	 * the function stop_scan should stop both the syncro and
 	 * background scanning and can sleep.
-	 * The fucntion start_scan should initiate the background
+	 * The function start_scan should initiate the background
 	 * scanning and can't sleep.
 	 */
 	void (*scan_syncro)(struct net_device *dev);
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index a21b4d91a596..4feecec8609c 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -19,6 +19,7 @@
 #include <linux/random.h>
 #include <linux/delay.h>
 #include <linux/uaccess.h>
+#include <linux/etherdevice.h>
 #include "dot11d.h"
 
 short rtllib_is_54g(struct rtllib_network *net)
@@ -266,7 +267,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee)
 		else
 			ieee->seq_ctrl[0]++;
 
-		/* check wether the managed packet queued greater than 5 */
+		/* check whether the managed packet queued greater than 5 */
 		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)) {
@@ -1687,7 +1688,7 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 		 * if the network does broadcast and the user did set essid
 		 * check if essid match
 		 * if the ap is not set, check that the user set the bssid
-		 * and the network does bradcast and that those two bssid match
+		 * and the network does broadcast and that those two bssid match
 		 */
 		if ((apset && apmatch &&
 		   ((ssidset && ssidbroad && ssidmatch) ||
@@ -1843,7 +1844,7 @@ static short probe_rq_parse(struct rtllib_device *ieee, struct sk_buff *skb,
 
 	bssid_match =
 	  (memcmp(header->addr3, ieee->current_network.bssid, ETH_ALEN) != 0) &&
-	  (memcmp(header->addr3, "\xff\xff\xff\xff\xff\xff", ETH_ALEN) != 0);
+	  (!is_broadcast_ether_addr(header->addr3));
 	if (bssid_match)
 		return -1;
 
@@ -2442,7 +2443,7 @@ inline int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 	return 0;
 }
 
-/* following are for a simplier TX queue management.
+/* 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 use the kernel netif_* and takes
@@ -2619,13 +2620,7 @@ void rtllib_wake_all_queues(struct rtllib_device *ieee)
 inline void rtllib_randomize_cell(struct rtllib_device *ieee)
 {
 
-	get_random_bytes(ieee->current_network.bssid, ETH_ALEN);
-
-	/* an IBSS cell address must have the two less significant
-	 * bits of the first byte = 2
-	 */
-	ieee->current_network.bssid[0] &= ~0x01;
-	ieee->current_network.bssid[0] |= 0x02;
+	random_ether_addr(ieee->current_network.bssid);
 }
 
 /* called in user context only */
@@ -3361,9 +3356,7 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 			       param->u.crypt.key_len);
 		return -EINVAL;
 	}
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= NUM_WEP_KEYS)
 			return -EINVAL;
 		crypt = &ieee->crypt_info.crypt[param->u.crypt.idx];
@@ -3411,8 +3404,7 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 
 		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 
-		new_crypt = (struct lib80211_crypt_data *)
-			kmalloc(sizeof(*new_crypt), GFP_KERNEL);
+		new_crypt = kmalloc(sizeof(*new_crypt), GFP_KERNEL);
 		if (new_crypt == NULL) {
 			ret = -ENOMEM;
 			goto done;
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 1bb6b52e0f24..740cf85e9d5b 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -14,6 +14,8 @@
  */
 
 
+#include <linux/etherdevice.h>
+
 #include "rtllib.h"
 #include "dot11d.h"
 /* FIXME: add A freqs */
@@ -137,7 +139,6 @@ int rtllib_wx_set_wap(struct rtllib_device *ieee,
 {
 
 	int ret = 0;
-	u8 zero[] = {0, 0, 0, 0, 0, 0};
 	unsigned long flags;
 
 	short ifup = ieee->proto_started;
@@ -157,7 +158,7 @@ int rtllib_wx_set_wap(struct rtllib_device *ieee,
 		goto out;
 	}
 
-	if (memcmp(temp->sa_data, zero, ETH_ALEN) == 0) {
+	if (is_zero_ether_addr(temp->sa_data)) {
 		spin_lock_irqsave(&ieee->lock, flags);
 		memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
 		ieee->wap_set = 0;
@@ -177,7 +178,7 @@ int rtllib_wx_set_wap(struct rtllib_device *ieee,
 
 	ieee->cannot_notify = false;
 	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
-	ieee->wap_set = (memcmp(temp->sa_data, zero, ETH_ALEN) != 0);
+	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index 1c0a1db6420f..13f45c3125ce 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -2114,7 +2114,7 @@ struct ieee80211_device {
                                   struct ieee80211_network * network, u16 type);
         int (*is_qos_active) (struct net_device *dev, struct sk_buff *skb);
 
-	/* Softmac-generated frames (mamagement) are TXed via this
+	/* Softmac-generated frames (management) are TXed via this
 	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
 	 * not set. As some cards may have different HW queues that
 	 * one might want to use for data and management frames
@@ -2192,7 +2192,7 @@ struct ieee80211_device {
 	int (*handle_assoc_response) (struct net_device * dev, struct ieee80211_assoc_response_frame * resp, struct ieee80211_network * network);
 
 
-	/* check whether Tx hw resouce available */
+	/* check whether Tx hw resource available */
 	short (*check_nic_enough_desc)(struct net_device *dev, int queue_index);
 	//added by wb for HT related
 //	void (*SwChnlByTimerHandler)(struct net_device *dev, int channel);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index f6ff8cff313a..7a0707810fd0 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -20,6 +20,8 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <asm/uaccess.h>
+#include <linux/etherdevice.h>
+
 #include "dot11d.h"
 
 u8 rsn_authen_cipher_suite[16][4] = {
@@ -269,7 +271,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 		else
 			ieee->seq_ctrl[0]++;
 
-		/* check wether the managed packet queued greater than 5 */
+		/* check whether the managed packet queued greater than 5 */
 		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) ) {
@@ -1448,7 +1450,7 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 			( apset && apmatch &&
 				((ssidset && ssidbroad && ssidmatch) || (ssidbroad && !ssidset) || (!ssidbroad && ssidset)) ) ||
 			/* if the ap is not set, check that the user set the bssid
-			 * and the network does bradcast and that those two bssid matches
+			 * and the network does broadcast and that those two bssid matches
 			 */
 			(!apset && ssidset && ssidbroad && ssidmatch)
 			){
@@ -2286,13 +2288,7 @@ void ieee80211_stop_queue(struct ieee80211_device *ieee)
 inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
 {
 
-	get_random_bytes(ieee->current_network.bssid, ETH_ALEN);
-
-	/* an IBSS cell address must have the two less significant
-	 * bits of the first byte = 2
-	 */
-	ieee->current_network.bssid[0] &= ~0x01;
-	ieee->current_network.bssid[0] |= 0x02;
+	random_ether_addr(ieee->current_network.bssid);
 }
 
 /* called in user context only */
@@ -2520,7 +2516,7 @@ void ieee80211_associate_retry_wq(struct work_struct *work)
 
 	/* until we do not set the state to IEEE80211_NOLINK
 	* there are no possibility to have someone else trying
-	* to start an association procdure (we get here with
+	* to start an association procedure (we get here with
 	* ieee->state = IEEE80211_ASSOCIATING).
 	* When we set the state to IEEE80211_NOLINK it is possible
 	* that the RX path run an attempt to associate, but
@@ -2969,9 +2965,7 @@ static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
 			       param->u.crypt.key_len);
 		return -EINVAL;
 	}
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= WEP_KEYS)
 			return -EINVAL;
 		crypt = &ieee->crypt[param->u.crypt.idx];
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index cb5a3c32974e..421da8a07697 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -14,6 +14,8 @@
  */
 
 
+#include <linux/etherdevice.h>
+
 #include "ieee80211.h"
 #include "dot11d.h"
 /* FIXME: add A freqs */
@@ -136,7 +138,6 @@ int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 {
 
 	int ret = 0;
-	u8 zero[] = {0,0,0,0,0,0};
 	unsigned long flags;
 
 	short ifup = ieee->proto_started;//dev->flags & IFF_UP;
@@ -165,7 +166,7 @@ int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
-	ieee->wap_set = memcmp(temp->sa_data, zero,ETH_ALEN)!=0;
+	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
 
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 27d083a70eb2..1ebea3daea2d 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -1,6 +1,6 @@
 /********************************************************************************************************************************
  * This file is created to process BA Action Frame. According to 802.11 spec, there are 3 BA action types at all. And as BA is
- * related to TS, this part need some struture defined in QOS side code. Also TX RX is going to be resturctured, so how to send
+ * related to TS, this part need some structure defined in QOS side code. Also TX RX is going to be resturctured, so how to send
  * ADDBAREQ ADDBARSP and DELBA packet is still on consideration. Temporarily use MANAGE QUEUE instead of Normal Queue.
  * WB 2008-05-27
  * *****************************************************************************************************************************/
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
index 0b1a1fc09391..a60b39cdb472 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
@@ -15,7 +15,7 @@
 #define HT_OPMODE_MIXED			3
 
 //
-// MIMO Power Save Setings
+// MIMO Power Save Settings
 //
 #define MIMO_PS_STATIC				0
 #define MIMO_PS_DYNAMIC			1
@@ -242,7 +242,7 @@ typedef struct _RT_HIGH_THROUGHPUT{
 	u8				bEnableHT;
 	u8				bCurrentHTSupport;
 
-	u8				bRegBW40MHz;				// Tx 40MHz channel capablity
+	u8				bRegBW40MHz;				// Tx 40MHz channel capability
 	u8				bCurBW40MHz;				// Tx 40MHz channel capability
 
 	u8				bRegShortGI40MHz;			// Tx Short GI for 40Mhz
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index e88a839b2a91..ebb523904edc 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -912,7 +912,7 @@ u8 HTFilterMCSRate( struct ieee80211_device* ieee, u8* pSupportMCS, u8* pOperate
 
 	u8 i=0;
 
-	// filter out operational rate set not supported by AP, the lenth of it is 16
+	// filter out operational rate set not supported by AP, the length of it is 16
 	for(i=0;i<=15;i++){
 		pOperateMCS[i] = ieee->Regdot11HTOperationalRateSet[i]&pSupportMCS[i];
 	}
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 5981d6658320..5a2fab9fa772 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -2232,24 +2232,15 @@ short rtl8192_usb_initendpoints(struct net_device *dev)
 	memset(priv->rx_urb, 0, sizeof(struct urb*) * MAX_RX_URB);
 	priv->pp_rxskb = kcalloc(MAX_RX_URB, sizeof(struct sk_buff *),
 				 GFP_KERNEL);
-	if (priv->pp_rxskb == NULL)
-		goto destroy;
-
-	goto _middle;
-
-
-destroy:
-	kfree(priv->pp_rxskb);
-	kfree(priv->rx_urb);
-
-	priv->pp_rxskb = NULL;
-	priv->rx_urb = NULL;
-
-	DMESGE("Endpoint Alloc Failure");
-	return -ENOMEM;
+	if (!priv->pp_rxskb) {
+		kfree(priv->rx_urb);
 
+		priv->pp_rxskb = NULL;
+		priv->rx_urb = NULL;
 
-_middle:
+		DMESGE("Endpoint Alloc Failure");
+		return -ENOMEM;
+	}
 
 	printk("End of initendpoints\n");
 	return 0;
@@ -2808,9 +2799,7 @@ static void rtl8192_init_priv_variable(struct net_device* dev)
 		(priv->EarlyRxThreshold == 7 ? RCR_ONLYERLPKT:0);
 
 	priv->AcmControl = 0;
-	priv->pFirmware = kmalloc(sizeof(rt_firmware), GFP_KERNEL);
-	if (priv->pFirmware)
-	memset(priv->pFirmware, 0, sizeof(rt_firmware));
+	priv->pFirmware = kzalloc(sizeof(rt_firmware), GFP_KERNEL);
 
 	/* rx related queue */
 	skb_queue_head_init(&priv->rx_queue);
diff --git a/drivers/staging/rtl8192u/r819xU_HTType.h b/drivers/staging/rtl8192u/r819xU_HTType.h
index e07f8b17a0d6..6c1d05e1e820 100644
--- a/drivers/staging/rtl8192u/r819xU_HTType.h
+++ b/drivers/staging/rtl8192u/r819xU_HTType.h
@@ -16,7 +16,7 @@
 #define HT_OPMODE_MIXED			3
 
 //
-// MIMO Power Save Setings
+// MIMO Power Save Settings
 //
 #define MIMO_PS_STATIC				0
 #define MIMO_PS_DYNAMIC			1
diff --git a/drivers/staging/rtl8192u/r819xU_phyreg.h b/drivers/staging/rtl8192u/r819xU_phyreg.h
index 50f24dce8b16..cca34c05f6a5 100644
--- a/drivers/staging/rtl8192u/r819xU_phyreg.h
+++ b/drivers/staging/rtl8192u/r819xU_phyreg.h
@@ -443,7 +443,7 @@
 #define bCCKRxIG                  			0x7f00
 #define bCCKLNAPolarity           		0x800000
 #define bCCKRx1stGain             		0x7f0000
-#define bCCKRFExtend              		0x20000000 //CCK Rx inital gain polarity
+#define bCCKRFExtend              		0x20000000 //CCK Rx initial gain polarity
 #define bCCKRxAGCSatLevel        		0x1f000000
 #define bCCKRxAGCSatCount       		0xe0
 #define bCCKRxRFSettle            		0x1f       //AGCsamp_dly
diff --git a/drivers/staging/rtl8712/drv_types.h b/drivers/staging/rtl8712/drv_types.h
index 62b55663c63a..a074fe810169 100644
--- a/drivers/staging/rtl8712/drv_types.h
+++ b/drivers/staging/rtl8712/drv_types.h
@@ -70,9 +70,7 @@ struct	qos_priv	{
 #include "rtl871x_event.h"
 #include "rtl871x_led.h"
 
-#define SPEC_DEV_ID_NONE BIT(0)
 #define SPEC_DEV_ID_DISABLE_HT BIT(1)
-#define SPEC_DEV_ID_ENABLE_PS BIT(2)
 
 struct specific_device_id {
 	u32		flags;
@@ -127,13 +125,6 @@ struct registry_priv {
 	u8 wifi_test;
 };
 
-/* For registry parameters */
-#define RGTRY_OFT(field) ((addr_t)FIELD_OFFSET(struct registry_priv, field))
-#define RGTRY_SZ(field)   sizeof(((struct registry_priv *)0)->field)
-#define BSSID_OFT(field) ((addr_t)FIELD_OFFSET(struct ndis_wlan_bssid_ex, \
-			 field))
-#define BSSID_SZ(field)   sizeof(((struct ndis_wlan_bssid_ex *)0)->field)
-
 struct dvobj_priv {
 	struct _adapter *padapter;
 	u32 nr_endpoint;
diff --git a/drivers/staging/rtl8712/ethernet.h b/drivers/staging/rtl8712/ethernet.h
index 882d61b2e959..90954203776d 100644
--- a/drivers/staging/rtl8712/ethernet.h
+++ b/drivers/staging/rtl8712/ethernet.h
@@ -35,14 +35,6 @@
 
 /*!< Is Multicast Address? */
 #define RT_ETH_IS_MULTICAST(_pAddr)	((((u8 *)(_pAddr))[0]&0x01) != 0)
-/*!< Is Broadcast Address? */
-#define RT_ETH_IS_BROADCAST(_pAddr)	(				\
-			((u8 *)(_pAddr))[0] == 0xff	&&		\
-			((u8 *)(_pAddr))[1] == 0xff	&&		\
-			((u8 *)(_pAddr))[2] == 0xff	&&		\
-			((u8 *)(_pAddr))[3] == 0xff	&&		\
-			((u8 *)(_pAddr))[4] == 0xff	&&		\
-			((u8 *)(_pAddr))[5] == 0xff)
 
 #endif /* #ifndef __INC_ETHERNET_H */
 
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index 448f00dd68fe..e00f7918d261 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -96,7 +96,7 @@ static char *initmac;
 /* if wifi_test = 1, driver will disable the turbo mode and pass it to
  * firmware private.
  */
-static int wifi_test = 0;
+static int wifi_test;
 
 module_param_string(ifname, ifname, sizeof(ifname), S_IRUGO|S_IWUSR);
 module_param(wifi_test, int, 0644);
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 8e82ce2fee38..c76732cdb183 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -374,6 +374,8 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 		a_len -= ETH_HLEN;
 		/* Allocate new skb for releasing to upper layer */
 		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if (!sub_skb)
+			break;
 		skb_reserve(sub_skb, 12);
 		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
 		memcpy(data_ptr, pdata, nSubframe_Length);
@@ -1094,6 +1096,8 @@ static int recvbuf2recvframe(struct _adapter *padapter, struct sk_buff *pskb)
 			precvframe->u.hdr.rx_end = pkt_copy->data + alloc_sz;
 		} else {
 			precvframe->u.hdr.pkt = skb_clone(pskb, GFP_ATOMIC);
+			if (!precvframe->u.hdr.pkt)
+				return _FAIL;
 			precvframe->u.hdr.rx_head = pbuf;
 			precvframe->u.hdr.rx_data = pbuf;
 			precvframe->u.hdr.rx_tail = pbuf;
@@ -1127,6 +1131,9 @@ static void recv_tasklet(void *priv)
 		recvbuf2recvframe(padapter, pskb);
 		skb_reset_tail_pointer(pskb);
 		pskb->len = 0;
-		skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+		if (!skb_cloned(pskb))
+			skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+		else
+			consume_skb(pskb);
 	}
 }
diff --git a/drivers/staging/rtl8712/rtl8712_recv.h b/drivers/staging/rtl8712/rtl8712_recv.h
index 8efbd1fa035f..fd9e3fc4c226 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.h
+++ b/drivers/staging/rtl8712/rtl8712_recv.h
@@ -41,7 +41,7 @@
 #define RECV_BLK_SZ 512
 #define RECV_BLK_CNT 16
 #define RECV_BLK_TH RECV_BLK_CNT
-#define MAX_RECVBUF_SZ (30720) /* 30K */
+#define MAX_RECVBUF_SZ 9100
 #define RECVBUFF_ALIGN_SZ 512
 #define RSVD_ROOM_SZ (0)
 /*These definition is used for Rx packet reordering.*/
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.c b/drivers/staging/rtl8712/rtl8712_xmit.c
index 3d23514c0222..4e3f09420c1e 100644
--- a/drivers/staging/rtl8712/rtl8712_xmit.c
+++ b/drivers/staging/rtl8712/rtl8712_xmit.c
@@ -376,7 +376,7 @@ u8 r8712_dump_aggr_xframe(struct xmit_buf *pxmitbuf,
 {
 	struct _adapter *padapter = pxmitframe->padapter;
 	struct dvobj_priv *pdvobj = (struct dvobj_priv *) &padapter->dvobjpriv;
-	struct tx_desc * ptxdesc = (struct tx_desc *)pxmitbuf->pbuf;
+	struct tx_desc *ptxdesc = (struct tx_desc *)pxmitbuf->pbuf;
 	struct cmd_hdr *pcmd_hdr = (struct cmd_hdr *)
 		(pxmitbuf->pbuf + TXDESC_SIZE);
 	u16 total_length = (u16) (ptxdesc->txdw0 & 0xffff);
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index 35e781fca4a0..c9a6a7fbb89c 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -407,9 +407,7 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 	if (param_len != (u32)((u8 *) param->u.crypt.key - (u8 *)param) +
 			 param->u.crypt.key_len)
 		return -EINVAL;
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= WEP_KEYS) {
 			/* for large key indices, set the default (0) */
 			param->u.crypt.idx = 0;
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.c b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
index f352b32355a0..d3ab24e34e3d 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_set.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
@@ -131,10 +131,7 @@ u8 r8712_set_802_11_bssid(struct _adapter *padapter, u8 *bssid)
 	u8 status = true;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 &&
-	     bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) ||
-	    (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF &&
-	     bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) {
+	if (is_zero_ether_addr(bssid) || is_broadcast_ether_addr(bssid)) {
 		status = false;
 		return status;
 	}
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index dc7adc132d12..c51ad9ed4b52 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -28,6 +28,8 @@
 
 #define _RTL871X_MLME_C_
 
+#include <linux/etherdevice.h>
+
 #include "osdep_service.h"
 #include "drv_types.h"
 #include "recv_osdep.h"
@@ -146,9 +148,8 @@ static struct wlan_network *_r8712_find_network(struct  __queue *scanned_queue,
 	unsigned long irqL;
 	struct list_head *phead, *plist;
 	struct wlan_network *pnetwork = NULL;
-	u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
 
-	if (!memcmp(zero_addr, addr, ETH_ALEN))
+	if (is_zero_ether_addr(addr))
 		return NULL;
 	spin_lock_irqsave(&scanned_queue->lock, irqL);
 	phead = get_list_head(scanned_queue);
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.h b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
index 6024c4f63d5b..70ff924fba04 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.h
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
@@ -30,26 +30,7 @@
 #include "drv_types.h"
 
 
-#define FW_PWR0	0
-#define FW_PWR1		1
-#define FW_PWR2		2
-#define FW_PWR3		3
-
-
-#define HW_PWR0	7
-#define HW_PWR1		6
-#define HW_PWR2		2
-#define HW_PWR3	0
-#define HW_PWR4	8
-
-#define FW_PWRMSK	0x7
-
-
-#define XMIT_ALIVE	BIT(0)
-#define RECV_ALIVE	BIT(1)
 #define CMD_ALIVE	BIT(2)
-#define EVT_ALIVE	BIT(3)
-
 
 enum Power_Mgnt {
 	PS_MODE_ACTIVE	= 0	,
@@ -66,7 +47,6 @@ enum Power_Mgnt {
 	PS_MODE_NUM
 };
 
-
 /*
 	BIT[2:0] = HW state
 	BIT[3] = Protocol PS state, 0: register active state,
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index c9d1743e5c5d..23ec684b60e1 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -32,6 +32,7 @@
 #include <linux/slab.h>
 #include <linux/if_ether.h>
 #include <linux/kmemleak.h>
+#include <linux/etherdevice.h>
 
 #include "osdep_service.h"
 #include "drv_types.h"
@@ -331,8 +332,8 @@ static sint sta2sta_data_frame(struct _adapter *adapter,
 			return _FAIL;
 		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast))
 			return _FAIL;
-		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
-		    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		if (is_zero_ether_addr(pattrib->bssid) ||
+		    is_zero_ether_addr(mybssid) ||
 		    (memcmp(pattrib->bssid, mybssid, ETH_ALEN)))
 			return _FAIL;
 		sta_addr = pattrib->src;
@@ -409,8 +410,8 @@ static sint ap2sta_data_frame(struct _adapter *adapter,
 		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast))
 			return _FAIL;
 		/* check BSSID */
-		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
-		     !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		if (is_zero_ether_addr(pattrib->bssid) ||
+		     is_zero_ether_addr(mybssid) ||
 		     (memcmp(pattrib->bssid, mybssid, ETH_ALEN)))
 			return _FAIL;
 		if (bmcast)
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index 7b92927a04dc..e33fd6db246d 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -58,7 +58,7 @@ struct arc4context {
 	u8 state[256];
 };
 
-static void arcfour_init(struct arc4context *parc4ctx, u8 * key, u32 key_len)
+static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
 {
 	u32	t, u;
 	u32	keyindex;
@@ -288,7 +288,7 @@ static void secmicclear(struct mic_data *pmicdata)
 	pmicdata->M = 0;
 }
 
-void r8712_secmicsetkey(struct mic_data *pmicdata, u8 * key)
+void r8712_secmicsetkey(struct mic_data *pmicdata, u8 *key)
 {
 	/* Set the key */
 	pmicdata->K0 = secmicgetuint32(key);
@@ -320,7 +320,7 @@ static void secmicappendbyte(struct mic_data *pmicdata, u8 b)
 	}
 }
 
-void r8712_secmicappend(struct mic_data *pmicdata, u8 * src, u32 nbytes)
+void r8712_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
 {
 	/* This is simple */
 	while (nbytes > 0) {
@@ -1368,7 +1368,7 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 					   precvframe)->u.hdr.attrib;
 	struct	security_priv *psecuritypriv = &padapter->securitypriv;
 
-	pframe = (unsigned char *)((union recv_frame*)precvframe)->
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->
 		 u.hdr.rx_data;
 	/* 4 start to encrypt each fragment */
 	if ((prxattrib->encrypt == _AES_)) {
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index c758c40e0c85..6b73843e580a 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -37,7 +37,6 @@
 #include "recv_osdep.h"
 #include "xmit_osdep.h"
 #include "rtl8712_efuse.h"
-#include "usb_vendor_req.h"
 #include "usb_ops.h"
 #include "usb_osintf.h"
 
diff --git a/drivers/staging/rtl8712/usb_osintf.h b/drivers/staging/rtl8712/usb_osintf.h
index d95797aac37a..609f9210cc46 100644
--- a/drivers/staging/rtl8712/usb_osintf.h
+++ b/drivers/staging/rtl8712/usb_osintf.h
@@ -28,9 +28,6 @@
 
 #include "osdep_service.h"
 #include "drv_types.h"
-#include "usb_vendor_req.h"
-
-#define USBD_HALTED(Status) ((u32)(Status) >> 30 == 3)
 
 extern char *r8712_initmac;
 
diff --git a/drivers/staging/rtl8712/usb_vendor_req.h b/drivers/staging/rtl8712/usb_vendor_req.h
deleted file mode 100644
index 82335a83d0d2..000000000000
--- a/drivers/staging/rtl8712/usb_vendor_req.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-#ifndef _USB_VENDOR_REQUEST_H_
-#define _USB_VENDOR_REQUEST_H_
-
-/*4	Set/Get Register related wIndex/Data */
-#define	RT_USB_RESET_MASK_OFF		0
-#define	RT_USB_RESET_MASK_ON		1
-#define	RT_USB_SLEEP_MASK_OFF		0
-#define	RT_USB_SLEEP_MASK_ON		1
-#define	RT_USB_LDO_ON			1
-#define	RT_USB_LDO_OFF			0
-
-/*4	Set/Get SYSCLK related	wValue or Data */
-#define	RT_USB_SYSCLK_32KHZ		0
-#define	RT_USB_SYSCLK_40MHZ		1
-#define	RT_USB_SYSCLK_60MHZ		2
-
-enum RT_USB_BREQUEST {
-	RT_USB_SET_REGISTER		= 1,
-	RT_USB_SET_SYSCLK		= 2,
-	RT_USB_GET_SYSCLK		= 3,
-	RT_USB_GET_REGISTER		= 4
-};
-
-enum RT_USB_WVALUE {
-	RT_USB_RESET_MASK	=	1,
-	RT_USB_SLEEP_MASK	=	2,
-	RT_USB_USB_HRCPWM	=	3,
-	RT_USB_LDO		=	4,
-	RT_USB_BOOT_TYPE	=	5
-};
-
-#endif
-
diff --git a/drivers/staging/rts5139/rts51x_fop.c b/drivers/staging/rts5139/rts51x_fop.c
index e1200fe89579..bf1a9e64e874 100644
--- a/drivers/staging/rts5139/rts51x_fop.c
+++ b/drivers/staging/rts5139/rts51x_fop.c
@@ -79,7 +79,7 @@ static int rts51x_sd_direct_cmnd(struct rts51x_chip *chip,
 
 	case 1:
 		/* Read from card */
-		buf = kmalloc(cmnd->buf_len, GFP_KERNEL);
+		buf = kzalloc(cmnd->buf_len, GFP_KERNEL);
 		if (!buf)
 			TRACE_RET(chip, STATUS_NOMEM);
 
diff --git a/drivers/staging/rts5139/trace.h b/drivers/staging/rts5139/trace.h
index 0584b8ab43ca..c9dfb1ea4115 100644
--- a/drivers/staging/rts5139/trace.h
+++ b/drivers/staging/rts5139/trace.h
@@ -93,35 +93,9 @@ do {									\
 #endif
 
 #ifdef CONFIG_RTS5139_DEBUG
-static inline void rts51x_dump(u8 *buf, int buf_len)
-{
-	int i;
-	u8 tmp[16] = { 0 };
-	u8 *_ptr = buf;
-
-	for (i = 0; i < ((buf_len) / 16); i++) {
-		RTS51X_DEBUGP("%02x %02x %02x %02x %02x %02x %02x %02x "
-			       "%02x %02x %02x %02x %02x %02x %02x %02x\n",
-			       _ptr[0], _ptr[1], _ptr[2], _ptr[3], _ptr[4],
-			       _ptr[5], _ptr[6], _ptr[7], _ptr[8], _ptr[9],
-			       _ptr[10], _ptr[11], _ptr[12], _ptr[13], _ptr[14],
-			       _ptr[15]);
-		_ptr += 16;
-	}
-	if ((buf_len) % 16) {
-		memcpy(tmp, _ptr, (buf_len) % 16);
-		_ptr = tmp;
-		RTS51X_DEBUGP("%02x %02x %02x %02x %02x %02x %02x %02x "
-			       "%02x %02x %02x %02x %02x %02x %02x %02x\n",
-			       _ptr[0], _ptr[1], _ptr[2], _ptr[3], _ptr[4],
-			       _ptr[5], _ptr[6], _ptr[7], _ptr[8], _ptr[9],
-			       _ptr[10], _ptr[11], _ptr[12], _ptr[13], _ptr[14],
-			       _ptr[15]);
-	}
-}
-
-#define RTS51X_DUMP(buf, buf_len)	\
-		rts51x_dump((u8 *)(buf), (buf_len))
+#define RTS51X_DUMP(buf, buf_len)					\
+	print_hex_dump(KERN_DEBUG, RTS51X_TIP, DUMP_PREFIX_NONE,	\
+				16, 1, (buf), (buf_len), false)
 
 #define CATCH_TRIGGER(chip)					\
 do {								\
diff --git a/drivers/staging/rts_pstor/ms.c b/drivers/staging/rts_pstor/ms.c
index 7cc2b53f20d0..16a5c16fb6ab 100644
--- a/drivers/staging/rts_pstor/ms.c
+++ b/drivers/staging/rts_pstor/ms.c
@@ -109,9 +109,8 @@ static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode, u8 tpc, u16 s
 	u8 val, err_code = 0;
 	enum dma_data_direction dir;
 
-	if (!buf || !buf_len) {
+	if (!buf || !buf_len)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (trans_mode == MS_TM_AUTO_READ) {
 		dir = DMA_FROM_DEVICE;
@@ -151,18 +150,17 @@ static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode, u8 tpc, u16 s
 				    use_sg, dir, chip->mspro_timeout);
 	if (retval < 0) {
 		ms_set_err_code(chip, err_code);
-		if (retval == -ETIMEDOUT) {
+		if (retval == -ETIMEDOUT)
 			retval = STATUS_TIMEDOUT;
-		} else {
+		else
 			retval = STATUS_FAIL;
-		}
+
 		TRACE_RET(chip, retval);
 	}
 
 	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
-	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -173,9 +171,8 @@ 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)) {
+	if (!data || (data_len < cnt))
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	rtsx_init_cmd(chip);
 
@@ -183,9 +180,8 @@ static int ms_write_bytes(struct rtsx_chip *chip,
 		rtsx_add_cmd(chip, WRITE_REG_CMD,
 			     PPBUF_BASE2 + i, 0xFF, data[i]);
 	}
-	if (cnt % 2) {
+	if (cnt % 2)
 		rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF, 0xFF);
-	}
 
 	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
 	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
@@ -238,9 +234,8 @@ static int ms_read_bytes(struct rtsx_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *dat
 	int retval, i;
 	u8 *ptr;
 
-	if (!data) {
+	if (!data)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	rtsx_init_cmd(chip);
 
@@ -252,14 +247,13 @@ static int ms_read_bytes(struct rtsx_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *dat
 	rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF, MS_TRANSFER_START | MS_TM_READ_BYTES);
 	rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
 
-	for (i = 0; i < data_len - 1; i++) {
+	for (i = 0; i < data_len - 1; i++)
 	       rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
-	}
-	if (data_len % 2) {
+
+	if (data_len % 2)
 		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0, 0);
-	} else {
+	else
 		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1, 0, 0);
-	}
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 5000);
 	if (retval < 0) {
@@ -293,9 +287,8 @@ static int ms_read_bytes(struct rtsx_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *dat
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
 
-	for (i = 0; i < data_len; i++) {
+	for (i = 0; i < data_len; i++)
 		data[i] = ptr[i];
-	}
 
 	if ((tpc == PRO_READ_SHORT_DATA) && (data_len == 8)) {
 		RTSX_DEBUGP("Read format progress:\n");
@@ -343,34 +336,31 @@ static int ms_set_init_para(struct rtsx_chip *chip)
 	int retval;
 
 	if (CHK_HG8BIT(ms_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			ms_card->ms_clock = chip->asic_ms_hg_clk;
-		} else {
+		else
 			ms_card->ms_clock = chip->fpga_ms_hg_clk;
-		}
+
 	} else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			ms_card->ms_clock = chip->asic_ms_4bit_clk;
-		} else {
+		else
 			ms_card->ms_clock = chip->fpga_ms_4bit_clk;
-		}
+
 	} else {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			ms_card->ms_clock = chip->asic_ms_1bit_clk;
-		} else {
+		else
 			ms_card->ms_clock = chip->fpga_ms_1bit_clk;
-		}
 	}
 
 	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -381,14 +371,12 @@ static int ms_switch_clock(struct rtsx_chip *chip)
 	int retval;
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -461,9 +449,8 @@ static int ms_pull_ctl_enable(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0) {
+	if (retval < 0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -482,40 +469,37 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (!chip->ft2_fast_mode)
 		wait_timeout(250);
 
 	retval = enable_card_clock(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (chip->asic_code) {
 		retval = ms_pull_ctl_enable(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		RTSX_WRITE_REG(chip, FPGA_PULL_CTL, FPGA_MS_PULL_CTL_BIT | 0x20, 0);
 	}
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_on(chip, MS_CARD);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		wait_timeout(150);
 
 #ifdef SUPPORT_OCP
-		if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 			oc_mask = MS_OC_NOW | MS_OC_EVER;
-		} else {
+		else
 			oc_mask = SD_OC_NOW | SD_OC_EVER;
-		}
+
 		if (chip->ocp_stat & oc_mask) {
 			RTSX_DEBUGP("Over current, OCPSTAT is 0x%x\n",
 				     chip->ocp_stat);
@@ -539,9 +523,8 @@ static int ms_prepare_reset(struct rtsx_chip *chip)
 	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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -553,26 +536,23 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG, 6, NO_WAIT_INT);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		}
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, PPBUF_BASE2 + 2, &val);
 	RTSX_DEBUGP("Type register: 0x%x\n", val);
 	if (val != 0x01) {
-		if (val != 0x02) {
+		if (val != 0x02)
 			ms_card->check_ms_flow = 1;
-		}
+
 		TRACE_RET(chip, STATUS_FAIL);
 	}
 
@@ -587,11 +567,11 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 	RTSX_DEBUGP("Class register: 0x%x\n", val);
 	if (val == 0) {
 		RTSX_READ_REG(chip, PPBUF_BASE2, &val);
-		if (val & WRT_PRTCT) {
+		if (val & WRT_PRTCT)
 			chip->card_wp |= MS_CARD;
-		} else {
+		else
 			chip->card_wp &= ~MS_CARD;
-		}
+
 	} else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
 		chip->card_wp |= MS_CARD;
 	} else {
@@ -606,11 +586,11 @@ static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 	if (val == 0) {
 		ms_card->ms_type &= 0x0F;
 	} else if (val == 7) {
-		if (switch_8bit_bus) {
+		if (switch_8bit_bus)
 			ms_card->ms_type |= MS_HG;
-		} else {
+		else
 			ms_card->ms_type &= 0x0F;
-		}
+
 	} else {
 		TRACE_RET(chip, STATUS_FAIL);
 	}
@@ -633,17 +613,15 @@ static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 
 		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 			retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-			if (retval == STATUS_SUCCESS) {
+			if (retval == STATUS_SUCCESS)
 				break;
-			}
 		}
-		if (i == MS_MAX_RETRY_COUNT) {
+		if (i == MS_MAX_RETRY_COUNT)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
-		if (k > 100) {
+		if (k > 100)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		k++;
 		wait_timeout(100);
 	} while (!(val & INT_REG_CED));
@@ -653,16 +631,14 @@ static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (val & INT_REG_ERR) {
-		if (val & INT_REG_CMDNK) {
+		if (val & INT_REG_CMDNK)
 			chip->card_wp |= (MS_CARD);
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 	/* --  end confirm CPU startup */
 
@@ -681,9 +657,8 @@ static int ms_switch_parallel_bus(struct rtsx_chip *chip)
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -698,26 +673,22 @@ static int ms_switch_8bit_bus(struct rtsx_chip *chip)
 	data[1] = 0;
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		}
 	}
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_WRITE_REG(chip, MS_CFG, 0x98, MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
 	ms_card->ms_type |= MS_8BIT;
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -730,19 +701,16 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_identify_media_type(chip, switch_8bit_bus);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_confirm_cpu_startup(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_switch_parallel_bus(chip);
 		if (retval != STATUS_SUCCESS) {
@@ -756,18 +724,16 @@ static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
 		}
 	}
 
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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 = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
@@ -790,9 +756,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf[0] = 0;
 	buf[1] = mode;
@@ -802,19 +767,16 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, MS_TRANS_CFG, buf);
-	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR)) {
+	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -836,15 +798,14 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (CHK_MS8BIT(ms_card)) {
+	if (CHK_MS8BIT(ms_card))
 		data[0] = PARALLEL_8BIT_IF;
-	} else {
+	else
 		data[0] = PARALLEL_4BIT_IF;
-	}
+
 	data[1] = 0;
 
 	data[2] = 0x40;
@@ -856,24 +817,21 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT, data, 8);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		}
 	}
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf = kmalloc(64 * 512, GFP_KERNEL);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			continue;
-		}
+
 		retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
 		if (retval != STATUS_SUCCESS) {
 			kfree(buf);
@@ -885,11 +843,10 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 		}
 		retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
 				0x40, WAIT_INT, 0, 0, buf, 64 * 512);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		} else {
+		else
 			rtsx_clear_ms_error(chip);
-		}
 	}
 	if (retval != STATUS_SUCCESS) {
 		kfree(buf);
@@ -963,9 +920,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 			}
 
 #ifdef SUPPORT_MSXC
-			if (buf[cur_addr_off + 8] == 0x13) {
+			if (buf[cur_addr_off + 8] == 0x13)
 				ms_card->ms_type |= MS_XC;
-			}
 #endif
 #ifdef SUPPORT_PCGL_1P18
 			found_sys_info = 1;
@@ -1046,18 +1002,15 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_MSXC
 	if (CHK_MSXC(ms_card)) {
-		if (class_code != 0x03) {
+		if (class_code != 0x03)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
-		if (class_code != 0x02) {
+		if (class_code != 0x02)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 #else
-	if (class_code != 0x02) {
+	if (class_code != 0x02)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 #endif
 
 	if (device_type != 0x00) {
@@ -1069,9 +1022,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 		}
 	}
 
-	if (sub_class & 0xC0) {
+	if (sub_class & 0xC0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
 		class_code, device_type, sub_class);
@@ -1117,23 +1069,20 @@ Retry:
 	if (retval != STATUS_SUCCESS) {
 		if (ms_card->switch_8bit_fail) {
 			retval = ms_pro_reset_flow(chip, 0);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		} else {
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 	}
 
 	retval = ms_read_attribute_info(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 #ifdef XC_POWERCLASS
-	if (CHK_HG8BIT(ms_card)) {
+	if (CHK_HG8BIT(ms_card))
 		change_power_class = 0;
-	}
 
 	if (change_power_class && CHK_MSXC(ms_card)) {
 		u8 power_class_en = chip->ms_power_class_en;
@@ -1141,11 +1090,10 @@ Retry:
 		RTSX_DEBUGP("power_class_en = 0x%x\n", power_class_en);
 		RTSX_DEBUGP("change_power_class = %d\n", change_power_class);
 
-		if (change_power_class) {
+		if (change_power_class)
 			power_class_en &= (1 << (change_power_class - 1));
-		} else {
+		else
 			power_class_en = 0;
-		}
 
 		if (power_class_en) {
 			u8 power_class_mode = (ms_card->raw_sys_info[46] & 0x18) >> 3;
@@ -1165,16 +1113,14 @@ Retry:
 
 #ifdef SUPPORT_MAGIC_GATE
 	retval = mg_set_tpc_para_sub(chip, 0, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 #endif
 
-	if (CHK_HG8BIT(ms_card)) {
+	if (CHK_HG8BIT(ms_card))
 		chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
-	} else {
+	else
 		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1185,14 +1131,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
 		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
@@ -1211,9 +1155,8 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 	u8 val, data[10];
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHK_MS4BIT(ms_card)) {
 		/* Parallel interface */
@@ -1233,9 +1176,8 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
@@ -1244,15 +1186,14 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
 		TRACE_RET(chip, STATUS_FAIL);
@@ -1260,20 +1201,18 @@ static int ms_read_extra_data(struct rtsx_chip *chip,
 	if (val & INT_REG_CED) {
 		if (val & INT_REG_ERR) {
 			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 			retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 	}
 
 	retval = ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, data, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (buf && buf_len) {
 		if (buf_len > MS_EXTRA_SIZE)
@@ -1291,45 +1230,40 @@ static int ms_write_extra_data(struct rtsx_chip *chip,
 	int retval, i;
 	u8 val, data[16];
 
-	if (!buf || (buf_len < MS_EXTRA_SIZE)) {
+	if (!buf || (buf_len < MS_EXTRA_SIZE))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6 + MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(block_addr >> 8);
 	data[3] = (u8)block_addr;
 	data[4] = 0x40;
 	data[5] = page_num;
 
-	for (i = 6; i < MS_EXTRA_SIZE + 6; i++) {
+	for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
 		data[i] = buf[i - 6];
-	}
 
 	retval = ms_write_bytes(chip, WRITE_REG , (6+MS_EXTRA_SIZE), NO_WAIT_INT, data, 16);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
 		TRACE_RET(chip, STATUS_FAIL);
@@ -1352,15 +1286,14 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 	u8 val, data[6];
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(block_addr >> 8);
 	data[3] = (u8)block_addr;
@@ -1368,20 +1301,18 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
 		TRACE_RET(chip, STATUS_FAIL);
@@ -1394,9 +1325,9 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 				TRACE_RET(chip, STATUS_FAIL);
 			}
 			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				ms_set_err_code(chip,  MS_FLASH_WRITE_ERROR);
-			}
+
 		} else {
 			if (!(val & INT_REG_BREQ)) {
 				ms_set_err_code(chip, MS_BREQ_ERROR);
@@ -1406,13 +1337,11 @@ static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
 	}
 
 	retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA, 0, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
+	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1425,22 +1354,20 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(phy_blk >> 8);
 	data[3] = (u8)phy_blk;
@@ -1450,20 +1377,17 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (val & INT_REG_CMDNK) {
 		ms_set_err_code(chip, MS_CMD_NK);
@@ -1488,17 +1412,16 @@ static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
 	u8 val, data[6];
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(phy_blk >> 8);
 	data[3] = (u8)phy_blk;
@@ -1506,21 +1429,18 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 ERASE_RTY:
 	retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (val & INT_REG_CMDNK) {
 		if (i < 3) {
@@ -1546,9 +1466,8 @@ ERASE_RTY:
 
 static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
 {
-	if (!extra || (extra_len < MS_EXTRA_SIZE)) {
+	if (!extra || (extra_len < MS_EXTRA_SIZE))
 		return;
-	}
 
 	memset(extra, 0xFF, MS_EXTRA_SIZE);
 
@@ -1583,9 +1502,8 @@ static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk, u8 sta
 		}
 
 		retval = ms_write_extra_data(chip, phy_blk, i, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1603,27 +1521,23 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 	RTSX_DEBUGP("start_page = %d, end_page = %d\n", start_page, end_page);
 
 	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
 
 	if (val & BUF_FULL) {
 		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (!(val & INT_REG_CED)) {
 			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
@@ -1640,17 +1554,16 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 		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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 
-		if (CHK_MS4BIT(ms_card)) {
+		if (CHK_MS4BIT(ms_card))
 			data[0] = 0x88;
-		} else {
+		else
 			data[0] = 0x80;
-		}
+
 		data[1] = 0;
 		data[2] = (u8)(old_blk >> 8);
 		data[3] = (u8)old_blk;
@@ -1658,20 +1571,17 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 		data[5] = i;
 
 		retval = ms_write_bytes(chip, WRITE_REG , 6, NO_WAIT_INT, data, 6);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
@@ -1689,15 +1599,14 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 				}
 
 				retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA, 0, NO_WAIT_INT);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 
 				if (uncorrect_flag) {
 					ms_set_page_status(log_blk, setPS_NG, extra, MS_EXTRA_SIZE);
-					if (i == 0) {
+					if (i == 0)
 						extra[0] &= 0xEF;
-					}
+
 					ms_write_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
 					RTSX_DEBUGP("page %d : extra[0] = 0x%x\n", i, extra[0]);
 					MS_SET_BAD_BLOCK_FLG(ms_card);
@@ -1710,13 +1619,11 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 				for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT; rty_cnt++) {
 					retval = ms_transfer_tpc(chip, MS_TM_NORMAL_WRITE,
 							WRITE_PAGE_DATA, 0, NO_WAIT_INT);
-					if (retval == STATUS_SUCCESS) {
+					if (retval == STATUS_SUCCESS)
 						break;
-					}
 				}
-				if (rty_cnt == MS_MAX_RETRY_COUNT) {
+				if (rty_cnt == MS_MAX_RETRY_COUNT)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 
 			if (!(val & INT_REG_BREQ)) {
@@ -1730,45 +1637,41 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 		ms_set_err_code(chip, MS_NO_ERROR);
 
-		if (CHK_MS4BIT(ms_card)) {
+		if (CHK_MS4BIT(ms_card))
 			data[0] = 0x88;
-		} else {
+		else
 			data[0] = 0x80;
-		}
+
 		data[1] = 0;
 		data[2] = (u8)(new_blk >> 8);
 		data[3] = (u8)new_blk;
 		data[4] = 0x20;
 		data[5] = i;
 
-		if ((extra[0] & 0x60) != 0x60) {
+		if ((extra[0] & 0x60) != 0x60)
 			data[6] = extra[0];
-		} else {
+		else
 			data[6] = 0xF8;
-		}
+
 		data[6 + 1] = 0xFF;
 		data[6 + 2] = (u8)(log_blk >> 8);
 		data[6 + 3] = (u8)log_blk;
 
-		for (j = 4; j <= MS_EXTRA_SIZE; j++) {
+		for (j = 4; j <= MS_EXTRA_SIZE; j++)
 			data[6 + j] = 0xFF;
-		}
 
 		retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE), NO_WAIT_INT, data, 16);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
@@ -1784,17 +1687,16 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 
 		if (i == 0) {
 			retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 
 			ms_set_err_code(chip, MS_NO_ERROR);
 
-			if (CHK_MS4BIT(ms_card)) {
+			if (CHK_MS4BIT(ms_card))
 				data[0] = 0x88;
-			} else {
+			else
 				data[0] = 0x80;
-			}
+
 			data[1] = 0;
 			data[2] = (u8)(old_blk >> 8);
 			data[3] = (u8)old_blk;
@@ -1804,20 +1706,17 @@ static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 			data[7] = 0xFF;
 
 			retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 8);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 
 			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 
 			if (val & INT_REG_CMDNK) {
 				ms_set_err_code(chip, MS_CMD_NK);
@@ -1848,28 +1747,24 @@ static int reset_ms(struct rtsx_chip *chip)
 #endif
 
 	retval = ms_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_card->ms_type |= TYPE_MS;
 
 	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, PPBUF_BASE2, &val);
-	if (val & WRT_PRTCT) {
+	if (val & WRT_PRTCT)
 		chip->card_wp |= MS_CARD;
-	} else {
+	else
 		chip->card_wp &= ~MS_CARD;
-	}
 
 	i = 0;
 
@@ -1913,21 +1808,18 @@ RE_SEARCH:
 	}
 
 	retval = ms_read_page(chip, ms_card->boot_block, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	/* Read MS system information as sys_info */
 	rtsx_init_cmd(chip);
 
-	for (i = 0; i < 96; i++) {
+	for (i = 0; i < 96; i++)
 		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0, 0);
-	}
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0) {
+	if (retval < 0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ptr = rtsx_get_cmd_data(chip);
 	memcpy(ms_card->raw_sys_info, ptr, 96);
@@ -1938,21 +1830,18 @@ RE_SEARCH:
 	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
 	rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
 
-	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3; reg_addr++) {
+	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3; reg_addr++)
 		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-	}
 
-	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++) {
+	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
 		rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-	}
 
 	rtsx_add_cmd(chip, READ_REG_CMD, MS_Device_Type, 0, 0);
 	rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
 
 	retval = rtsx_send_cmd(chip, MS_CARD, 100);
-	if (retval < 0) {
+	if (retval < 0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ptr = rtsx_get_cmd_data(chip);
 
@@ -1975,9 +1864,8 @@ RE_SEARCH:
 		goto RE_SEARCH;
 	}
 
-	if ((ptr[14] == 1) || (ptr[14] == 3)) {
+	if ((ptr[14] == 1) || (ptr[14] == 3))
 		chip->card_wp |= MS_CARD;
-	}
 
 	/* BLOCK_SIZE_0, BLOCK_SIZE_1 */
 	block_size = ((u16)ptr[6] << 8) | ptr[7];
@@ -2026,17 +1914,15 @@ RE_SEARCH:
 	/* Switch I/F Mode */
 	if (ptr[15]) {
 		retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		RTSX_WRITE_REG(chip, PPBUF_BASE2, 0xFF, 0x88);
 		RTSX_WRITE_REG(chip, PPBUF_BASE2 + 1, 0xFF, 0);
 
 		retval = ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG , 1, NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		RTSX_WRITE_REG(chip, MS_CFG, 0x58 | MS_NO_CHECK_INT,
 				MS_BUS_WIDTH_4 | PUSH_TIME_ODD | MS_NO_CHECK_INT);
@@ -2044,11 +1930,10 @@ RE_SEARCH:
 		ms_card->ms_type |= MS_4BIT;
 	}
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-	} else {
+	else
 		chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2065,30 +1950,27 @@ 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) {
+	if (ms_card->segment == NULL)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_read_page(chip, ms_card->boot_block, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, INIT_FAIL);
-	}
 
 	reg_addr = PPBUF_BASE2;
 	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
 		retval = rtsx_read_register(chip, reg_addr++, &val1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, INIT_FAIL);
-		}
+
 		retval = rtsx_read_register(chip, reg_addr++, &val2);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, INIT_FAIL);
-		}
 
 		defect_block = ((u16)val1 << 8) | val2;
-		if (defect_block == 0xFFFF) {
+		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;
 	}
@@ -2141,9 +2023,8 @@ static void ms_set_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off, u16
 		return;
 
 	segment = &(ms_card->segment[seg_no]);
-	if (segment->l2p_table) {
+	if (segment->l2p_table)
 		segment->l2p_table[log_off] = phy_blk;
-	}
 }
 
 static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
@@ -2156,9 +2037,9 @@ static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
 	segment = &(ms_card->segment[seg_no]);
 
 	segment->free_table[segment->set_index++] = phy_blk;
-	if (segment->set_index >= MS_FREE_TABLE_CNT) {
+	if (segment->set_index >= MS_FREE_TABLE_CNT)
 		segment->set_index = 0;
-	}
+
 	segment->unused_blk_cnt++;
 }
 
@@ -2175,9 +2056,9 @@ static u16 ms_get_unused_block(struct rtsx_chip *chip, int seg_no)
 
 	phy_blk = segment->free_table[segment->get_index];
 	segment->free_table[segment->get_index++] = 0xFFFF;
-	if (segment->get_index >= MS_FREE_TABLE_CNT) {
+	if (segment->get_index >= MS_FREE_TABLE_CNT)
 		segment->get_index = 0;
-	}
+
 	segment->unused_blk_cnt--;
 
 	return phy_blk;
@@ -2199,27 +2080,27 @@ static int ms_arbitrate_l2p(struct rtsx_chip *chip, u16 phy_blk, u16 log_off, u8
 
 	if (us1 != us2) {
 		if (us1 == 0) {
-			if (!(chip->card_wp & MS_CARD)) {
+			if (!(chip->card_wp & MS_CARD))
 				ms_erase_block(chip, tmp_blk);
-			}
+
 			ms_set_unused_block(chip, tmp_blk);
 			segment->l2p_table[log_off] = phy_blk;
 		} else {
-			if (!(chip->card_wp & MS_CARD)) {
+			if (!(chip->card_wp & MS_CARD))
 				ms_erase_block(chip, phy_blk);
-			}
+
 			ms_set_unused_block(chip, phy_blk);
 		}
 	} else {
 		if (phy_blk < tmp_blk) {
-			if (!(chip->card_wp & MS_CARD)) {
+			if (!(chip->card_wp & MS_CARD))
 				ms_erase_block(chip, phy_blk);
-			}
+
 			ms_set_unused_block(chip, phy_blk);
 		} else {
-			if (!(chip->card_wp & MS_CARD)) {
+			if (!(chip->card_wp & MS_CARD))
 				ms_erase_block(chip, tmp_blk);
-			}
+
 			ms_set_unused_block(chip, tmp_blk);
 			segment->l2p_table[log_off] = phy_blk;
 		}
@@ -2240,9 +2121,8 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 
 	if (ms_card->segment == NULL) {
 		retval = ms_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, retval);
-		}
 	}
 
 	if (ms_card->segment[seg_no].build_flag) {
@@ -2250,27 +2130,24 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 		return STATUS_SUCCESS;
 	}
 
-	if (seg_no == 0) {
+	if (seg_no == 0)
 		table_size = 494;
-	} else {
+	else
 		table_size = 496;
-	}
 
 	segment = &(ms_card->segment[seg_no]);
 
 	if (segment->l2p_table == NULL) {
 		segment->l2p_table = (u16 *)vmalloc(table_size * 2);
-		if (segment->l2p_table == NULL) {
+		if (segment->l2p_table == NULL)
 			TRACE_GOTO(chip, BUILD_FAIL);
-		}
 	}
 	memset((u8 *)(segment->l2p_table), 0xff, table_size * 2);
 
 	if (segment->free_table == NULL) {
 		segment->free_table = (u16 *)vmalloc(MS_FREE_TABLE_CNT * 2);
-		if (segment->free_table == NULL) {
+		if (segment->free_table == NULL)
 			TRACE_GOTO(chip, BUILD_FAIL);
-		}
 	}
 	memset((u8 *)(segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
 
@@ -2368,13 +2245,11 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 
 	/* Logical Address Confirmation Process */
 	if (seg_no == ms_card->segment_cnt - 1) {
-		if (segment->unused_blk_cnt < 2) {
+		if (segment->unused_blk_cnt < 2)
 			chip->card_wp |= MS_CARD;
-		}
 	} else {
-		if (segment->unused_blk_cnt < 1) {
+		if (segment->unused_blk_cnt < 1)
 			chip->card_wp |= MS_CARD;
-		}
 	}
 
 	if (chip->card_wp & MS_CARD)
@@ -2388,9 +2263,9 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 				return STATUS_SUCCESS;
 			}
 			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_GOTO(chip, BUILD_FAIL);
-			}
+
 			segment->l2p_table[log_blk-ms_start_idx[seg_no]] = phy_blk;
 			if (seg_no == ms_card->segment_cnt - 1) {
 				if (segment->unused_blk_cnt < 2) {
@@ -2419,16 +2294,14 @@ 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) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 
 				segment->l2p_table[log_blk] = phy_blk;
 
 				retval = ms_set_bad_block(chip, tmp_blk);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 		}
 	}
@@ -2458,14 +2331,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_card->ms_type = 0;
 
@@ -2473,27 +2344,24 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		} else {
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 	}
 
 	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	RTSX_DEBUGP("ms_card->ms_type = 0x%x\n", ms_card->ms_type);
@@ -2520,9 +2388,8 @@ static int mspro_set_rw_cmd(struct rtsx_chip *chip, u32 start_sec, u16 sec_cnt,
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2556,21 +2423,18 @@ static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
 	RTSX_DEBUGP("--%s--\n", __func__);
 
 	if (chip->asic_code) {
-		if (ms_card->ms_clock > 30) {
+		if (ms_card->ms_clock > 30)
 			ms_card->ms_clock -= 20;
-		}
 	} else {
-		if (ms_card->ms_clock == CLK_80) {
+		if (ms_card->ms_clock == CLK_80)
 			ms_card->ms_clock = CLK_60;
-		} else if (ms_card->ms_clock == CLK_60) {
+		else if (ms_card->ms_clock == CLK_60)
 			ms_card->ms_clock = CLK_40;
-		}
 	}
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2616,15 +2480,13 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	}
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
 		trans_mode = MS_TM_AUTO_READ;
-	} else {
+	else
 		trans_mode = MS_TM_AUTO_WRITE;
-	}
 
 	RTSX_READ_REG(chip, MS_TRANS_CFG, &val);
 
@@ -2639,9 +2501,8 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			ms_card->total_sec_cnt = 0;
 			if (val & MS_INT_BREQ) {
 				retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 
 				rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
 			}
@@ -2651,17 +2512,16 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 	if (!ms_card->seq_mode) {
 		ms_card->total_sec_cnt = 0;
 		if (sector_cnt >= SEQ_START_CRITERIA) {
-			if ((ms_card->capacity - start_sector) > 0xFE00) {
+			if ((ms_card->capacity - start_sector) > 0xFE00)
 				count = 0xFE00;
-			} else {
+			else
 				count = (u16)(ms_card->capacity - start_sector);
-			}
+
 			if (count > sector_cnt) {
-				if (mode_2k) {
+				if (mode_2k)
 					ms_card->seq_mode |= MODE_2K_SEQ;
-				} else {
+				else
 					ms_card->seq_mode |= MODE_512_SEQ;
-				}
 			}
 		} else {
 			count = sector_cnt;
@@ -2686,9 +2546,9 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 
-		if (val & MS_INT_BREQ) {
+		if (val & MS_INT_BREQ)
 			ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-		}
+
 		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
 			RTSX_DEBUGP("MSPro CRC error, tune clock!\n");
 			chip->rw_need_retry = 1;
@@ -2739,11 +2599,10 @@ static int mspro_read_format_progress(struct rtsx_chip *chip, const int short_da
 		TRACE_RET(chip, STATUS_FAIL);
 	}
 
-	if (short_data_len >= 256) {
+	if (short_data_len >= 256)
 		cnt = 0;
-	} else {
+	else
 		cnt = (u8)short_data_len;
-	}
 
 	retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, MS_NO_CHECK_INT);
 	if (retval != STATUS_SUCCESS) {
@@ -2778,9 +2637,8 @@ static int mspro_read_format_progress(struct rtsx_chip *chip, const int short_da
 			ms_card->format_status = FORMAT_FAIL;
 			TRACE_RET(chip, STATUS_FAIL);
 		}
-		if (tmp & (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR)) {
+		if (tmp & (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR))
 			break;
-		}
 
 		wait_timeout(1);
 	}
@@ -2843,14 +2701,12 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip, int short_data_l
 	RTSX_DEBUGP("--%s--\n", __func__);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	memset(buf, 0, 2);
 	switch (short_data_len) {
@@ -2874,25 +2730,22 @@ int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip, int short_data_l
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (quick_format) {
+	if (quick_format)
 		para = 0x0000;
-	} else {
+	else
 		para = 0x0001;
-	}
+
 	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, MS_TRANS_CFG, &tmp);
 
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
+	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
 		ms_card->pro_under_formatting = 1;
@@ -2930,15 +2783,14 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk, u16 log_b
 	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(phy_blk >> 8);
 	data[3] = (u8)phy_blk;
@@ -2950,16 +2802,14 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk, u16 log_b
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
 	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ptr = buf;
 
@@ -2972,9 +2822,9 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk, u16 log_b
 		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		if (val & INT_REG_CMDNK) {
 			ms_set_err_code(chip, MS_CMD_NK);
 			TRACE_RET(chip, STATUS_FAIL);
@@ -3006,15 +2856,14 @@ static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk, u16 log_b
 		if (page_addr == (end_page - 1)) {
 			if (!(val & INT_REG_CED)) {
 				retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 
 			retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 			if (!(val & INT_REG_CED)) {
 				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
 				TRACE_RET(chip, STATUS_FAIL);
@@ -3079,15 +2928,14 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk, u16 new_
 
 	if (!start_page) {
 		retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
-		if (CHK_MS4BIT(ms_card)) {
+		if (CHK_MS4BIT(ms_card))
 			data[0] = 0x88;
-		} else {
+		else
 			data[0] = 0x80;
-		}
+
 		data[1] = 0;
 		data[2] = (u8)(old_blk >> 8);
 		data[3] = (u8)old_blk;
@@ -3097,74 +2945,66 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk, u16 new_
 		data[7] = 0xFF;
 
 		retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 8);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ms_set_err_code(chip, MS_NO_ERROR);
 
-	if (CHK_MS4BIT(ms_card)) {
+	if (CHK_MS4BIT(ms_card))
 		data[0] = 0x88;
-	} else {
+	else
 		data[0] = 0x80;
-	}
+
 	data[1] = 0;
 	data[2] = (u8)(new_blk >> 8);
 	data[3] = (u8)new_blk;
-	if ((end_page - start_page) == 1) {
+	if ((end_page - start_page) == 1)
 		data[4] = 0x20;
-	} else {
+	else
 		data[4] = 0;
-	}
+
 	data[5] = start_page;
 	data[6] = 0xF8;
 	data[7] = 0xFF;
 	data[8] = (u8)(log_blk >> 8);
 	data[9] = (u8)log_blk;
 
-	for (i = 0x0A; i < 0x10; i++) {
+	for (i = 0x0A; i < 0x10; i++)
 		data[i] = 0xFF;
-	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE, NO_WAIT_INT, data, 16);
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
 		if (retval == STATUS_SUCCESS)
 			break;
 	}
-	if (i == MS_MAX_RETRY_COUNT) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	ptr = buf;
 	for (page_addr = start_page; page_addr < end_page; page_addr++) {
@@ -3210,17 +3050,15 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk, u16 new_
 			ms_set_err_code(chip, MS_TO_ERROR);
 			rtsx_clear_ms_error(chip);
 
-			if (retval == -ETIMEDOUT) {
+			if (retval == -ETIMEDOUT)
 				TRACE_RET(chip, STATUS_TIMEDOUT);
-			} else {
+			else
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 
 		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if ((end_page - start_page) == 1) {
 			if (!(val & INT_REG_CED)) {
@@ -3231,15 +3069,13 @@ static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk, u16 new_
 			if (page_addr == (end_page - 1)) {
 				if (!(val & INT_REG_CED)) {
 					retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
-					if (retval != STATUS_SUCCESS) {
+					if (retval != STATUS_SUCCESS)
 						TRACE_RET(chip, STATUS_FAIL);
-					}
 				}
 
 				retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 
 			if ((page_addr == (end_page - 1)) || (page_addr == ms_card->page_off)) {
@@ -3266,9 +3102,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	seg_no = old_blk >> 9;
 
@@ -3277,9 +3112,8 @@ static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
 		ms_set_bad_block(chip, old_blk);
 	} else {
 		retval = ms_erase_block(chip, old_blk);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			ms_set_unused_block(chip, old_blk);
-		}
 	}
 
 	ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
@@ -3294,9 +3128,8 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -3311,17 +3144,15 @@ int ms_delay_write(struct rtsx_chip *chip)
 
 	if (delay_write->delay_write_flag) {
 		retval = ms_set_init_para(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		delay_write->delay_write_flag = 0;
 		retval = ms_finish_write(chip,
 				delay_write->old_phyblock, delay_write->new_phyblock,
 				delay_write->logblock, delay_write->pageoff);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -3330,11 +3161,10 @@ int ms_delay_write(struct rtsx_chip *chip)
 
 static inline void ms_rw_fail(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 {
-	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	} else {
+	else
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-	}
 }
 
 static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 sector_cnt)
@@ -3449,11 +3279,11 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32
 	RTSX_DEBUGP("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n", seg_no, old_blk, new_blk);
 
 	while (total_sec_cnt) {
-		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1)) {
+		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
 			end_page = ms_card->page_off + 1;
-		} else {
+		else
 			end_page = start_page + (u8)total_sec_cnt;
-		}
+
 		page_cnt = end_page - start_page;
 
 		RTSX_DEBUGP("start_page = %d, end_page = %d, page_cnt = %d\n",
@@ -3482,9 +3312,9 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32
 		if (srb->sc_data_direction == DMA_TO_DEVICE) {
 			if (end_page == (ms_card->page_off + 1)) {
 				retval = ms_erase_block(chip, old_blk);
-				if (retval == STATUS_SUCCESS) {
+				if (retval == STATUS_SUCCESS)
 					ms_set_unused_block(chip, old_blk);
-				}
+
 				ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
 			}
 		}
@@ -3565,11 +3395,10 @@ int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 	struct ms_info *ms_card = &(chip->ms_card);
 	int retval;
 
-	if (CHK_MSPRO(ms_card)) {
+	if (CHK_MSPRO(ms_card))
 		retval = mspro_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-	} else {
+	else
 		retval = ms_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-	}
 
 	return retval;
 }
@@ -3609,14 +3438,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	val = *rtsx_get_cmd_data(chip);
-	if (val & MS_INT_ERR) {
+	if (val & MS_INT_ERR)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -3678,9 +3505,8 @@ 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) {
+	if (i == MS_MAX_RETRY_COUNT)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (check_ms_err(chip)) {
 		rtsx_clear_ms_error(chip);
@@ -3697,14 +3523,13 @@ static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type, u8 mg_entry_num
 
 	RTSX_DEBUGP("--%s--\n", __func__);
 
-	if (type == 0) {
+	if (type == 0)
 		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_TPCParm, 1);
-	} else {
+	else
 		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	}
-	if (retval != STATUS_SUCCESS) {
+
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf[0] = 0;
 	buf[1] = 0;
@@ -3715,9 +3540,8 @@ static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type, u8 mg_entry_num
 		buf[5] = mg_entry_num;
 	}
 	retval = ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6, NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -3739,9 +3563,8 @@ int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
 	if (retval != STATUS_SUCCESS) {
@@ -3751,9 +3574,9 @@ int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	memset(buf1, 0, 32);
 	rtsx_stor_get_xfer_buf(buf2, min(12, (int)scsi_bufflen(srb)), srb);
-	for (i = 0; i < 8; i++) {
+	for (i = 0; i < 8; i++)
 		buf1[8+i] = buf2[4+i];
-	}
+
 	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf1, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
@@ -3780,14 +3603,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf = kmalloc(1540, GFP_KERNEL);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	buf[0] = 0x04;
 	buf[1] = 0x1A;
@@ -3835,9 +3656,8 @@ int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
 	if (retval != STATUS_SUCCESS) {
@@ -3875,12 +3695,12 @@ int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	bufflen = min(12, (int)scsi_bufflen(srb));
 	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
 
-	for (i = 0; i < 8; i++) {
+	for (i = 0; i < 8; i++)
 		buf[i] = buf[4+i];
-	}
-	for (i = 0; i < 24; i++) {
+
+	for (i = 0; i < 24; i++)
 		buf[8+i] = 0;
-	}
+
 	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA,
 				32, WAIT_INT, buf, 32);
 	if (retval != STATUS_SUCCESS) {
@@ -3911,9 +3731,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
 	if (retval != STATUS_SUCCESS) {
@@ -3968,9 +3787,8 @@ int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	ms_cleanup_work(chip);
 
 	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
 	if (retval != STATUS_SUCCESS) {
@@ -3981,12 +3799,12 @@ int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	bufflen = min(12, (int)scsi_bufflen(srb));
 	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
 
-	for (i = 0; i < 8; i++) {
+	for (i = 0; i < 8; i++)
 		buf[i] = buf[4+i];
-	}
-	for (i = 0; i < 24; i++) {
+
+	for (i = 0; i < 24; i++)
 		buf[8+i] = 0;
-	}
+
 	retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
 	if (retval != STATUS_SUCCESS) {
 		set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
@@ -4016,14 +3834,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	buf[0] = 0x04;
 	buf[1] = 0x02;
@@ -4073,14 +3889,12 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	bufflen = min(1028, (int)scsi_bufflen(srb));
 	rtsx_stor_get_xfer_buf(buf, bufflen, srb);
@@ -4088,11 +3902,10 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
 	if (retval != STATUS_SUCCESS) {
 		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0) {
+			if ((buf[5] & 0xC0) != 0)
 				set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			} else {
+			else
 				set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-			}
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
 		}
@@ -4121,11 +3934,10 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if ((retval < 0) || check_ms_err(chip)) {
 			rtsx_clear_ms_error(chip);
 			if (ms_card->mg_auth == 0) {
-				if ((buf[5] & 0xC0) != 0) {
+				if ((buf[5] & 0xC0) != 0)
 					set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-				} else {
+				else
 					set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-				}
 			} else {
 				set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
 			}
@@ -4139,11 +3951,10 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if ((retval != STATUS_SUCCESS) || check_ms_err(chip)) {
 		rtsx_clear_ms_error(chip);
 		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0) {
+			if ((buf[5] & 0xC0) != 0)
 				set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			} else {
+			else
 				set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-			}
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
 		}
@@ -4187,14 +3998,13 @@ int ms_power_off_card3v3(struct rtsx_chip *chip)
 	int retval;
 
 	retval = disable_card_clock(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (chip->asic_code) {
 		retval = ms_pull_ctl_disable(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
 			FPGA_MS_PULL_CTL_BIT | 0x20, FPGA_MS_PULL_CTL_BIT);
@@ -4202,9 +4012,8 @@ int ms_power_off_card3v3(struct rtsx_chip *chip)
 	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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -4234,9 +4043,8 @@ int release_ms_card(struct rtsx_chip *chip)
 #endif
 
 	retval = ms_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rts_pstor/rtsx.c b/drivers/staging/rts_pstor/rtsx.c
index 5fb05a2edebb..afe9c2e763d7 100644
--- a/drivers/staging/rts_pstor/rtsx.c
+++ b/drivers/staging/rts_pstor/rtsx.c
@@ -20,6 +20,8 @@
  *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/blkdev.h>
 #include <linux/kthread.h>
 #include <linux/sched.h>
@@ -170,14 +172,14 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 
 	/* check for state-transition errors */
 	if (chip->srb != NULL) {
-		printk(KERN_ERR "Error in %s: chip->srb = %p\n",
+		dev_err(&dev->pci->dev, "Error in %s: chip->srb = %p\n",
 			__func__, chip->srb);
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
 
 	/* fail the command if we are disconnecting */
 	if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
-		printk(KERN_INFO "Fail command during disconnect\n");
+		dev_info(&dev->pci->dev, "Fail command during disconnect\n");
 		srb->result = DID_NO_CONNECT << 16;
 		done(srb);
 		return 0;
@@ -204,14 +206,14 @@ static int command_abort(struct scsi_cmnd *srb)
 	struct rtsx_dev *dev = host_to_rtsx(host);
 	struct rtsx_chip *chip = dev->chip;
 
-	printk(KERN_INFO "%s called\n", __func__);
+	dev_info(&dev->pci->dev, "%s called\n", __func__);
 
 	scsi_lock(host);
 
 	/* Is this command still active? */
 	if (chip->srb != srb) {
 		scsi_unlock(host);
-		printk(KERN_INFO "-- nothing to abort\n");
+		dev_info(&dev->pci->dev, "-- nothing to abort\n");
 		return FAILED;
 	}
 
@@ -230,8 +232,9 @@ static int command_abort(struct scsi_cmnd *srb)
 static int device_reset(struct scsi_cmnd *srb)
 {
 	int result = 0;
+	struct rtsx_dev *dev = host_to_rtsx(srb->device->host);
 
-	printk(KERN_INFO "%s called\n", __func__);
+	dev_info(&dev->pci->dev, "%s called\n", __func__);
 
 	return result < 0 ? FAILED : SUCCESS;
 }
@@ -240,8 +243,9 @@ static int device_reset(struct scsi_cmnd *srb)
 static int bus_reset(struct scsi_cmnd *srb)
 {
 	int result = 0;
+	struct rtsx_dev *dev = host_to_rtsx(srb->device->host);
 
-	printk(KERN_INFO "%s called\n", __func__);
+	dev_info(&dev->pci->dev, "%s called\n", __func__);
 
 	return result < 0 ? FAILED : SUCCESS;
 }
@@ -303,14 +307,15 @@ static int rtsx_acquire_irq(struct rtsx_dev *dev)
 {
 	struct rtsx_chip *chip = dev->chip;
 
-	printk(KERN_INFO "%s: chip->msi_en = %d, pci->irq = %d\n",
-			__func__, chip->msi_en, dev->pci->irq);
+	dev_info(&dev->pci->dev, "%s: chip->msi_en = %d, pci->irq = %d\n",
+		 __func__, chip->msi_en, dev->pci->irq);
 
 	if (request_irq(dev->pci->irq, rtsx_interrupt,
 			chip->msi_en ? 0 : IRQF_SHARED,
 			CR_DRIVER_NAME, dev)) {
-		printk(KERN_ERR "rtsx: unable to grab IRQ %d, "
-		       "disabling device\n", dev->pci->irq);
+		dev_err(&dev->pci->dev,
+			"rtsx: unable to grab IRQ %d, disabling device\n",
+			dev->pci->irq);
 		return -1;
 	}
 
@@ -347,12 +352,8 @@ static int rtsx_suspend(struct pci_dev *pci, pm_message_t state)
 	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
-	printk(KERN_INFO "Ready to suspend\n");
-
-	if (!dev) {
-		printk(KERN_ERR "Invalid memory\n");
+	if (!dev)
 		return 0;
-	}
 
 	/* lock the device pointers */
 	mutex_lock(&(dev->dev_mutex));
@@ -386,12 +387,8 @@ static int rtsx_resume(struct pci_dev *pci)
 	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
-	printk(KERN_INFO "Ready to resume\n");
-
-	if (!dev) {
-		printk(KERN_ERR "Invalid memory\n");
+	if (!dev)
 		return 0;
-	}
 
 	chip = dev->chip;
 
@@ -401,8 +398,9 @@ static int rtsx_resume(struct pci_dev *pci)
 	pci_set_power_state(pci, PCI_D0);
 	pci_restore_state(pci);
 	if (pci_enable_device(pci) < 0) {
-		printk(KERN_ERR "%s: pci_enable_device failed, "
-		       "disabling device\n", CR_DRIVER_NAME);
+		dev_err(&dev->pci->dev,
+			"%s: pci_enable_device failed, disabling device\n",
+			CR_DRIVER_NAME);
 		/* unlock the device pointers */
 		mutex_unlock(&dev->dev_mutex);
 		return -EIO;
@@ -435,12 +433,8 @@ static void rtsx_shutdown(struct pci_dev *pci)
 	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
-	printk(KERN_INFO "Ready to shutdown\n");
-
-	if (!dev) {
-		printk(KERN_ERR "Invalid memory\n");
+	if (!dev)
 		return;
-	}
 
 	chip = dev->chip;
 
@@ -475,7 +469,7 @@ static int rtsx_control_thread(void *__dev)
 
 		/* if the device has disconnected, we are free to exit */
 		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
-			printk(KERN_INFO "-- rtsx-control exiting\n");
+			dev_info(&dev->pci->dev, "-- rtsx-control exiting\n");
 			mutex_unlock(&dev->dev_mutex);
 			break;
 		}
@@ -495,7 +489,7 @@ static int rtsx_control_thread(void *__dev)
 		 * is UNKNOWN
 		 */
 		if (chip->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
-			printk(KERN_ERR "UNKNOWN data direction\n");
+			dev_err(&dev->pci->dev, "UNKNOWN data direction\n");
 			chip->srb->result = DID_ERROR << 16;
 		}
 
@@ -503,14 +497,14 @@ static int rtsx_control_thread(void *__dev)
 		 * the maximum known LUN
 		 */
 		else if (chip->srb->device->id) {
-			printk(KERN_ERR "Bad target number (%d:%d)\n",
+			dev_err(&dev->pci->dev, "Bad target number (%d:%d)\n",
 				chip->srb->device->id,
 				chip->srb->device->lun);
 			chip->srb->result = DID_BAD_TARGET << 16;
 		}
 
 		else if (chip->srb->device->lun > chip->max_lun) {
-			printk(KERN_ERR "Bad LUN (%d:%d)\n",
+			dev_err(&dev->pci->dev, "Bad LUN (%d:%d)\n",
 				chip->srb->device->id,
 				chip->srb->device->lun);
 			chip->srb->result = DID_BAD_TARGET << 16;
@@ -534,7 +528,7 @@ static int rtsx_control_thread(void *__dev)
 			chip->srb->scsi_done(chip->srb);
 		} else {
 SkipForAbort:
-			printk(KERN_ERR "scsi command aborted\n");
+			dev_err(&dev->pci->dev, "scsi command aborted\n");
 		}
 
 		if (rtsx_chk_stat(chip, RTSX_STAT_ABORT)) {
@@ -594,7 +588,7 @@ static int rtsx_polling_thread(void *__dev)
 
 		/* if the device has disconnected, we are free to exit */
 		if (rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
-			printk(KERN_INFO "-- rtsx-polling exiting\n");
+			dev_info(&dev->pci->dev, "-- rtsx-polling exiting\n");
 			mutex_unlock(&dev->dev_mutex);
 			break;
 		}
@@ -683,13 +677,13 @@ Exit:
 /* Release all our dynamic resources */
 static void rtsx_release_resources(struct rtsx_dev *dev)
 {
-	printk(KERN_INFO "-- %s\n", __func__);
+	dev_info(&dev->pci->dev, "-- %s\n", __func__);
 
 	/* Tell the control thread to exit.  The SCSI host must
 	 * already have been removed so it won't try to queue
 	 * any more commands.
 	 */
-	printk(KERN_INFO "-- sending exit command to thread\n");
+	dev_info(&dev->pci->dev, "-- sending exit command to thread\n");
 	complete(&dev->cmnd_ready);
 	if (dev->ctl_thread)
 		wait_for_completion(&dev->control_exit);
@@ -774,8 +768,9 @@ static int rtsx_scan_thread(void *__dev)
 
 	/* Wait for the timeout to expire or for a disconnect */
 	if (delay_use > 0) {
-		printk(KERN_INFO "%s: waiting for device "
-				"to settle before scanning\n", CR_DRIVER_NAME);
+		dev_info(&dev->pci->dev,
+			 "%s: waiting for device to settle before scanning\n",
+			 CR_DRIVER_NAME);
 		wait_event_interruptible_timeout(dev->delay_wait,
 				rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT),
 				delay_use * HZ);
@@ -784,7 +779,8 @@ static int rtsx_scan_thread(void *__dev)
 	/* If the device is still connected, perform the scanning */
 	if (!rtsx_chk_stat(chip, RTSX_STAT_DISCONNECT)) {
 		scsi_scan_host(rtsx_to_host(dev));
-		printk(KERN_INFO "%s: device scan complete\n", CR_DRIVER_NAME);
+		dev_info(&dev->pci->dev, "%s: device scan complete\n",
+			 CR_DRIVER_NAME);
 
 		/* Should we unbind if no devices were detected? */
 	}
@@ -906,14 +902,14 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 
 	err = pci_enable_device(pci);
 	if (err < 0) {
-		printk(KERN_ERR "PCI enable device failed!\n");
+		dev_err(&pci->dev, "PCI enable device failed!\n");
 		return err;
 	}
 
 	err = pci_request_regions(pci, CR_DRIVER_NAME);
 	if (err < 0) {
-		printk(KERN_ERR "PCI request regions for %s failed!\n",
-		       CR_DRIVER_NAME);
+		dev_err(&pci->dev, "PCI request regions for %s failed!\n",
+			CR_DRIVER_NAME);
 		pci_disable_device(pci);
 		return err;
 	}
@@ -924,7 +920,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	 */
 	host = scsi_host_alloc(&rtsx_host_template, sizeof(*dev));
 	if (!host) {
-		printk(KERN_ERR "Unable to allocate the scsi host\n");
+		dev_err(&pci->dev, "Unable to allocate the scsi host\n");
 		pci_release_regions(pci);
 		pci_disable_device(pci);
 		return -ENOMEM;
@@ -949,12 +945,12 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	dev->pci = pci;
 	dev->irq = -1;
 
-	printk(KERN_INFO "Resource length: 0x%x\n",
-	       (unsigned int)pci_resource_len(pci, 0));
+	dev_info(&pci->dev, "Resource length: 0x%x\n",
+		 (unsigned int)pci_resource_len(pci, 0));
 	dev->addr = pci_resource_start(pci, 0);
 	dev->remap_addr = ioremap_nocache(dev->addr, pci_resource_len(pci, 0));
 	if (dev->remap_addr == NULL) {
-		printk(KERN_ERR "ioremap error\n");
+		dev_err(&pci->dev, "ioremap error\n");
 		err = -ENXIO;
 		goto errout;
 	}
@@ -963,13 +959,13 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	 * Using "unsigned long" cast here to eliminate gcc warning in
 	 * 64-bit system
 	 */
-	printk(KERN_INFO "Original address: 0x%lx, remapped address: 0x%lx\n",
-	       (unsigned long)(dev->addr), (unsigned long)(dev->remap_addr));
+	dev_info(&pci->dev, "Original address: 0x%lx, remapped address: 0x%lx\n",
+		 (unsigned long)(dev->addr), (unsigned long)(dev->remap_addr));
 
 	dev->rtsx_resv_buf = dma_alloc_coherent(&(pci->dev), RTSX_RESV_BUF_LEN,
 			&(dev->rtsx_resv_buf_addr), GFP_KERNEL);
 	if (dev->rtsx_resv_buf == NULL) {
-		printk(KERN_ERR "alloc dma buffer fail\n");
+		dev_err(&pci->dev, "alloc dma buffer fail\n");
 		err = -ENXIO;
 		goto errout;
 	}
@@ -983,7 +979,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 
 	rtsx_init_options(dev->chip);
 
-	printk(KERN_INFO "pci->irq = %d\n", pci->irq);
+	dev_info(&pci->dev, "pci->irq = %d\n", pci->irq);
 
 	if (dev->chip->msi_en) {
 		if (pci_enable_msi(pci) < 0)
@@ -1008,7 +1004,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	/* Start up our control thread */
 	th = kthread_run(rtsx_control_thread, dev, CR_DRIVER_NAME);
 	if (IS_ERR(th)) {
-		printk(KERN_ERR "Unable to start control thread\n");
+		dev_err(&pci->dev, "Unable to start control thread\n");
 		err = PTR_ERR(th);
 		goto errout;
 	}
@@ -1016,14 +1012,14 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 
 	err = scsi_add_host(host, &pci->dev);
 	if (err) {
-		printk(KERN_ERR "Unable to add the scsi host\n");
+		dev_err(&pci->dev, "Unable to add the scsi host\n");
 		goto errout;
 	}
 
 	/* Start up the thread for delayed SCSI-device scanning */
 	th = kthread_run(rtsx_scan_thread, dev, "rtsx-scan");
 	if (IS_ERR(th)) {
-		printk(KERN_ERR "Unable to start the device-scanning thread\n");
+		dev_err(&pci->dev, "Unable to start the device-scanning thread\n");
 		complete(&dev->scanning_done);
 		quiesce_and_remove_host(dev);
 		err = PTR_ERR(th);
@@ -1033,7 +1029,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	/* Start up the thread for polling thread */
 	th = kthread_run(rtsx_polling_thread, dev, "rtsx-polling");
 	if (IS_ERR(th)) {
-		printk(KERN_ERR "Unable to start the device-polling thread\n");
+		dev_err(&pci->dev, "Unable to start the device-polling thread\n");
 		quiesce_and_remove_host(dev);
 		err = PTR_ERR(th);
 		goto errout;
@@ -1046,7 +1042,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 
 	/* We come here if there are any problems */
 errout:
-	printk(KERN_ERR "rtsx_probe() failed\n");
+	dev_err(&pci->dev, "rtsx_probe() failed\n");
 	release_everything(dev);
 
 	return err;
@@ -1057,7 +1053,7 @@ static void __devexit rtsx_remove(struct pci_dev *pci)
 {
 	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
 
-	printk(KERN_INFO "rtsx_remove() called\n");
+	dev_info(&pci->dev, "rtsx_remove() called\n");
 
 	quiesce_and_remove_host(dev);
 	release_everything(dev);
@@ -1090,18 +1086,18 @@ static struct pci_driver driver = {
 
 static int __init rtsx_init(void)
 {
-	printk(KERN_INFO "Initializing Realtek PCIE storage driver...\n");
+	pr_info("Initializing Realtek PCIE storage driver...\n");
 
 	return pci_register_driver(&driver);
 }
 
 static void __exit rtsx_exit(void)
 {
-	printk(KERN_INFO "rtsx_exit() called\n");
+	pr_info("rtsx_exit() called\n");
 
 	pci_unregister_driver(&driver);
 
-	printk(KERN_INFO "%s module exit\n", CR_DRIVER_NAME);
+	pr_info("%s module exit\n", CR_DRIVER_NAME);
 }
 
 module_init(rtsx_init)
diff --git a/drivers/staging/rts_pstor/rtsx_card.c b/drivers/staging/rts_pstor/rtsx_card.c
index 4f971f2e930a..539aa6a27788 100644
--- a/drivers/staging/rts_pstor/rtsx_card.c
+++ b/drivers/staging/rts_pstor/rtsx_card.c
@@ -309,11 +309,10 @@ static void release_sdio(struct rtsx_chip *chip)
 		if (chip->chip_insert_with_sdio) {
 			chip->chip_insert_with_sdio = 0;
 
-			if (CHECK_PID(chip, 0x5288)) {
+			if (CHECK_PID(chip, 0x5288))
 				rtsx_write_register(chip, 0xFE5A, 0x08, 0x00);
-			} else {
+			else
 				rtsx_write_register(chip, 0xFE70, 0x80, 0x00);
-			}
 		}
 
 		rtsx_write_register(chip, SDIO_CTRL, SDIO_CD_CTRL, 0);
@@ -379,11 +378,10 @@ void rtsx_reset_cards(struct rtsx_chip *chip)
 	if (chip->need_reset & XD_CARD) {
 		chip->card_exist |= XD_CARD;
 
-		if (chip->xd_show_cnt >= MAX_SHOW_CNT) {
+		if (chip->xd_show_cnt >= MAX_SHOW_CNT)
 			do_reset_xd_card(chip);
-		} else {
+		else
 			chip->xd_show_cnt++;
-		}
 	}
 	if (CHECK_PID(chip, 0x5288) && CHECK_BARO_PKG(chip, QFN)) {
 		if (chip->card_exist & XD_CARD) {
@@ -404,11 +402,10 @@ void rtsx_reset_cards(struct rtsx_chip *chip)
 	if (chip->need_reset & MS_CARD) {
 		chip->card_exist |= MS_CARD;
 
-		if (chip->ms_show_cnt >= MAX_SHOW_CNT) {
+		if (chip->ms_show_cnt >= MAX_SHOW_CNT)
 			do_reset_ms_card(chip);
-		} else {
+		else
 			chip->ms_show_cnt++;
-		}
 	}
 }
 
@@ -473,13 +470,12 @@ void card_cd_debounce(struct rtsx_chip *chip, unsigned long *need_reset, unsigne
 				release_map |= MS_CARD;
 		}
 	} else {
-		if (chip->int_reg & XD_EXIST) {
+		if (chip->int_reg & XD_EXIST)
 			reset_map |= XD_CARD;
-		} else if (chip->int_reg & SD_EXIST) {
+		else if (chip->int_reg & SD_EXIST)
 			reset_map |= SD_CARD;
-		} else if (chip->int_reg & MS_EXIST) {
+		else if (chip->int_reg & MS_EXIST)
 			reset_map |= MS_CARD;
-		}
 	}
 
 	if (reset_map) {
@@ -489,21 +485,21 @@ void card_cd_debounce(struct rtsx_chip *chip, unsigned long *need_reset, unsigne
 		for (i = 0; i < (DEBOUNCE_CNT); i++) {
 			chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
 
-			if (chip->int_reg & XD_EXIST) {
+			if (chip->int_reg & XD_EXIST)
 				xd_cnt++;
-			} else {
+			else
 				xd_cnt = 0;
-			}
-			if (chip->int_reg & SD_EXIST) {
+
+			if (chip->int_reg & SD_EXIST)
 				sd_cnt++;
-			} else {
+			else
 				sd_cnt = 0;
-			}
-			if (chip->int_reg & MS_EXIST) {
+
+			if (chip->int_reg & MS_EXIST)
 				ms_cnt++;
-			} else {
+			else
 				ms_cnt = 0;
-			}
+
 			wait_timeout(30);
 		}
 
@@ -677,9 +673,8 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 
 	RTSX_DEBUGP("Switch SSC clock to %dMHz (cur_clk = %d)\n", clk, chip->cur_clk);
 
-	if ((clk <= 2) || (N > max_N)) {
+	if ((clk <= 2) || (N > max_N))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	mcu_cnt = (u8)(125/clk + 3);
 	if (CHECK_PID(chip, 0x5209)) {
@@ -700,32 +695,29 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 	if (chip->ssc_en) {
 		if (CHECK_PID(chip, 0x5209)) {
 			if (chip->cur_card == SD_CARD) {
-				if (CHK_SD_SDR104(sd_card)) {
+				if (CHK_SD_SDR104(sd_card))
 					ssc_depth = chip->ssc_depth_sd_sdr104;
-				} else if (CHK_SD_SDR50(sd_card)) {
+				else if (CHK_SD_SDR50(sd_card))
 					ssc_depth = chip->ssc_depth_sd_sdr50;
-				} else if (CHK_SD_DDR50(sd_card)) {
+				else if (CHK_SD_DDR50(sd_card))
 					ssc_depth = double_depth(chip->ssc_depth_sd_ddr50);
-				} else if (CHK_SD_HS(sd_card)) {
+				else if (CHK_SD_HS(sd_card))
 					ssc_depth = double_depth(chip->ssc_depth_sd_hs);
-				} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
+				else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card))
 					ssc_depth = double_depth(chip->ssc_depth_mmc_52m);
-				} else {
+				else
 					ssc_depth = double_depth(chip->ssc_depth_low_speed);
-				}
 			} else if (chip->cur_card == MS_CARD) {
 				if (CHK_MSPRO(ms_card)) {
-					if (CHK_HG8BIT(ms_card)) {
+					if (CHK_HG8BIT(ms_card))
 						ssc_depth = double_depth(chip->ssc_depth_ms_hg);
-					} else {
+					else
 						ssc_depth = double_depth(chip->ssc_depth_ms_4bit);
-					}
 				} else {
-					if (CHK_MS4BIT(ms_card)) {
+					if (CHK_MS4BIT(ms_card))
 						ssc_depth = double_depth(chip->ssc_depth_ms_4bit);
-					} else {
+					else
 						ssc_depth = double_depth(chip->ssc_depth_low_speed);
-					}
 				}
 			} else {
 				ssc_depth = double_depth(chip->ssc_depth_low_speed);
@@ -733,23 +725,23 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 
 			if (ssc_depth) {
 				if (div == CLK_DIV_2) {
-					if (ssc_depth > 1) {
+					if (ssc_depth > 1)
 						ssc_depth -= 1;
-					} else {
+					else
 						ssc_depth = SSC_DEPTH_4M;
-					}
+
 				} else if (div == CLK_DIV_4) {
-					if (ssc_depth > 2) {
+					if (ssc_depth > 2)
 						ssc_depth -= 2;
-					} else {
+					else
 						ssc_depth = SSC_DEPTH_4M;
-					}
+
 				} else if (div == CLK_DIV_8) {
-					if (ssc_depth > 3) {
+					if (ssc_depth > 3)
 						ssc_depth -= 3;
-					} else {
+					else
 						ssc_depth = SSC_DEPTH_4M;
-					}
+
 				}
 			}
 		} else {
@@ -760,11 +752,10 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 		ssc_depth = 0;
 	}
 
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		ssc_depth_mask = SSC_DEPTH_MASK;
-	} else {
+	else
 		ssc_depth_mask = 0x03;
-	}
 
 	RTSX_DEBUGP("ssc_depth = %d\n", ssc_depth);
 
@@ -781,9 +772,8 @@ int switch_ssc_clock(struct rtsx_chip *chip, int clk)
 	}
 
 	retval = rtsx_send_cmd(chip, 0, WAIT_TIME);
-	if (retval < 0) {
+	if (retval < 0)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	udelay(10);
 	RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
@@ -976,25 +966,23 @@ int card_power_on(struct rtsx_chip *chip, u8 card)
 
 	rtsx_init_cmd(chip);
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val1);
-	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD)) {
+	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD))
 		rtsx_add_cmd(chip, WRITE_REG_CMD, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_SUSPEND);
-	}
+
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	udelay(chip->pmos_pwr_on_interval);
 
 	rtsx_init_cmd(chip);
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, mask, val2);
-	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD)) {
+	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD))
 		rtsx_add_cmd(chip, WRITE_REG_CMD, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_ON);
-	}
+
 	retval = rtsx_send_cmd(chip, 0, 100);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1016,9 +1004,8 @@ int card_power_off(struct rtsx_chip *chip, u8 card)
 	}
 
 	RTSX_WRITE_REG(chip, CARD_PWR_CTL, mask, val);
-	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD)) {
+	if (CHECK_PID(chip, 0x5209) && (card == SD_CARD))
 		RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_OFF);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1029,9 +1016,8 @@ int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 sec_addr, u16 sec
 	unsigned int lun = SCSI_LUN(srb);
 	int i;
 
-	if (chip->rw_card[lun] == NULL) {
+	if (chip->rw_card[lun] == NULL)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	for (i = 0; i < 3; i++) {
 		chip->rw_need_retry = 0;
@@ -1042,9 +1028,9 @@ int card_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 sec_addr, u16 sec
 				rtsx_release_chip(chip);
 				TRACE_RET(chip, STATUS_FAIL);
 			}
-			if (detect_card_cd(chip, chip->cur_card) != STATUS_SUCCESS) {
+			if (detect_card_cd(chip, chip->cur_card) != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 			if (!chip->rw_need_retry) {
 				RTSX_DEBUGP("RW fail, but no need to retry\n");
 				break;
@@ -1066,26 +1052,26 @@ int card_share_mode(struct rtsx_chip *chip, int card)
 
 	if (CHECK_PID(chip, 0x5209) || CHECK_PID(chip, 0x5208)) {
 		mask = CARD_SHARE_MASK;
-		if (card == SD_CARD) {
+		if (card == SD_CARD)
 			value = CARD_SHARE_48_SD;
-		} else if (card == MS_CARD) {
+		else if (card == MS_CARD)
 			value = CARD_SHARE_48_MS;
-		} else if (card == XD_CARD) {
+		else if (card == XD_CARD)
 			value = CARD_SHARE_48_XD;
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	} else if (CHECK_PID(chip, 0x5288)) {
 		mask = 0x03;
-		if (card == SD_CARD) {
+		if (card == SD_CARD)
 			value = CARD_SHARE_BAROSSA_SD;
-		} else if (card == MS_CARD) {
+		else if (card == MS_CARD)
 			value = CARD_SHARE_BAROSSA_MS;
-		} else if (card == XD_CARD) {
+		else if (card == XD_CARD)
 			value = CARD_SHARE_BAROSSA_XD;
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	} else {
 		TRACE_RET(chip, STATUS_FAIL);
 	}
@@ -1103,24 +1089,23 @@ int select_card(struct rtsx_chip *chip, int card)
 	if (chip->cur_card != card) {
 		u8 mod;
 
-		if (card == SD_CARD) {
+		if (card == SD_CARD)
 			mod = SD_MOD_SEL;
-		} else if (card == MS_CARD) {
+		else if (card == MS_CARD)
 			mod = MS_MOD_SEL;
-		} else if (card == XD_CARD) {
+		else if (card == XD_CARD)
 			mod = XD_MOD_SEL;
-		} else if (card == SPI_CARD) {
+		else if (card == SPI_CARD)
 			mod = SPI_MOD_SEL;
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		RTSX_WRITE_REG(chip, CARD_SELECT, 0x07, mod);
 		chip->cur_card = card;
 
 		retval =  card_share_mode(chip, card);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1137,20 +1122,18 @@ void toggle_gpio(struct rtsx_chip *chip, u8 gpio)
 
 void turn_on_led(struct rtsx_chip *chip, u8 gpio)
 {
-	if (CHECK_PID(chip, 0x5288)) {
+	if (CHECK_PID(chip, 0x5288))
 		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), (u8)(1 << gpio));
-	} else {
+	else
 		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), 0);
-	}
 }
 
 void turn_off_led(struct rtsx_chip *chip, u8 gpio)
 {
-	if (CHECK_PID(chip, 0x5288)) {
+	if (CHECK_PID(chip, 0x5288))
 		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), 0);
-	} else {
+	else
 		rtsx_write_register(chip, CARD_GPIO, (u8)(1 << gpio), (u8)(1 << gpio));
-	}
 }
 
 int detect_card_cd(struct rtsx_chip *chip, int card)
@@ -1169,67 +1152,60 @@ int detect_card_cd(struct rtsx_chip *chip, int card)
 	}
 
 	status = rtsx_readl(chip, RTSX_BIPR);
-	if (!(status & card_cd)) {
+	if (!(status & card_cd))
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
 
 int check_card_exist(struct rtsx_chip *chip, unsigned int lun)
 {
-	if (chip->card_exist & chip->lun2card[lun]) {
+	if (chip->card_exist & chip->lun2card[lun])
 		return 1;
-	}
 
 	return 0;
 }
 
 int check_card_ready(struct rtsx_chip *chip, unsigned int lun)
 {
-	if (chip->card_ready & chip->lun2card[lun]) {
+	if (chip->card_ready & chip->lun2card[lun])
 		return 1;
-	}
 
 	return 0;
 }
 
 int check_card_wp(struct rtsx_chip *chip, unsigned int lun)
 {
-	if (chip->card_wp & chip->lun2card[lun]) {
+	if (chip->card_wp & chip->lun2card[lun])
 		return 1;
-	}
 
 	return 0;
 }
 
 int check_card_fail(struct rtsx_chip *chip, unsigned int lun)
 {
-	if (chip->card_fail & chip->lun2card[lun]) {
+	if (chip->card_fail & chip->lun2card[lun])
 		return 1;
-	}
 
 	return 0;
 }
 
 int check_card_ejected(struct rtsx_chip *chip, unsigned int lun)
 {
-	if (chip->card_ejected & chip->lun2card[lun]) {
+	if (chip->card_ejected & chip->lun2card[lun])
 		return 1;
-	}
 
 	return 0;
 }
 
 u8 get_lun_card(struct rtsx_chip *chip, unsigned int lun)
 {
-	if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD) {
+	if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD)
 		return (u8)XD_CARD;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
+	else if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD)
 		return (u8)SD_CARD;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
+	else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD)
 		return (u8)MS_CARD;
-	}
 
 	return 0;
 }
diff --git a/drivers/staging/rts_pstor/rtsx_chip.c b/drivers/staging/rts_pstor/rtsx_chip.c
index 5452069fbe08..d8e691b99028 100644
--- a/drivers/staging/rts_pstor/rtsx_chip.c
+++ b/drivers/staging/rts_pstor/rtsx_chip.c
@@ -105,11 +105,10 @@ void rtsx_enable_bus_int(struct rtsx_chip *chip)
 		reg |= DELINK_INT_EN;
 #ifdef SUPPORT_OCP
 	if (CHECK_PID(chip, 0x5209)) {
-		if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 			reg |= MS_OC_INT_EN | SD_OC_INT_EN;
-		} else {
+		else
 			reg |= SD_OC_INT_EN;
-		}
 	} else {
 		reg |= OC_INT_EN;
 	}
@@ -186,20 +185,20 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 		u8 cd_toggle_mask = 0;
 
 		RTSX_READ_REG(chip, TLPTISTAT, &tmp);
-		if (CHECK_PID(chip, 0x5209)) {
+		if (CHECK_PID(chip, 0x5209))
 			cd_toggle_mask = 0x10;
-		} else {
+		else
 			cd_toggle_mask = 0x08;
-		}
+
 		if (tmp & cd_toggle_mask) {
 			/* Disable sdio_bus_auto_switch */
-			if (CHECK_PID(chip, 0x5288)) {
+			if (CHECK_PID(chip, 0x5288))
 				RTSX_WRITE_REG(chip, 0xFE5A, 0x08, 0x00);
-			} else if (CHECK_PID(chip, 0x5208)) {
+			else if (CHECK_PID(chip, 0x5208))
 				RTSX_WRITE_REG(chip, 0xFE70, 0x80, 0x00);
-			} else {
+			else
 				RTSX_WRITE_REG(chip, SDIO_CFG, SDIO_BUS_AUTO_SWITCH, 0);
-			}
+
 			RTSX_WRITE_REG(chip, TLPTISTAT, 0xFF, tmp);
 
 			chip->need_reset |= SD_CARD;
@@ -208,16 +207,14 @@ 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) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			} else {
 				RTSX_WRITE_REG(chip, FPGA_PULL_CTL, FPGA_SD_PULL_CTL_BIT | 0x20, 0);
 			}
 			retval = card_share_mode(chip, SD_CARD);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 
 			/* Enable sdio_bus_auto_switch */
 			if (CHECK_PID(chip, 0x5288)) {
@@ -232,11 +229,11 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 			chip->sd_io = 1;
 		}
 	} else {
-		if (CHECK_PID(chip, 0x5209)) {
+		if (CHECK_PID(chip, 0x5209))
 			RTSX_WRITE_REG(chip, TLPTISTAT, 0x10, 0x10);
-		} else {
+		else
 			RTSX_WRITE_REG(chip, TLPTISTAT, 0x08, 0x08);
-		}
+
 		chip->need_reset |= SD_CARD;
 	}
 
@@ -257,48 +254,47 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 		/* optimize PHY */
 		retval = rtsx_write_phy_register(chip, 0x00, 0xB966);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x01, 0x713F);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x03, 0xA549);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x06, 0xB235);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x07, 0xEF40);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x1E, 0xF8EB);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = rtsx_write_phy_register(chip, 0x19, 0xFE6C);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		wait_timeout(1);
 		retval = rtsx_write_phy_register(chip, 0x0A, 0x05C0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 
 		retval = rtsx_write_cfg_dw(chip, 1, 0x110, 0xFFFF, 0xFFFF);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		retval = rtsx_read_phy_register(chip, 0x08, &val);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		RTSX_DEBUGP("Read from phy 0x08: 0x%04x\n", val);
 
 		if (chip->phy_voltage) {
@@ -308,9 +304,9 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 			val |= chip->phy_voltage;
 			RTSX_DEBUGP("Write to phy 0x08: 0x%04x\n", val);
 			retval = rtsx_write_phy_register(chip, 0x08, val);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 		} else {
 			chip->phy_voltage = (u8)(val & 0x3F);
 			RTSX_DEBUGP("Default, chip->phy_voltage = 0x%x\n", chip->phy_voltage);
@@ -324,11 +320,11 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_OCP
 	/* SSC power on, OCD power on */
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 		RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, 0);
-	} else {
+	else
 		RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, MS_OC_POWER_DOWN);
-	}
+
 	if (CHECK_PID(chip, 0x5209)) {
 		RTSX_WRITE_REG(chip, OCPPARA1, SD_OCP_TIME_MASK | MS_OCP_TIME_MASK,
 				    SD_OCP_TIME_800 | MS_OCP_TIME_800);
@@ -352,9 +348,8 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 	RTSX_WRITE_REG(chip, FPDCTL, OC_POWER_DOWN, OC_POWER_DOWN);
 #endif
 
-	if (!CHECK_PID(chip, 0x5288)) {
+	if (!CHECK_PID(chip, 0x5288))
 		RTSX_WRITE_REG(chip, CARD_GPIO_DIR, 0xFF, 0x03);
-	}
 
 	/* Turn off LED */
 	RTSX_WRITE_REG(chip, CARD_GPIO, 0xFF, 0x03);
@@ -364,9 +359,8 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 	/* Card driving select */
 	RTSX_WRITE_REG(chip, CARD_DRIVE_SEL, 0xFF, chip->card_drive_sel);
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		RTSX_WRITE_REG(chip, SD30_DRIVE_SEL, 0x07, chip->sd30_drive_sel_3v3);
-	}
 
 #ifdef LED_AUTO_BLINK
 	RTSX_WRITE_REG(chip, CARD_AUTO_BLINK, 0xFF,
@@ -394,36 +388,34 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 			if (CHK_SDIO_EXIST(chip)) {
 				if (CHECK_PID(chip, 0x5209)) {
 					retval = rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-					if (retval != STATUS_SUCCESS) {
+					if (retval != STATUS_SUCCESS)
 						TRACE_RET(chip, STATUS_FAIL);
-					}
+
 				} else if (CHECK_PID(chip, 0x5288)) {
 					retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-					if (retval != STATUS_SUCCESS) {
+					if (retval != STATUS_SUCCESS)
 						TRACE_RET(chip, STATUS_FAIL);
-					}
 				}
 			}
 		} else {
-			if (CHECK_PID(chip, 0x5208)) {
+			if (CHECK_PID(chip, 0x5208))
 				RTSX_WRITE_REG(chip, ASPM_FORCE_CTL, 0xFF, 0x3F);
-			}
 
 			retval = rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, 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;
-				if (CHECK_PID(chip, 0x5288)) {
+				if (CHECK_PID(chip, 0x5288))
 					retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-				} else {
+				else
 					retval = rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-				}
-				if (retval != STATUS_SUCCESS) {
+
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 			}
 
 			chip->aspm_enabled = 1;
@@ -431,49 +423,45 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 	} else {
 		if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
 			retval = rtsx_write_phy_register(chip, 0x07, 0x0129);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 		retval = rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	retval = rtsx_write_config_byte(chip, 0x81, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHK_SDIO_EXIST(chip)) {
-		if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_PID(chip, 0x5288))
 			retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF00, 0x0100);
-		} else {
+		else
 			retval = rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF00, 0x0100);
-		}
-		if (retval != STATUS_SUCCESS) {
+
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	}
 
 	if (CHECK_PID(chip, 0x5209)) {
 		retval = rtsx_write_cfg_dw(chip, 0, 0x70C, 0xFF000000, 0x5B);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	if (CHECK_PID(chip, 0x5288)) {
 		if (!CHK_SDIO_EXIST(chip)) {
 			retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF, 0x0103);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 			retval = rtsx_write_cfg_dw(chip, 2, 0x84, 0xFF, 0x03);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 		}
 	}
 
@@ -499,29 +487,29 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 			if (chip->ic_version >= IC_VER_D) {
 				u16 reg;
 				retval = rtsx_read_phy_register(chip, 0x00, &reg);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 				reg &= 0xFE7F;
 				reg |= 0x80;
 				retval = rtsx_write_phy_register(chip, 0x00, reg);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 				retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 				reg &= 0xFFF7;
 				retval = rtsx_write_phy_register(chip, 0x1C, reg);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 			}
 
-			if (chip->driver_first_load && (chip->ic_version < IC_VER_C)) {
+			if (chip->driver_first_load && (chip->ic_version < IC_VER_C))
 				rtsx_calibration(chip);
-			}
+
 		} else {
 			rtsx_enable_bus_int(chip);
 		}
@@ -550,18 +538,18 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 	RTSX_DEBUGP("In rtsx_reset_chip, chip->int_reg = 0x%x\n", chip->int_reg);
 	if (chip->int_reg & SD_EXIST) {
 #ifdef HW_AUTO_SWITCH_SD_BUS
-		if (CHECK_PID(chip, 0x5208) && (chip->ic_version < IC_VER_C)) {
+		if (CHECK_PID(chip, 0x5208) && (chip->ic_version < IC_VER_C))
 			retval = rtsx_pre_handle_sdio_old(chip);
-		} else {
+		else
 			retval = rtsx_pre_handle_sdio_new(chip);
-		}
+
 		RTSX_DEBUGP("chip->need_reset = 0x%x (rtsx_reset_chip)\n", (unsigned int)(chip->need_reset));
 #else  /* HW_AUTO_SWITCH_SD_BUS */
 		retval = rtsx_pre_handle_sdio_old(chip);
 #endif  /* HW_AUTO_SWITCH_SD_BUS */
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	} else {
 		chip->sd_io = 0;
 		RTSX_WRITE_REG(chip, SDIO_CTRL, SDIO_BUS_CTRL | SDIO_CD_CTRL, 0);
@@ -572,9 +560,8 @@ NextCard:
 		chip->need_reset |= XD_CARD;
 	if (chip->int_reg & MS_EXIST)
 		chip->need_reset |= MS_CARD;
-	if (chip->int_reg & CARD_EXIST) {
+	if (chip->int_reg & CARD_EXIST)
 		RTSX_WRITE_REG(chip, SSC_CTL1, SSC_RSTB, SSC_RSTB);
-	}
 
 	RTSX_DEBUGP("In rtsx_init_chip, chip->need_reset = 0x%x\n", (unsigned int)(chip->need_reset));
 
@@ -587,9 +574,8 @@ NextCard:
 
 	if (chip->remote_wakeup_en && !chip->auto_delink_en) {
 		RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x07);
-		if (chip->aux_pwr_exist) {
+		if (chip->aux_pwr_exist)
 			RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x33);
-		}
 	} else {
 		RTSX_WRITE_REG(chip, WAKE_SEL_CTL, 0x07, 0x04);
 		RTSX_WRITE_REG(chip, PME_FORCE_CTL, 0xFF, 0x30);
@@ -598,18 +584,16 @@ NextCard:
 	if (CHECK_PID(chip, 0x5208) && (chip->ic_version >= IC_VER_D)) {
 		RTSX_WRITE_REG(chip, PETXCFG, 0x1C, 0x14);
 	} else if (CHECK_PID(chip, 0x5209)) {
-		if (chip->force_clkreq_0) {
+		if (chip->force_clkreq_0)
 			RTSX_WRITE_REG(chip, PETXCFG, 0x08, 0x08);
-		} else {
+		else
 			RTSX_WRITE_REG(chip, PETXCFG, 0x08, 0x00);
-		}
 	}
 
 	if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
 		retval = rtsx_clr_phy_reg_bit(chip, 0x1C, 2);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	if (chip->ft2_fast_mode) {
@@ -665,11 +649,10 @@ static int rts5209_init(struct rtsx_chip *chip)
 	u8 val = 0;
 
 	val = rtsx_readb(chip, 0x1C);
-	if ((val & 0x10) == 0) {
+	if ((val & 0x10) == 0)
 		chip->asic_code = 1;
-	} else {
+	else
 		chip->asic_code = 0;
-	}
 
 	chip->ic_version = val & 0x0F;
 	chip->phy_debug_mode = 0;
@@ -679,9 +662,9 @@ static int rts5209_init(struct rtsx_chip *chip)
 	chip->ms_power_class_en = 0x03;
 
 	retval = rtsx_read_cfg_dw(chip, 0, 0x724, &lval);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	RTSX_DEBUGP("dw in 0x724: 0x%x\n", lval);
 	val = (u8)lval;
 	if (!(val & 0x80)) {
@@ -690,17 +673,16 @@ static int rts5209_init(struct rtsx_chip *chip)
 		else
 			chip->lun_mode = SD_MS_2LUN;
 
-		if (val & 0x04) {
+		if (val & 0x04)
 			SET_SDIO_EXIST(chip);
-		} else {
+		else
 			CLR_SDIO_EXIST(chip);
-		}
 
-		if (val & 0x02) {
+		if (val & 0x02)
 			chip->hw_bypass_sd = 0;
-		} else {
+		else
 			chip->hw_bypass_sd = 1;
-		}
+
 	} else {
 		SET_SDIO_EXIST(chip);
 		chip->hw_bypass_sd = 0;
@@ -714,24 +696,21 @@ static int rts5209_init(struct rtsx_chip *chip)
 		val = (u8)(lval >> 8);
 
 		clk = (val >> 5) & 0x07;
-		if (clk != 0x07) {
+		if (clk != 0x07)
 			chip->asic_sd_sdr50_clk = 98 - clk * 2;
-		}
 
-		if (val & 0x10) {
+		if (val & 0x10)
 			chip->auto_delink_en = 1;
-		} else {
+		else
 			chip->auto_delink_en = 0;
-		}
 
 		if (chip->ss_en == 2) {
 			chip->ss_en = 0;
 		} else {
-			if (val & 0x08) {
+			if (val & 0x08)
 				chip->ss_en = 1;
-			} else {
+			else
 				chip->ss_en = 0;
-			}
 		}
 
 		clk = val & 0x07;
@@ -750,21 +729,21 @@ static int rts5209_init(struct rtsx_chip *chip)
 		if (clk != 0x03)
 			chip->asic_sd_ddr50_clk = (48 - clk * 2) * 2;
 
-		if (val & 0x01) {
+		if (val & 0x01)
 			chip->sdr104_en = 1;
-		} else {
+		else
 			chip->sdr104_en = 0;
-		}
-		if (val & 0x02) {
+
+		if (val & 0x02)
 			chip->ddr50_en = 1;
-		} else {
+		else
 			chip->ddr50_en = 0;
-		}
-		if (val & 0x04) {
+
+		if (val & 0x04)
 			chip->sdr50_en = 1;
-		} else {
+		else
 			chip->sdr50_en = 0;
-		}
+
 
 		val = (u8)(lval >> 24);
 
@@ -772,11 +751,10 @@ static int rts5209_init(struct rtsx_chip *chip)
 		if (clk != 0x07)
 			chip->asic_sd_sdr104_clk = 206 - clk * 3;
 
-		if (val & 0x10) {
+		if (val & 0x10)
 			chip->power_down_in_ss = 1;
-		} else {
+		else
 			chip->power_down_in_ss = 0;
-		}
 
 		chip->ms_power_class_en = val & 0x03;
 	}
@@ -786,20 +764,19 @@ static int rts5209_init(struct rtsx_chip *chip)
 
 		retval = rtsx_read_pci_cfg_byte(0x00,
 						0x1C, 0x02, 0x58, &reg58);
-		if (retval < 0) {
+		if (retval < 0)
 			return STATUS_SUCCESS;
-		}
+
 		retval = rtsx_read_pci_cfg_byte(0x00,
 						0x1C, 0x02, 0x5B, &reg5b);
-		if (retval < 0) {
+		if (retval < 0)
 			return STATUS_SUCCESS;
-		}
 
 		RTSX_DEBUGP("reg58 = 0x%x, reg5b = 0x%x\n", reg58, reg5b);
 
-		if ((reg58 == 0x00) && (reg5b == 0x01)) {
+		if ((reg58 == 0x00) && (reg5b == 0x01))
 			chip->auto_delink_en = 0;
-		}
+
 	}
 
 	return STATUS_SUCCESS;
@@ -813,24 +790,23 @@ static int rts5208_init(struct rtsx_chip *chip)
 
 	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
 	RTSX_READ_REG(chip, CLK_SEL, &val);
-	if (val == 0) {
+	if (val == 0)
 		chip->asic_code = 1;
-	} else {
+	else
 		chip->asic_code = 0;
-	}
 
 	if (chip->asic_code) {
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		RTSX_DEBUGP("Value of phy register 0x1C is 0x%x\n", reg);
 		chip->ic_version = (reg >> 4) & 0x07;
-		if (reg & PHY_DEBUG_MODE) {
+		if (reg & PHY_DEBUG_MODE)
 			chip->phy_debug_mode = 1;
-		} else {
+		else
 			chip->phy_debug_mode = 0;
-		}
+
 	} else {
 		RTSX_READ_REG(chip, 0xFE80, &val);
 		chip->ic_version = val;
@@ -839,33 +815,29 @@ static int rts5208_init(struct rtsx_chip *chip)
 
 	RTSX_READ_REG(chip, PDINFO, &val);
 	RTSX_DEBUGP("PDINFO: 0x%x\n", val);
-	if (val & AUX_PWR_DETECTED) {
+	if (val & AUX_PWR_DETECTED)
 		chip->aux_pwr_exist = 1;
-	} else {
+	else
 		chip->aux_pwr_exist = 0;
-	}
 
 	RTSX_READ_REG(chip, 0xFE50, &val);
-	if (val & 0x01) {
+	if (val & 0x01)
 		chip->hw_bypass_sd = 1;
-	} else {
+	else
 		chip->hw_bypass_sd = 0;
-	}
 
 	rtsx_read_config_byte(chip, 0x0E, &val);
-	if (val & 0x80) {
+	if (val & 0x80)
 		SET_SDIO_EXIST(chip);
-	} else {
+	else
 		CLR_SDIO_EXIST(chip);
-	}
 
 	if (chip->use_hw_setting) {
 		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
-		if (val & 0x80) {
+		if (val & 0x80)
 			chip->auto_delink_en = 1;
-		} else {
+		else
 			chip->auto_delink_en = 0;
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -879,63 +851,57 @@ static int rts5288_init(struct rtsx_chip *chip)
 
 	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
 	RTSX_READ_REG(chip, CLK_SEL, &val);
-	if (val == 0) {
+	if (val == 0)
 		chip->asic_code = 1;
-	} else {
+	else
 		chip->asic_code = 0;
-	}
 
 	chip->ic_version = 0;
 	chip->phy_debug_mode = 0;
 
 	RTSX_READ_REG(chip, PDINFO, &val);
 	RTSX_DEBUGP("PDINFO: 0x%x\n", val);
-	if (val & AUX_PWR_DETECTED) {
+	if (val & AUX_PWR_DETECTED)
 		chip->aux_pwr_exist = 1;
-	} else {
+	else
 		chip->aux_pwr_exist = 0;
-	}
 
 	RTSX_READ_REG(chip, CARD_SHARE_MODE, &val);
 	RTSX_DEBUGP("CARD_SHARE_MODE: 0x%x\n", val);
-	if (val & 0x04) {
+	if (val & 0x04)
 		chip->baro_pkg = QFN;
-	} else {
+	else
 		chip->baro_pkg = LQFP;
-	}
 
 	RTSX_READ_REG(chip, 0xFE5A, &val);
-	if (val & 0x10) {
+	if (val & 0x10)
 		chip->hw_bypass_sd = 1;
-	} else {
+	else
 		chip->hw_bypass_sd = 0;
-	}
 
 	retval = rtsx_read_cfg_dw(chip, 0, 0x718, &lval);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	max_func = (u8)((lval >> 29) & 0x07);
 	RTSX_DEBUGP("Max function number: %d\n", max_func);
-	if (max_func == 0x02) {
+	if (max_func == 0x02)
 		SET_SDIO_EXIST(chip);
-	} else {
+	else
 		CLR_SDIO_EXIST(chip);
-	}
 
 	if (chip->use_hw_setting) {
 		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
-		if (val & 0x80) {
+		if (val & 0x80)
 			chip->auto_delink_en = 1;
-		} else {
+		else
 			chip->auto_delink_en = 0;
-		}
 
-		if (CHECK_BARO_PKG(chip, LQFP)) {
+		if (CHECK_BARO_PKG(chip, LQFP))
 			chip->lun_mode = SD_MS_1LUN;
-		} else {
+		else
 			chip->lun_mode = DEFAULT_SINGLE;
-		}
+
 	}
 
 	return STATUS_SUCCESS;
@@ -990,22 +956,21 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 		chip->rw_fail_cnt[i] = 0;
 	}
 
-	if (!check_sd_speed_prior(chip->sd_speed_prior)) {
+	if (!check_sd_speed_prior(chip->sd_speed_prior))
 		chip->sd_speed_prior = 0x01040203;
-	}
+
 	RTSX_DEBUGP("sd_speed_prior = 0x%08x\n", chip->sd_speed_prior);
 
-	if (!check_sd_current_prior(chip->sd_current_prior)) {
+	if (!check_sd_current_prior(chip->sd_current_prior))
 		chip->sd_current_prior = 0x00010203;
-	}
+
 	RTSX_DEBUGP("sd_current_prior = 0x%08x\n", chip->sd_current_prior);
 
-	if ((chip->sd_ddr_tx_phase > 31) || (chip->sd_ddr_tx_phase < 0)) {
+	if ((chip->sd_ddr_tx_phase > 31) || (chip->sd_ddr_tx_phase < 0))
 		chip->sd_ddr_tx_phase = 0;
-	}
-	if ((chip->mmc_ddr_tx_phase > 31) || (chip->mmc_ddr_tx_phase < 0)) {
+
+	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);
 	wait_timeout(200);
@@ -1014,24 +979,23 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 
 	if (CHECK_PID(chip, 0x5209)) {
 		retval = rts5209_init(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	} else if (CHECK_PID(chip, 0x5208)) {
 		retval = rts5208_init(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	} else if (CHECK_PID(chip, 0x5288)) {
 		retval = rts5288_init(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 	}
 
-	if (chip->ss_en == 2) {
+	if (chip->ss_en == 2)
 		chip->ss_en = 0;
-	}
 
 	RTSX_DEBUGP("chip->asic_code = %d\n", chip->asic_code);
 	RTSX_DEBUGP("chip->ic_version = 0x%x\n", chip->ic_version);
@@ -1068,9 +1032,8 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_reset_chip(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1118,19 +1081,19 @@ static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 			chip->aspm_level[1] = reg1;
 		}
 
-		if ((reg0 & 0x03) && (reg1 & 0x03)) {
+		if ((reg0 & 0x03) && (reg1 & 0x03))
 			maybe_support_aspm = 1;
-		}
+
 	} else {
-		if (reg0 & 0x03) {
+		if (reg0 & 0x03)
 			maybe_support_aspm = 1;
-		}
+
 	}
 
 	if (reg_changed) {
-		if (maybe_support_aspm) {
+		if (maybe_support_aspm)
 			chip->aspm_l0s_l1_en = 0x03;
-		}
+
 		RTSX_DEBUGP("aspm_level[0] = 0x%02x, aspm_level[1] = 0x%02x\n",
 			      chip->aspm_level[0], chip->aspm_level[1]);
 
@@ -1177,13 +1140,13 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 			if (chip->ocp_int & MS_OC_INT)
 				ms_power_off_card3v3(chip);
 		} else {
-			if (chip->card_exist & SD_CARD) {
+			if (chip->card_exist & SD_CARD)
 				sd_power_off_card3v3(chip);
-			} else if (chip->card_exist & MS_CARD) {
+			else if (chip->card_exist & MS_CARD)
 				ms_power_off_card3v3(chip);
-			} else if (chip->card_exist & XD_CARD) {
+			else if (chip->card_exist & XD_CARD)
 				xd_power_off_card3v3(chip);
-			}
+
 		}
 
 		chip->ocp_int = 0;
@@ -1226,9 +1189,9 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 			if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
 				u32 val;
 				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
-				if (val & 0x07) {
+				if (val & 0x07)
 					ss_allowed = 0;
-				}
+
 			}
 		}
 	} else {
@@ -1284,9 +1247,9 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 
 			turn_off_led(chip, LED_GPIO);
 
-			if (chip->auto_power_down && !chip->card_ready && !chip->sd_io) {
+			if (chip->auto_power_down && !chip->card_ready && !chip->sd_io)
 				rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
-			}
+
 		}
 	}
 
@@ -1299,9 +1262,9 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		break;
 
 	case RTSX_STAT_IDLE:
-		if (chip->sd_io && !chip->sd_int) {
+		if (chip->sd_io && !chip->sd_int)
 			try_to_switch_sdio_ctrl(chip);
-		}
+
 		rtsx_enable_aspm(chip);
 		break;
 
@@ -1313,9 +1276,8 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 #ifdef SUPPORT_OCP
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
 #ifdef CONFIG_RTS_PSTOR_DEBUG
-		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER | MS_OC_NOW | MS_OC_EVER)) {
+		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER | MS_OC_NOW | MS_OC_EVER))
 			RTSX_DEBUGP("Over current, OCPSTAT is 0x%x\n", chip->ocp_stat);
-		}
 #endif
 
 		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
@@ -1362,28 +1324,27 @@ Delink_Stage:
 			if (chip->auto_delink_cnt == delink_stage1_cnt) {
 				rtsx_set_stat(chip, RTSX_STAT_DELINK);
 
-				if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
+				if (chip->asic_code && CHECK_PID(chip, 0x5208))
 					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-				}
+
 				if (chip->card_exist) {
 					RTSX_DEBUGP("False card inserted, do force delink\n");
 
-					if (enter_L1) {
+					if (enter_L1)
 						rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
-					}
+
 					rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x0A);
 
-					if (enter_L1) {
+					if (enter_L1)
 						rtsx_enter_L1(chip);
-					}
 
 					chip->auto_delink_cnt = delink_stage3_cnt + 1;
 				} else {
 					RTSX_DEBUGP("No card inserted, do delink\n");
 
-					if (enter_L1) {
+					if (enter_L1)
 						rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
-					}
+
 #ifdef HW_INT_WRITE_CLR
 					if (CHECK_PID(chip, 0x5209)) {
 						rtsx_writel(chip, RTSX_BIPR, 0xFFFFFFFF);
@@ -1392,22 +1353,21 @@ Delink_Stage:
 #endif
 					rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 0x02);
 
-					if (enter_L1) {
+					if (enter_L1)
 						rtsx_enter_L1(chip);
-					}
+
 				}
 			}
 
 			if (chip->auto_delink_cnt == delink_stage2_cnt) {
 				RTSX_DEBUGP("Try to do force delink\n");
 
-				if (enter_L1) {
+				if (enter_L1)
 					rtsx_exit_L1(chip);
-				}
 
-				if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
+				if (chip->asic_code && CHECK_PID(chip, 0x5208))
 					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-				}
+
 				rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x0A);
 			}
 
@@ -1472,9 +1432,9 @@ 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) {
+			if (data != (u8)val)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
+
 			return STATUS_SUCCESS;
 		}
 	}
@@ -1487,9 +1447,8 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 	u32 val = 2 << 30;
 	int i;
 
-	if (data) {
+	if (data)
 		*data = 0;
-	}
 
 	val |= (u32)(addr & 0x3FFF) << 16;
 
@@ -1497,18 +1456,15 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
 		val = rtsx_readl(chip, RTSX_HAIMR);
-		if ((val & (1 << 31)) == 0) {
+		if ((val & (1 << 31)) == 0)
 			break;
-		}
 	}
 
-	if (i >= MAX_RW_REG_CNT) {
+	if (i >= MAX_RW_REG_CNT)
 		TRACE_RET(chip, STATUS_TIMEDOUT);
-	}
 
-	if (data) {
+	if (data)
 		*data = (u8)(val & 0xFF);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1537,9 +1493,8 @@ int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask, u3
 
 		for (i = 0; i < MAX_RW_REG_CNT; i++) {
 			RTSX_READ_REG(chip, CFGRWCTL, &tmp);
-			if ((tmp & 0x80) == 0) {
+			if ((tmp & 0x80) == 0)
 				break;
-			}
 		}
 	}
 
@@ -1558,9 +1513,8 @@ int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val)
 
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
 		RTSX_READ_REG(chip, CFGRWCTL, &tmp);
-		if ((tmp & 0x80) == 0) {
+		if ((tmp & 0x80) == 0)
 			break;
-		}
 	}
 
 	for (i = 0; i < 4; i++) {
@@ -1568,9 +1522,8 @@ int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val)
 		data |= (u32)tmp << (i * 8);
 	}
 
-	if (val) {
+	if (val)
 		*val = data;
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1585,21 +1538,19 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf, int l
 
 	RTSX_DEBUGP("%s\n", __func__);
 
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_NOMEM);
-	}
 
-	if ((len + offset) % 4) {
+	if ((len + offset) % 4)
 		dw_len = (len + offset) / 4 + 1;
-	} else {
+	else
 		dw_len = (len + offset) / 4;
-	}
+
 	RTSX_DEBUGP("dw_len = %d\n", dw_len);
 
 	data = vzalloc(dw_len * 4);
-	if (!data) {
+	if (!data)
 		TRACE_RET(chip, STATUS_NOMEM);
-	}
 
 	mask = vzalloc(dw_len * 4);
 	if (!mask) {
@@ -1645,17 +1596,16 @@ int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf, int le
 
 	RTSX_DEBUGP("%s\n", __func__);
 
-	if ((len + offset) % 4) {
+	if ((len + offset) % 4)
 		dw_len = (len + offset) / 4 + 1;
-	} else {
+	else
 		dw_len = (len + offset) / 4;
-	}
+
 	RTSX_DEBUGP("dw_len = %d\n", dw_len);
 
 	data = (u32 *)vmalloc(dw_len * 4);
-	if (!data) {
+	if (!data)
 		TRACE_RET(chip, STATUS_NOMEM);
-	}
 
 	for (i = 0; i < dw_len; i++) {
 		retval = rtsx_read_cfg_dw(chip, func, aligned_addr + i * 4, data + i);
@@ -1700,9 +1650,8 @@ int rtsx_write_phy_register(struct rtsx_chip *chip, u8 addr, u16 val)
 		}
 	}
 
-	if (!finished) {
+	if (!finished)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1724,9 +1673,8 @@ int rtsx_read_phy_register(struct rtsx_chip *chip, u8 addr, u16 *val)
 		}
 	}
 
-	if (!finished) {
+	if (!finished)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_READ_REG(chip, PHYDATA0, &tmp);
 	data = tmp;
@@ -1753,9 +1701,8 @@ int rtsx_read_efuse(struct rtsx_chip *chip, u8 addr, u8 *val)
 		udelay(1);
 	}
 
-	if (data & 0x80) {
+	if (data & 0x80)
 		TRACE_RET(chip, STATUS_TIMEDOUT);
-	}
 
 	RTSX_READ_REG(chip, EFUSE_DATA, &data);
 	if (val)
@@ -1786,9 +1733,8 @@ int rtsx_write_efuse(struct rtsx_chip *chip, u8 addr, u8 val)
 			wait_timeout(3);
 		}
 
-		if (data & 0x80) {
+		if (data & 0x80)
 			TRACE_RET(chip, STATUS_TIMEDOUT);
-		}
 
 		wait_timeout(5);
 	}
@@ -1802,15 +1748,14 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (value & (1 << bit)) {
 		value &= ~(1 << bit);
 		retval = rtsx_write_phy_register(chip, reg, value);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1822,15 +1767,14 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	if (0 == (value & (1 << bit))) {
 		value |= (1 << bit);
 		retval = rtsx_write_phy_register(chip, reg, value);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1861,11 +1805,11 @@ static void rtsx_handle_pm_dstate(struct rtsx_chip *chip, u8 dstate)
 	if (CHK_SDIO_EXIST(chip)) {
 		u8 func_no;
 
-		if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_PID(chip, 0x5288))
 			func_no = 2;
-		} else {
+		else
 			func_no = 1;
-		}
+
 		rtsx_read_cfg_dw(chip, func_no, 0x84, &ultmp);
 		RTSX_DEBUGP("pm_dstate of function %d: 0x%x\n", (int)func_no, ultmp);
 		rtsx_write_cfg_dw(chip, func_no, 0x84, 0xFF, dstate);
@@ -1898,11 +1842,10 @@ void rtsx_enter_ss(struct rtsx_chip *chip)
 	}
 
 	if (CHK_SDIO_EXIST(chip)) {
-		if (CHECK_PID(chip, 0x5288)) {
+		if (CHECK_PID(chip, 0x5288))
 			rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF00, 0x0100);
-		} else {
+		else
 			rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF00, 0x0100);
-		}
 	}
 
 	if (chip->auto_delink_en) {
@@ -1953,11 +1896,11 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 	u32 ocp_int = 0;
 
 	if (CHECK_PID(chip, 0x5209)) {
-		if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 			ocp_int = MS_OC_INT | SD_OC_INT;
-		} else {
+		else
 			ocp_int = SD_OC_INT;
-		}
+
 	} else {
 		ocp_int = OC_INT;
 	}
@@ -1976,9 +1919,8 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
 
 #ifdef HW_INT_WRITE_CLR
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		rtsx_writel(chip, RTSX_BIPR, chip->int_reg);
-	}
 #endif
 
 	if (((chip->int_reg & int_enable) == 0) || (chip->int_reg == 0xFFFFFFFF))
@@ -1988,9 +1930,8 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 		if (CHECK_PID(chip, 0x5209)) {
 			u8 val;
 			rtsx_read_config_byte(chip, 0x05, &val);
-			if (val & 0x04) {
+			if (val & 0x04)
 				return STATUS_FAIL;
-			}
 		}
 	}
 
@@ -2107,16 +2048,15 @@ void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat)
 		RTSX_DEBUGP("Host enter S1\n");
 		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, HOST_ENTER_S1);
 	} else if (pm_stat == PM_S3) {
-		if (chip->s3_pwr_off_delay > 0) {
+		if (chip->s3_pwr_off_delay > 0)
 			wait_timeout(chip->s3_pwr_off_delay);
-		}
+
 		RTSX_DEBUGP("Host enter S3\n");
 		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, HOST_ENTER_S3);
 	}
 
-	if (chip->do_delink_before_power_down && chip->auto_delink_en) {
+	if (chip->do_delink_before_power_down && chip->auto_delink_en)
 		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 2);
-	}
 
 	rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
 
@@ -2143,11 +2083,10 @@ void rtsx_enable_aspm(struct rtsx_chip *chip)
 
 			if (CHK_SDIO_EXIST(chip)) {
 				u16 val = chip->aspm_l0s_l1_en | 0x0100;
-				if (CHECK_PID(chip, 0x5288)) {
+				if (CHECK_PID(chip, 0x5288))
 					rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF, val);
-				} else {
+				else
 					rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFFFF, val);
-				}
 			}
 		}
 	}
@@ -2167,11 +2106,11 @@ void rtsx_disable_aspm(struct rtsx_chip *chip)
 
 			if (chip->asic_code && CHECK_PID(chip, 0x5208))
 				rtsx_write_phy_register(chip, 0x07, 0x0129);
-			if (CHECK_PID(chip, 0x5208)) {
+			if (CHECK_PID(chip, 0x5208))
 				rtsx_write_register(chip, ASPM_FORCE_CTL, 0xF3, 0x30);
-			} else {
+			else
 				rtsx_write_config_byte(chip, LCTLR, 0x00);
-			}
+
 			wait_timeout(1);
 		}
 	}
@@ -2186,9 +2125,8 @@ int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 	u16 reg_addr;
 	u8 *ptr;
 
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	ptr = buf;
 	reg_addr = PPBUF_BASE2;
@@ -2199,9 +2137,8 @@ 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) {
+		if (retval < 0)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		memcpy(ptr, rtsx_get_cmd_data(chip), 256);
 		ptr += 256;
@@ -2214,9 +2151,8 @@ 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) {
+		if (retval < 0)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	memcpy(ptr, rtsx_get_cmd_data(chip), buf_len%256);
@@ -2231,9 +2167,8 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 	u16 reg_addr;
 	u8 *ptr;
 
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, STATUS_ERROR);
-	}
 
 	ptr = buf;
 	reg_addr = PPBUF_BASE2;
@@ -2246,9 +2181,8 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 		}
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0) {
+		if (retval < 0)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	if (buf_len%256) {
@@ -2260,9 +2194,8 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 		}
 
 		retval = rtsx_send_cmd(chip, 0, 250);
-		if (retval < 0) {
+		if (retval < 0)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -2270,9 +2203,8 @@ 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) {
+	if (rtsx_readl(chip, 0) == 0xFFFFFFFF)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2288,17 +2220,15 @@ int rtsx_force_power_on(struct rtsx_chip *chip, u8 ctl)
 #ifdef SUPPORT_OCP
 	if (ctl & OC_PDCTL) {
 		mask |= SD_OC_POWER_DOWN;
-		if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+		if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 			mask |= MS_OC_POWER_DOWN;
-		}
 	}
 #endif
 
 	if (mask) {
 		retval = rtsx_write_register(chip, FPDCTL, mask, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (CHECK_PID(chip, 0x5288))
 			wait_timeout(200);
@@ -2326,9 +2256,8 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
diff --git a/drivers/staging/rts_pstor/rtsx_scsi.c b/drivers/staging/rts_pstor/rtsx_scsi.c
index f2e5842d4c90..86c41b3a42a3 100644
--- a/drivers/staging/rts_pstor/rtsx_scsi.c
+++ b/drivers/staging/rts_pstor/rtsx_scsi.c
@@ -135,9 +135,9 @@ void scsi_show_command(struct scsi_cmnd *srb)
 	default: what = "(unknown command)"; unknown_cmd = 1; break;
 	}
 
-	if (srb->cmnd[0] != TEST_UNIT_READY) {
+	if (srb->cmnd[0] != TEST_UNIT_READY)
 		RTSX_DEBUGP("Command %s (%d bytes)\n", what, srb->cmd_len);
-	}
+
 	if (unknown_cmd) {
 		RTSX_DEBUGP("");
 		for (i = 0; i < srb->cmd_len && i < 16; i++)
@@ -317,11 +317,11 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	};
 
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
-		if (chip->lun2card[lun] == SD_CARD) {
+		if (chip->lun2card[lun] == SD_CARD)
 			inquiry_string = inquiry_sd;
-		} else {
+		else
 			inquiry_string = inquiry_ms;
-		}
+
 	} else if (CHECK_LUN_MODE(chip, SD_MS_1LUN)) {
 		inquiry_string = inquiry_sdms;
 	} else {
@@ -329,9 +329,8 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 #ifdef SUPPORT_MAGIC_GATE
 	if ((chip->mspro_formatter_enable) &&
@@ -340,23 +339,21 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if (chip->mspro_formatter_enable)
 #endif
 	{
-		if (!card || (card == MS_CARD)) {
+		if (!card || (card == MS_CARD))
 			pro_formatter_flag = 1;
-		}
 	}
 
 	if (pro_formatter_flag) {
-		if (scsi_bufflen(srb) < 56) {
+		if (scsi_bufflen(srb) < 56)
 			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		} else {
+		else
 			sendbytes = 56;
-		}
+
 	} else {
-		if (scsi_bufflen(srb) < 36) {
+		if (scsi_bufflen(srb) < 36)
 			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		} else {
+		else
 			sendbytes = 36;
-		}
 	}
 
 	if (sendbytes > 8) {
@@ -371,9 +368,8 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	if (pro_formatter_flag) {
-		if (sendbytes > 36) {
+		if (sendbytes > 36)
 			memcpy(buf + 36, formatter_inquiry_str, sendbytes - 36);
-		}
 	}
 
 	scsi_set_resid(srb, 0);
@@ -467,9 +463,8 @@ static int request_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	tmp = (unsigned char *)sense;
 	memcpy(buf, tmp, scsi_bufflen(srb));
@@ -494,15 +489,15 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 
 	if (cmd == MODE_SENSE) {
 		sys_info_offset = 8;
-		if (data_size > 0x68) {
+		if (data_size > 0x68)
 			data_size = 0x68;
-		}
+
 		buf[i++] = 0x67;  /* Mode Data Length */
 	} else {
 		sys_info_offset = 12;
-		if (data_size > 0x6C) {
+		if (data_size > 0x6C)
 			data_size = 0x6C;
-		}
+
 		buf[i++] = 0x00;  /* Mode Data Length (MSB) */
 		buf[i++] = 0x6A;  /* Mode Data Length (LSB) */
 	}
@@ -520,11 +515,11 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 		}
 
 		/* WP */
-		if (check_card_wp(chip, lun)) {
+		if (check_card_wp(chip, lun))
 			buf[i++] = 0x80;
-		} else {
+		else
 			buf[i++] = 0x00;
-		}
+
 	} else {
 		buf[i++] = 0x00;	/* MediaType */
 		buf[i++] = 0x00;	/* WP */
@@ -545,11 +540,10 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 		if (data_size >= 11)
 			buf[i++] = 0x00;		/* No Access Control */
 		if (data_size >= 12) {
-			if (support_format) {
+			if (support_format)
 				buf[i++] = 0xC0;	/* SF, SGM */
-			} else {
+			else
 				buf[i++] = 0x00;
-			}
 		}
 	} else {
 		/* The Following Data is the content of "Page 0x20" */
@@ -560,11 +554,10 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 		if (data_size >= 7)
 			buf[i++] = 0x00;		/* No Access Control */
 		if (data_size >= 8) {
-			if (support_format) {
+			if (support_format)
 				buf[i++] = 0xC0;	/* SF, SGM */
-			} else {
+			else
 				buf[i++] = 0x00;
-			}
 		}
 	}
 
@@ -600,9 +593,8 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	if ((chip->lun2card[lun] & MS_CARD)) {
 		if (!card || (card == MS_CARD)) {
 			dataSize = 108;
-			if (chip->mspro_formatter_enable) {
+			if (chip->mspro_formatter_enable)
 				pro_formatter_flag = 1;
-			}
 		}
 	}
 #else
@@ -615,9 +607,8 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 #endif
 
 	buf = kmalloc(dataSize, GFP_KERNEL);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	pageCode = srb->cmnd[2] & 0x3f;
 
@@ -632,11 +623,11 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				dataSize = 4;
 				buf[0] = 0x03;
 				buf[1] = 0x00;
-				if (check_card_wp(chip, lun)) {
+				if (check_card_wp(chip, lun))
 					buf[2] = 0x80;
-				} else {
+				else
 					buf[2] = 0x00;
-				}
+
 				buf[3] = 0x00;
 			}
 		} else {
@@ -648,11 +639,10 @@ static int mode_sense(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 				buf[0] = 0x00;
 				buf[1] = 0x06;
 				buf[2] = 0x00;
-				if (check_card_wp(chip, lun)) {
+				if (check_card_wp(chip, lun))
 					buf[3] = 0x80;
-				} else {
+				else
 					buf[3] = 0x00;
-				}
 				buf[4] = 0x00;
 				buf[5] = 0x00;
 				buf[6] = 0x00;
@@ -759,11 +749,11 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (chip->rw_fail_cnt[lun] == 3) {
 		RTSX_DEBUGP("read/write fail three times in succession\n");
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE)
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		} else {
+		else
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		}
+
 		TRACE_RET(chip, TRANSPORT_FAILED);
 	}
 
@@ -776,9 +766,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		if (CHECK_PID(chip, 0x5209) && chip->max_payload) {
 			u8 val = 0x10 | (chip->max_payload << 5);
 			retval = rtsx_write_cfg_dw(chip, 0, 0x78, 0xFF, val);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_ERROR);
-			}
 		}
 	}
 
@@ -789,11 +778,10 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 		} else {
 			chip->rw_fail_cnt[lun]++;
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+			if (srb->sc_data_direction == DMA_FROM_DEVICE)
 				set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			} else {
+			else
 				set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			}
 		}
 		retval = TRANSPORT_FAILED;
 		TRACE_GOTO(chip, Exit);
@@ -808,9 +796,8 @@ Exit:
 	if (srb->sc_data_direction == DMA_TO_DEVICE) {
 		if (CHECK_PID(chip, 0x5209) && chip->max_payload) {
 			retval = rtsx_write_cfg_dw(chip, 0, 0x78, 0xFF, 0x10);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_ERROR);
-			}
 		}
 	}
 
@@ -837,9 +824,8 @@ static int read_format_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
 
 	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	buf[i++] = 0;
 	buf[i++] = 0;
@@ -864,22 +850,20 @@ static int read_format_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 			buf[i++] = (unsigned char)(card_size >> 8);
 			buf[i++] = (unsigned char)card_size;
 
-			if (desc_cnt == 2) {
+			if (desc_cnt == 2)
 				buf[i++] = 2;
-			} else {
+			else
 				buf[i++] = 0;
-			}
 		} else {
 			buf[i++] = 0xFF;
 			buf[i++] = 0xFF;
 			buf[i++] = 0xFF;
 			buf[i++] = 0xFF;
 
-			if (desc_cnt == 2) {
+			if (desc_cnt == 2)
 				buf[i++] = 3;
-			} else {
+			else
 				buf[i++] = 0;
-			}
 		}
 
 		buf[i++] = 0x00;
@@ -916,9 +900,8 @@ static int read_capacity(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = kmalloc(8, GFP_KERNEL);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	card_size = get_card_size(chip, lun);
 	buf[0] = (unsigned char)((card_size - 1) >> 24);
@@ -956,9 +939,8 @@ static int read_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)srb->cmnd[4] << 8) | srb->cmnd[5];
 
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
@@ -1016,9 +998,8 @@ static int write_eeprom(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	} else {
 		len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
 		buf = (u8 *)vmalloc(len);
-		if (buf == NULL) {
+		if (buf == NULL)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		rtsx_stor_get_xfer_buf(buf, len, srb);
 		scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1061,9 +1042,8 @@ static int read_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
@@ -1114,9 +1094,8 @@ static int write_mem(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
 	buf = (u8 *)vmalloc(len);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1200,16 +1179,15 @@ static int trace_msg_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	clear = srb->cmnd[2];
 
 	buf = (unsigned char *)vmalloc(scsi_bufflen(srb));
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 	ptr = buf;
 
-	if (chip->trace_msg[chip->msg_idx].valid) {
+	if (chip->trace_msg[chip->msg_idx].valid)
 		msg_cnt = TRACE_ITEM_CNT;
-	} else {
+	else
 		msg_cnt = chip->msg_idx;
-	}
+
 	*(ptr++) = (u8)(msg_cnt >> 24);
 	*(ptr++) = (u8)(msg_cnt >> 16);
 	*(ptr++) = (u8)(msg_cnt >> 8);
@@ -1225,15 +1203,14 @@ static int trace_msg_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 		*(ptr++) = (u8)(chip->trace_msg[idx].line >> 8);
 		*(ptr++) = (u8)(chip->trace_msg[idx].line);
-		for (j = 0; j < MSG_FUNC_LEN; j++) {
+		for (j = 0; j < MSG_FUNC_LEN; j++)
 			*(ptr++) = chip->trace_msg[idx].func[j];
-		}
-		for (j = 0; j < MSG_FILE_LEN; j++) {
+
+		for (j = 0; j < MSG_FILE_LEN; j++)
 			*(ptr++) = chip->trace_msg[idx].file[j];
-		}
-		for (j = 0; j < TIME_VAL_LEN; j++) {
+
+		for (j = 0; j < TIME_VAL_LEN; j++)
 			*(ptr++) = chip->trace_msg[idx].timeval_buf[j];
-		}
 	}
 
 	rtsx_stor_set_xfer_buf(buf, scsi_bufflen(srb), srb);
@@ -1424,20 +1401,19 @@ static int dma_access_ring_buffer(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)(srb->cmnd[4]) << 8) | srb->cmnd[5];
 	len = min(len, (u16)scsi_bufflen(srb));
 
-	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+	if (srb->sc_data_direction == DMA_FROM_DEVICE)
 		RTSX_DEBUGP("Read from device\n");
-	} else {
+	else
 		RTSX_DEBUGP("Write to device\n");
-	}
 
 	retval = rtsx_transfer_data(chip, 0, scsi_sglist(srb), len,
 			scsi_sg_count(srb), srb->sc_data_direction, 1000);
 	if (retval < 0) {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
+		if (srb->sc_data_direction == DMA_FROM_DEVICE)
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		} else {
+		else
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		}
+
 		TRACE_RET(chip, TRANSPORT_FAILED);
 	}
 	scsi_set_resid(srb, 0);
@@ -1462,22 +1438,20 @@ static int get_dev_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	status[0] = (u8)(chip->product_id);
 	status[1] = chip->ic_version;
 
-	if (chip->auto_delink_en) {
+	if (chip->auto_delink_en)
 		status[2] = 0x10;
-	} else {
+	else
 		status[2] = 0x00;
-	}
 
 	status[3] = 20;
 	status[4] = 10;
 	status[5] = 05;
 	status[6] = 21;
 
-	if (chip->card_wp) {
+	if (chip->card_wp)
 		status[7] = 0x20;
-	} else {
+	else
 		status[7] = 0x00;
-	}
 
 #ifdef SUPPORT_OCP
 	status[8] = 0;
@@ -1489,67 +1463,60 @@ static int get_dev_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		oc_ever_mask = SD_OC_EVER;
 	}
 
-	if (chip->ocp_stat & oc_now_mask) {
+	if (chip->ocp_stat & oc_now_mask)
 		status[8] |= 0x02;
-	}
-	if (chip->ocp_stat & oc_ever_mask) {
+
+	if (chip->ocp_stat & oc_ever_mask)
 		status[8] |= 0x01;
-	}
 #endif
 
 	if (card == SD_CARD) {
 		if (CHK_SD(sd_card)) {
 			if (CHK_SD_HCXC(sd_card)) {
-				if (sd_card->capacity > 0x4000000) {
+				if (sd_card->capacity > 0x4000000)
 					status[0x0E] = 0x02;
-				} else {
+				else
 					status[0x0E] = 0x01;
-				}
 			} else {
 				status[0x0E] = 0x00;
 			}
 
-			if (CHK_SD_SDR104(sd_card)) {
+			if (CHK_SD_SDR104(sd_card))
 				status[0x0F] = 0x03;
-			} else if (CHK_SD_DDR50(sd_card)) {
+			else if (CHK_SD_DDR50(sd_card))
 				status[0x0F] = 0x04;
-			} else if (CHK_SD_SDR50(sd_card)) {
+			else if (CHK_SD_SDR50(sd_card))
 				status[0x0F] = 0x02;
-			} else if (CHK_SD_HS(sd_card)) {
+			else if (CHK_SD_HS(sd_card))
 				status[0x0F] = 0x01;
-			} else {
+			else
 				status[0x0F] = 0x00;
-			}
 		} else {
-			if (CHK_MMC_SECTOR_MODE(sd_card)) {
+			if (CHK_MMC_SECTOR_MODE(sd_card))
 				status[0x0E] = 0x01;
-			} else {
+			else
 				status[0x0E] = 0x00;
-			}
 
-			if (CHK_MMC_DDR52(sd_card)) {
+			if (CHK_MMC_DDR52(sd_card))
 				status[0x0F] = 0x03;
-			} else if (CHK_MMC_52M(sd_card)) {
+			else if (CHK_MMC_52M(sd_card))
 				status[0x0F] = 0x02;
-			} else if (CHK_MMC_26M(sd_card)) {
+			else if (CHK_MMC_26M(sd_card))
 				status[0x0F] = 0x01;
-			} else {
+			else
 				status[0x0F] = 0x00;
-			}
 		}
 	} else if (card == MS_CARD) {
 		if (CHK_MSPRO(ms_card)) {
-			if (CHK_MSXC(ms_card)) {
+			if (CHK_MSXC(ms_card))
 				status[0x0E] = 0x01;
-			} else {
+			else
 				status[0x0E] = 0x00;
-			}
 
-			if (CHK_HG8BIT(ms_card)) {
+			if (CHK_HG8BIT(ms_card))
 				status[0x0F] = 0x01;
-			} else {
+			else
 				status[0x0F] = 0x00;
-			}
 		}
 	}
 
@@ -1600,37 +1567,35 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
+
 		rtsx_disable_bus_int(chip);
 
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
+
 		reg |= 0x0001;
 		retval = rtsx_write_phy_register(chip, 0x1C, reg);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
 	} else {
 		chip->phy_debug_mode = 0;
 		retval = rtsx_write_register(chip, CDRESUMECTL, 0x77, 0x77);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
+
 		rtsx_enable_bus_int(chip);
 
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
+
 		reg &= 0xFFFE;
 		retval = rtsx_write_phy_register(chip, 0x1C, reg);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
 	}
 
 	return TRANSPORT_GOOD;
@@ -1737,9 +1702,8 @@ static int read_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (len) {
 		buf = (u8 *)vmalloc(len);
-		if (!buf) {
+		if (!buf)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		retval = rtsx_force_power_on(chip, SSC_PDCTL);
 		if (retval != STATUS_SUCCESS) {
@@ -1795,9 +1759,8 @@ static int write_phy_register(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
 
 		buf = (u8 *)vmalloc(len);
-		if (buf == NULL) {
+		if (buf == NULL)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		rtsx_stor_get_xfer_buf(buf, len, srb);
 		scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1886,9 +1849,8 @@ static int read_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = ((u16)srb->cmnd[6] << 8) | srb->cmnd[7];
 
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
@@ -1934,9 +1896,8 @@ static int write_eeprom2(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
 	buf = (u8 *)vmalloc(len);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -1980,9 +1941,8 @@ static int read_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	len = srb->cmnd[5];
 
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	retval = rtsx_force_power_on(chip, SSC_PDCTL);
 	if (retval != STATUS_SUCCESS) {
@@ -2029,9 +1989,8 @@ static int write_efuse(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (u8)min(scsi_bufflen(srb), (unsigned int)len);
 	buf = (u8 *)vmalloc(len);
-	if (buf == NULL) {
+	if (buf == NULL)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -2093,27 +2052,23 @@ Exit:
 	vfree(buf);
 
 	retval = card_power_off(chip, SPI_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	if (chip->asic_code) {
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_OFF);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		wait_timeout(600);
 
 		retval = rtsx_write_phy_register(chip, 0x08, val);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		retval = rtsx_write_register(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_ON);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 	}
 
 	return result;
@@ -2140,11 +2095,10 @@ static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	RTSX_DEBUGP("%s: func = %d, addr = 0x%x, len = %d\n", __func__, func, addr, len);
 
-	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
 		func_max = 1;
-	} else {
+	else
 		func_max = 0;
-	}
 
 	if (func > func_max) {
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
@@ -2152,9 +2106,8 @@ static int read_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	retval = rtsx_read_cfg_seq(chip, func, addr, buf, len);
 	if (retval != STATUS_SUCCESS) {
@@ -2193,11 +2146,10 @@ static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	RTSX_DEBUGP("%s: func = %d, addr = 0x%x\n", __func__, func, addr);
 
-	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
 		func_max = 1;
-	} else {
+	else
 		func_max = 0;
-	}
 
 	if (func > func_max) {
 		set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
@@ -2206,9 +2158,8 @@ static int write_cfg_byte(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
 	buf = (u8 *)vmalloc(len);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	rtsx_stor_get_xfer_buf(buf, len, srb);
 	scsi_set_resid(srb, scsi_bufflen(srb) - len);
@@ -2328,63 +2279,56 @@ static int read_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	rtsx_status[4] = (u8)lun;
 
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
-		if (chip->lun2card[lun] == SD_CARD) {
+		if (chip->lun2card[lun] == SD_CARD)
 			rtsx_status[5] = 2;
-		} else {
+		else
 			rtsx_status[5] = 3;
-		}
 	} else {
 		if (chip->card_exist) {
-			if (chip->card_exist & XD_CARD) {
+			if (chip->card_exist & XD_CARD)
 				rtsx_status[5] = 4;
-			} else if (chip->card_exist & SD_CARD) {
+			else if (chip->card_exist & SD_CARD)
 				rtsx_status[5] = 2;
-			} else if (chip->card_exist & MS_CARD) {
+			else if (chip->card_exist & MS_CARD)
 				rtsx_status[5] = 3;
-			} else {
+			else
 				rtsx_status[5] = 7;
-			}
 		} else {
 			rtsx_status[5] = 7;
 		}
 	}
 
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 		rtsx_status[6] = 2;
-	} else {
+	else
 		rtsx_status[6] = 1;
-	}
 
 	rtsx_status[7] = (u8)(chip->product_id);
 	rtsx_status[8] = chip->ic_version;
 
-	if (check_card_exist(chip, lun)) {
+	if (check_card_exist(chip, lun))
 		rtsx_status[9] = 1;
-	} else {
+	else
 		rtsx_status[9] = 0;
-	}
 
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 		rtsx_status[10] = 0;
-	} else {
+	else
 		rtsx_status[10] = 1;
-	}
 
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
-		if (chip->lun2card[lun] == SD_CARD) {
+		if (chip->lun2card[lun] == SD_CARD)
 			rtsx_status[11] = SD_CARD;
-		} else {
+		else
 			rtsx_status[11] = MS_CARD;
-		}
 	} else {
 		rtsx_status[11] = XD_CARD | SD_CARD | MS_CARD;
 	}
 
-	if (check_card_ready(chip, lun)) {
+	if (check_card_ready(chip, lun))
 		rtsx_status[12] = 1;
-	} else {
+	else
 		rtsx_status[12] = 0;
-	}
 
 	if (get_lun_card(chip, lun) == XD_CARD) {
 		rtsx_status[13] = 0x40;
@@ -2421,29 +2365,26 @@ static int read_status(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	} else {
 		if (CHECK_LUN_MODE(chip, DEFAULT_SINGLE)) {
 #ifdef SUPPORT_SDIO
-			if (chip->sd_io && chip->sd_int) {
+			if (chip->sd_io && chip->sd_int)
 				rtsx_status[13] = 0x60;
-			} else {
+			else
 				rtsx_status[13] = 0x70;
-			}
 #else
 			rtsx_status[13] = 0x70;
 #endif
 		} else {
-			if (chip->lun2card[lun] == SD_CARD) {
+			if (chip->lun2card[lun] == SD_CARD)
 				rtsx_status[13] = 0x20;
-			} else {
+			else
 				rtsx_status[13] = 0x30;
-			}
 		}
 	}
 
 	rtsx_status[14] = 0x78;
-	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip))
 		rtsx_status[15] = 0x83;
-	} else {
+	else
 		rtsx_status[15] = 0x82;
-	}
 
 	buf_len = min(scsi_bufflen(srb), (unsigned int)sizeof(rtsx_status));
 	rtsx_stor_set_xfer_buf(rtsx_status, buf_len, srb);
@@ -2482,7 +2423,7 @@ static int spi_vendor_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	unsigned int lun = SCSI_LUN(srb);
 	u8 gpio_dir;
 
-	if (CHECK_PID(chip, 0x5208) && CHECK_PID(chip, 0x5288)) {
+	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);
 	}
@@ -2538,9 +2479,8 @@ static int spi_vendor_cmd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	rtsx_write_register(chip, CARD_GPIO_DIR, 0x07, gpio_dir);
 
-	if (result != STATUS_SUCCESS) {
+	if (result != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 
 	return TRANSPORT_GOOD;
 }
@@ -2610,13 +2550,12 @@ void led_shine(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	unsigned int lun = SCSI_LUN(srb);
 	u16 sec_cnt;
 
-	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10)) {
+	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10))
 		sec_cnt = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
-	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
+	else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6))
 		sec_cnt = srb->cmnd[4];
-	} else {
+	else
 		return;
-	}
 
 	if (chip->rw_cap[lun] >= GPIO_TOGGLE_THRESHOLD) {
 		toggle_gpio(chip, LED_GPIO);
@@ -2659,11 +2598,10 @@ static int ms_format_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 	rtsx_set_stat(chip, RTSX_STAT_RUN);
 
-	if (srb->cmnd[8] & 0x01) {
+	if (srb->cmnd[8] & 0x01)
 		quick_format = 0;
-	} else {
+	else
 		quick_format = 1;
-	}
 
 	if (!(chip->card_ready & MS_CARD)) {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
@@ -2724,27 +2662,25 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		TRACE_RET(chip, TRANSPORT_FAILED);
 	}
 
-	if (dev_info_id == 0x15) {
+	if (dev_info_id == 0x15)
 		buf_len = data_len = 0x3A;
-	} else {
+	else
 		buf_len = data_len = 0x6A;
-	}
 
 	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (!buf) {
+	if (!buf)
 		TRACE_RET(chip, TRANSPORT_ERROR);
-	}
 
 	i = 0;
 	/*  GET Memory Stick Media Information Response Header */
 	buf[i++] = 0x00;		/* Data length MSB */
 	buf[i++] = data_len; 		/* Data length LSB */
 	/* Device Information Type Code */
-	if (CHK_MSXC(ms_card)) {
+	if (CHK_MSXC(ms_card))
 		buf[i++] = 0x03;
-	} else {
+	else
 		buf[i++] = 0x02;
-	}
+
 	/* SGM bit */
 	buf[i++] = 0x01;
 	/* Reserved */
@@ -2759,11 +2695,11 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	/* Device Information ID Number */
 	buf[i++] = dev_info_id;
 	/* Device Information Length */
-	if (dev_info_id == 0x15) {
+	if (dev_info_id == 0x15)
 		data_len = 0x31;
-	} else {
+	else
 		data_len = 0x61;
-	}
+
 	buf[i++] = 0x00;		/* Data length MSB */
 	buf[i++] = data_len; 		/* Data length LSB */
 	/* Valid Bit */
@@ -2778,11 +2714,10 @@ static int get_ms_information(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	rtsx_stor_set_xfer_buf(buf, buf_len, srb);
 
-	if (dev_info_id == 0x15) {
+	if (dev_info_id == 0x15)
 		scsi_set_resid(srb, scsi_bufflen(srb)-0x3C);
-	} else {
+	else
 		scsi_set_resid(srb, scsi_bufflen(srb)-0x6C);
-	}
 
 	kfree(buf);
 	return STATUS_SUCCESS;
@@ -2793,13 +2728,11 @@ static int ms_sp_cmnd(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 {
 	int retval = TRANSPORT_ERROR;
 
-	if (srb->cmnd[2] == MS_FORMAT) {
+	if (srb->cmnd[2] == MS_FORMAT)
 		retval = ms_format_cmnd(srb, chip);
-	}
 #ifdef SUPPORT_PCGL_1P18
-	else if (srb->cmnd[2] == GET_MS_INFORMATION) {
+	else if (srb->cmnd[2] == GET_MS_INFORMATION)
 		retval = get_ms_information(srb, chip);
-	}
 #endif
 
 	return retval;
@@ -2912,9 +2845,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -2926,9 +2859,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -2945,9 +2878,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -3014,9 +2947,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -3028,9 +2961,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -3042,9 +2975,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
@@ -3061,9 +2994,9 @@ 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
+
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
 			TRACE_RET(chip, TRANSPORT_FAILED);
diff --git a/drivers/staging/rts_pstor/rtsx_transport.c b/drivers/staging/rts_pstor/rtsx_transport.c
index 54a474235f26..1f9a42480443 100644
--- a/drivers/staging/rts_pstor/rtsx_transport.c
+++ b/drivers/staging/rts_pstor/rtsx_transport.c
@@ -218,9 +218,9 @@ void rtsx_add_cmd(struct rtsx_chip *chip,
 	val |= (u32)data;
 
 	spin_lock_irq(&chip->rtsx->reg_lock);
-	if (chip->ci < (HOST_CMDS_BUF_LEN / 4)) {
+	if (chip->ci < (HOST_CMDS_BUF_LEN / 4))
 		cb[(chip->ci)++] = cpu_to_le32(val);
-	}
+
 	spin_unlock_irq(&chip->rtsx->reg_lock);
 }
 
@@ -244,15 +244,14 @@ int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout)
 	long timeleft;
 	int err = 0;
 
-	if (card == SD_CARD) {
+	if (card == SD_CARD)
 		rtsx->check_card_cd = SD_EXIST;
-	} else if (card == MS_CARD) {
+	else if (card == MS_CARD)
 		rtsx->check_card_cd = MS_EXIST;
-	} else if (card == XD_CARD) {
+	else if (card == XD_CARD)
 		rtsx->check_card_cd = XD_EXIST;
-	} else {
+	else
 		rtsx->check_card_cd = 0;
-	}
 
 	spin_lock_irq(&rtsx->reg_lock);
 
@@ -281,11 +280,11 @@ int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout)
 	}
 
 	spin_lock_irq(&rtsx->reg_lock);
-	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
 		err = -EIO;
-	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
 		err = 0;
-	}
+
 	spin_unlock_irq(&rtsx->reg_lock);
 
 finish_send_cmd:
@@ -341,23 +340,21 @@ static int rtsx_transfer_sglist_adma_partial(struct rtsx_chip *chip, u8 card,
 	if ((sg == NULL) || (num_sg <= 0) || !offset || !index)
 		return -EIO;
 
-	if (dma_dir == DMA_TO_DEVICE) {
+	if (dma_dir == DMA_TO_DEVICE)
 		dir = HOST_TO_DEVICE;
-	} else if (dma_dir == DMA_FROM_DEVICE) {
+	else if (dma_dir == DMA_FROM_DEVICE)
 		dir = DEVICE_TO_HOST;
-	} else {
+	else
 		return -ENXIO;
-	}
 
-	if (card == SD_CARD) {
+	if (card == SD_CARD)
 		rtsx->check_card_cd = SD_EXIST;
-	} else if (card == MS_CARD) {
+	else if (card == MS_CARD)
 		rtsx->check_card_cd = MS_EXIST;
-	} else if (card == XD_CARD) {
+	else if (card == XD_CARD)
 		rtsx->check_card_cd = XD_EXIST;
-	} else {
+	else
 		rtsx->check_card_cd = 0;
-	}
 
 	spin_lock_irq(&rtsx->reg_lock);
 
@@ -405,11 +402,10 @@ static int rtsx_transfer_sglist_adma_partial(struct rtsx_chip *chip, u8 card,
 			*offset = 0;
 			*index = *index + 1;
 		}
-		if ((i == (sg_cnt - 1)) || !resid) {
+		if ((i == (sg_cnt - 1)) || !resid)
 			option = SG_VALID | SG_END | SG_TRANS_DATA;
-		} else {
+		else
 			option = SG_VALID | SG_TRANS_DATA;
-		}
 
 		rtsx_add_sg_tbl(chip, (u32)addr, (u32)len, option);
 
@@ -468,11 +464,11 @@ static int rtsx_transfer_sglist_adma_partial(struct rtsx_chip *chip, u8 card,
 	}
 
 	spin_lock_irq(&rtsx->reg_lock);
-	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
 		err = -EIO;
-	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
 		err = 0;
-	}
+
 	spin_unlock_irq(&rtsx->reg_lock);
 
 out:
@@ -501,23 +497,21 @@ static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
 	if ((sg == NULL) || (num_sg <= 0))
 		return -EIO;
 
-	if (dma_dir == DMA_TO_DEVICE) {
+	if (dma_dir == DMA_TO_DEVICE)
 		dir = HOST_TO_DEVICE;
-	} else if (dma_dir == DMA_FROM_DEVICE) {
+	else if (dma_dir == DMA_FROM_DEVICE)
 		dir = DEVICE_TO_HOST;
-	} else {
+	else
 		return -ENXIO;
-	}
 
-	if (card == SD_CARD) {
+	if (card == SD_CARD)
 		rtsx->check_card_cd = SD_EXIST;
-	} else if (card == MS_CARD) {
+	else if (card == MS_CARD)
 		rtsx->check_card_cd = MS_EXIST;
-	} else if (card == XD_CARD) {
+	else if (card == XD_CARD)
 		rtsx->check_card_cd = XD_EXIST;
-	} else {
+	else
 		rtsx->check_card_cd = 0;
-	}
 
 	spin_lock_irq(&rtsx->reg_lock);
 
@@ -537,11 +531,10 @@ static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
 		u32 val = TRIG_DMA;
 		int sg_cnt, j;
 
-		if (i == buf_cnt / (HOST_SG_TBL_BUF_LEN / 8)) {
+		if (i == buf_cnt / (HOST_SG_TBL_BUF_LEN / 8))
 			sg_cnt = buf_cnt % (HOST_SG_TBL_BUF_LEN / 8);
-		} else {
+		else
 			sg_cnt = (HOST_SG_TBL_BUF_LEN / 8);
-		}
 
 		chip->sgi = 0;
 		for (j = 0; j < sg_cnt; j++) {
@@ -552,11 +545,10 @@ static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
 			RTSX_DEBUGP("DMA addr: 0x%x, Len: 0x%x\n",
 				     (unsigned int)addr, len);
 
-			if (j == (sg_cnt - 1)) {
+			if (j == (sg_cnt - 1))
 				option = SG_VALID | SG_END | SG_TRANS_DATA;
-			} else {
+			else
 				option = SG_VALID | SG_TRANS_DATA;
-			}
 
 			rtsx_add_sg_tbl(chip, (u32)addr, (u32)len, option);
 
@@ -615,11 +607,11 @@ static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
 	}
 
 	spin_lock_irq(&rtsx->reg_lock);
-	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
 		err = -EIO;
-	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
 		err = 0;
-	}
+
 	spin_unlock_irq(&rtsx->reg_lock);
 
 out:
@@ -647,27 +639,25 @@ static int rtsx_transfer_buf(struct rtsx_chip *chip, u8 card, void *buf, size_t
 	if ((buf == NULL) || (len <= 0))
 		return -EIO;
 
-	if (dma_dir == DMA_TO_DEVICE) {
+	if (dma_dir == DMA_TO_DEVICE)
 		dir = HOST_TO_DEVICE;
-	} else if (dma_dir == DMA_FROM_DEVICE) {
+	else if (dma_dir == DMA_FROM_DEVICE)
 		dir = DEVICE_TO_HOST;
-	} else {
+	else
 		return -ENXIO;
-	}
 
 	addr = dma_map_single(&(rtsx->pci->dev), buf, len, dma_dir);
 	if (!addr)
 		return -ENOMEM;
 
-	if (card == SD_CARD) {
+	if (card == SD_CARD)
 		rtsx->check_card_cd = SD_EXIST;
-	} else if (card == MS_CARD) {
+	else if (card == MS_CARD)
 		rtsx->check_card_cd = MS_EXIST;
-	} else if (card == XD_CARD) {
+	else if (card == XD_CARD)
 		rtsx->check_card_cd = XD_EXIST;
-	} else {
+	else
 		rtsx->check_card_cd = 0;
-	}
 
 	val |= (u32)(dir & 0x01) << 29;
 	val |= (u32)(len & 0x00FFFFFF);
@@ -698,11 +688,11 @@ static int rtsx_transfer_buf(struct rtsx_chip *chip, u8 card, void *buf, size_t
 	}
 
 	spin_lock_irq(&rtsx->reg_lock);
-	if (rtsx->trans_result == TRANS_RESULT_FAIL) {
+	if (rtsx->trans_result == TRANS_RESULT_FAIL)
 		err = -EIO;
-	} else if (rtsx->trans_result == TRANS_RESULT_OK) {
+	else if (rtsx->trans_result == TRANS_RESULT_OK)
 		err = 0;
-	}
+
 	spin_unlock_irq(&rtsx->reg_lock);
 
 out:
diff --git a/drivers/staging/rts_pstor/sd.c b/drivers/staging/rts_pstor/sd.c
index 3cc9a489e4e8..c6a581c47cbc 100644
--- a/drivers/staging/rts_pstor/sd.c
+++ b/drivers/staging/rts_pstor/sd.c
@@ -192,9 +192,9 @@ RTY_SEND_CMD:
 
 		stat_idx = 16;
 	} else if (rsp_type != SD_RSP_TYPE_R0) {
-		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-		}
+
 		stat_idx = 5;
 	}
 
@@ -273,9 +273,8 @@ RTY_SEND_CMD:
 	if ((rsp_type == SD_RSP_TYPE_R1) || (rsp_type == SD_RSP_TYPE_R1b)) {
 		if ((cmd_idx != SEND_RELATIVE_ADDR) && (cmd_idx != SEND_IF_COND)) {
 			if (cmd_idx != STOP_TRANSMISSION) {
-				if (ptr[1] & 0x80) {
+				if (ptr[1] & 0x80)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 #ifdef SUPPORT_SD_LOCK
 			if (ptr[1] & 0x7D)
@@ -294,11 +293,10 @@ RTY_SEND_CMD:
 				RTSX_DEBUGP("ptr[3]: 0x%02x\n", ptr[3]);
 				TRACE_RET(chip, STATUS_FAIL);
 			}
-			if (ptr[3] & 0x01) {
+			if (ptr[3] & 0x01)
 				sd_card->sd_data_buf_ready = 1;
-			} else {
+			else
 				sd_card->sd_data_buf_ready = 0;
-			}
 		}
 	}
 
@@ -322,17 +320,15 @@ static int sd_read_data(struct rtsx_chip *chip,
 	if (!buf)
 		buf_len = 0;
 
-	if (buf_len > 512) {
+	if (buf_len > 512)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	rtsx_init_cmd(chip);
 
 	if (cmd_len) {
 		RTSX_DEBUGP("SD/MMC CMD %d\n", cmd[0] - 0x40);
-		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
+		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++)
 			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0 + i, 0xFF, cmd[i]);
-		}
 	}
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF, (u8)byte_cnt);
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_H, 0xFF, (u8)(byte_cnt >> 8));
@@ -344,9 +340,9 @@ static int sd_read_data(struct rtsx_chip *chip,
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF,
 			SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
 			SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	if (trans_mode != SD_TM_AUTO_TUNING) {
+	if (trans_mode != SD_TM_AUTO_TUNING)
 		rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
-	}
+
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_TRANSFER, 0xFF, trans_mode | SD_TRANSFER_START);
 	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END, SD_TRANSFER_END);
 
@@ -362,9 +358,8 @@ static int sd_read_data(struct rtsx_chip *chip,
 
 	if (buf && buf_len) {
 		retval = rtsx_read_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -390,9 +385,8 @@ static int sd_write_data(struct rtsx_chip *chip, u8 trans_mode,
 
 	if (buf && buf_len) {
 		retval = rtsx_write_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	rtsx_init_cmd(chip);
@@ -450,15 +444,13 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 			break;
 	}
 
-	if (i == 6) {
+	if (i == 6)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	memcpy(sd_card->raw_csd, rsp + 1, 15);
 
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		RTSX_READ_REG(chip, REG_SD_CMD5, sd_card->raw_csd + 15);
-	}
 
 	RTSX_DEBUGP("CSD Response:\n");
 	RTSX_DUMP(sd_card->raw_csd, 16);
@@ -469,35 +461,34 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 	trans_speed = rsp[4];
 	if ((trans_speed & 0x07) == 0x02) {
 		if ((trans_speed & 0xf8) >= 0x30) {
-			if (chip->asic_code) {
+			if (chip->asic_code)
 				sd_card->sd_clock = 47;
-			} else {
+			else
 				sd_card->sd_clock = CLK_50;
-			}
+
 		} else if ((trans_speed & 0xf8) == 0x28) {
-			if (chip->asic_code) {
+			if (chip->asic_code)
 				sd_card->sd_clock = 39;
-			} else {
+			else
 				sd_card->sd_clock = CLK_40;
-			}
+
 		} else if ((trans_speed & 0xf8) == 0x20) {
-			if (chip->asic_code) {
+			if (chip->asic_code)
 				sd_card->sd_clock = 29;
-			} else {
+			else
 				sd_card->sd_clock = CLK_30;
-			}
+
 		} else if ((trans_speed & 0xf8) >= 0x10) {
-			if (chip->asic_code) {
+			if (chip->asic_code)
 				sd_card->sd_clock = 23;
-			} else {
+			else
 				sd_card->sd_clock = CLK_20;
-			}
+
 		} else if ((trans_speed & 0x08) >= 0x08) {
-			if (chip->asic_code) {
+			if (chip->asic_code)
 				sd_card->sd_clock = 19;
-			} else {
+			else
 				sd_card->sd_clock = CLK_20;
-			}
 		} else {
 			TRACE_RET(chip, STATUS_FAIL);
 		}
@@ -527,9 +518,9 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 	}
 
 	if (check_wp) {
-		if (rsp[15] & 0x30) {
+		if (rsp[15] & 0x30)
 			chip->card_wp |= SD_CARD;
-		}
+
 		RTSX_DEBUGP("CSD WP Status: 0x%x\n", rsp[15]);
 	}
 
@@ -568,22 +559,21 @@ static int sd_set_sample_push_timing(struct rtsx_chip *chip)
 					CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
 			RTSX_WRITE_REG(chip, CLK_CTL, CLK_LOW_FREQ, 0);
 
-			if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_AUTO) {
+			if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_AUTO)
 				val = SD20_TX_NEG_EDGE;
-			} else if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY) {
+			else if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY)
 				val = SD20_TX_14_AHEAD;
-			} else {
+			else
 				val = SD20_TX_NEG_EDGE;
-			}
+
 			RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, SD20_TX_SEL_MASK, val);
 
 			if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) == SD_SAMPLE_POINT_AUTO) {
 				if (chip->asic_code) {
-					if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card)) {
+					if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card))
 						val = SD20_RX_14_DELAY;
-					} else {
+					else
 						val = SD20_RX_POS_EDGE;
-					}
 				} else {
 					val = SD20_RX_14_DELAY;
 				}
@@ -597,32 +587,28 @@ static int sd_set_sample_push_timing(struct rtsx_chip *chip)
 	} else {
 		u8 val = 0;
 
-		if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY) {
+		if ((chip->sd_ctl & SD_PUSH_POINT_CTL_MASK) == SD_PUSH_POINT_DELAY)
 			val |= 0x10;
-		}
 
 		if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) == SD_SAMPLE_POINT_AUTO) {
 			if (chip->asic_code) {
 				if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card)) {
-					if (val & 0x10) {
+					if (val & 0x10)
 						val |= 0x04;
-					} else {
+					else
 						val |= 0x08;
-					}
 				}
 			} else {
-				if (val & 0x10) {
+				if (val & 0x10)
 					val |= 0x04;
-				} else {
+				else
 					val |= 0x08;
-				}
 			}
 		} else if ((chip->sd_ctl & SD_SAMPLE_POINT_CTL_MASK) == SD_SAMPLE_POINT_DELAY) {
-			if (val & 0x10) {
+			if (val & 0x10)
 				val |= 0x04;
-			} else {
+			else
 				val |= 0x08;
-			}
 		}
 
 		RTSX_WRITE_REG(chip, REG_SD_CFG1, 0x1C, val);
@@ -636,41 +622,40 @@ static void sd_choose_proper_clock(struct rtsx_chip *chip)
 	struct sd_info *sd_card = &(chip->sd_card);
 
 	if (CHK_SD_SDR104(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = chip->asic_sd_sdr104_clk;
-		} else {
+		else
 			sd_card->sd_clock = chip->fpga_sd_sdr104_clk;
-		}
+
 	} else if (CHK_SD_DDR50(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = chip->asic_sd_ddr50_clk;
-		} else {
+		else
 			sd_card->sd_clock = chip->fpga_sd_ddr50_clk;
-		}
+
 	} else if (CHK_SD_SDR50(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = chip->asic_sd_sdr50_clk;
-		} else {
+		else
 			sd_card->sd_clock = chip->fpga_sd_sdr50_clk;
-		}
+
 	} else if (CHK_SD_HS(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = chip->asic_sd_hs_clk;
-		} else {
+		else
 			sd_card->sd_clock = chip->fpga_sd_hs_clk;
-		}
+
 	} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = chip->asic_mmc_52m_clk;
-		} else {
+		else
 			sd_card->sd_clock = chip->fpga_mmc_52m_clk;
-		}
+
 	} else if (CHK_MMC_26M(sd_card)) {
-		if (chip->asic_code) {
+		if (chip->asic_code)
 			sd_card->sd_clock = 48;
-		} else {
+		else
 			sd_card->sd_clock = CLK_50;
-		}
 	}
 }
 
@@ -683,13 +668,12 @@ static int sd_set_clock_divider(struct rtsx_chip *chip, u8 clk_div)
 		val = clk_div;
 	} else {
 		mask = 0x60;
-		if (clk_div == SD_CLK_DIVIDE_0) {
+		if (clk_div == SD_CLK_DIVIDE_0)
 			val = 0x00;
-		} else if (clk_div == SD_CLK_DIVIDE_128) {
+		else if (clk_div == SD_CLK_DIVIDE_128)
 			val = 0x40;
-		} else if (clk_div == SD_CLK_DIVIDE_256) {
+		else if (clk_div == SD_CLK_DIVIDE_256)
 			val = 0x20;
-		}
 	}
 
 	RTSX_WRITE_REG(chip, REG_SD_CFG1, mask, val);
@@ -703,16 +687,14 @@ static int sd_set_init_para(struct rtsx_chip *chip)
 	int retval;
 
 	retval = sd_set_sample_push_timing(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	sd_choose_proper_clock(chip);
 
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -735,9 +717,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -750,21 +731,18 @@ static int sd_update_lock_status(struct rtsx_chip *chip)
 	u8 rsp[5];
 
 	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1, rsp, 5);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (rsp[1] & 0x02) {
+	if (rsp[1] & 0x02)
 		sd_card->sd_lock_status |= SD_LOCKED;
-	} else {
+	else
 		sd_card->sd_lock_status &= ~SD_LOCKED;
-	}
 
 	RTSX_DEBUGP("sd_card->sd_lock_status = 0x%x\n", sd_card->sd_lock_status);
 
-	if (rsp[1] & 0x01) {
+	if (rsp[1] & 0x01)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -779,13 +757,11 @@ static int sd_wait_state_data_ready(struct rtsx_chip *chip, u8 state, u8 data_re
 	for (i = 0; i < polling_cnt; i++) {
 		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
 					     sd_card->sd_addr, SD_RSP_TYPE_R1, rsp, 5);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
-		if (((rsp[3] & 0x1E) == state) && ((rsp[3] & 0x01) == data_ready)) {
+		if (((rsp[3] & 0x1E) == state) && ((rsp[3] & 0x01) == data_ready))
 			return STATUS_SUCCESS;
-		}
 	}
 
 	TRACE_RET(chip, STATUS_FAIL);
@@ -798,18 +774,16 @@ static int sd_change_bank_voltage(struct rtsx_chip *chip, u8 voltage)
 	if (voltage == SD_IO_3V3) {
 		if (chip->asic_code) {
 			retval = rtsx_write_phy_register(chip, 0x08, 0x4FC0 | chip->phy_voltage);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		} else {
 			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8, 0);
 		}
 	} 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		} else {
 			RTSX_WRITE_REG(chip, SD_PAD_CTL, SD_IO_USING_1V8, SD_IO_USING_1V8);
 		}
@@ -828,9 +802,8 @@ static int sd_voltage_switch(struct rtsx_chip *chip)
 	RTSX_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, SD_CLK_TOGGLE_EN);
 
 	retval = sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	udelay(chip->sd_voltage_switch_delay);
 
@@ -842,9 +815,9 @@ static int sd_voltage_switch(struct rtsx_chip *chip)
 
 	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_FORCE_STOP);
 	retval = sd_change_bank_voltage(chip, SD_IO_1V8);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	wait_timeout(50);
 
 	RTSX_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_TOGGLE_EN);
@@ -893,9 +866,8 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 	if (tune_dir == TUNE_RX) {
 		SD_VP_CTL = SD_VPRX_CTL;
 		SD_DCMPS_CTL = SD_DCMPS_RX_CTL;
-		if (CHK_SD_DDR50(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
 			ddr_rx = 1;
-		}
 	} else {
 		SD_VP_CTL = SD_VPTX_CTL;
 		SD_DCMPS_CTL = SD_DCMPS_TX_CTL;
@@ -932,23 +904,22 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 		rtsx_add_cmd(chip, WRITE_REG_CMD, SD_DCMPS_CTL, DCMPS_CHANGE, DCMPS_CHANGE);
 		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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, Fail);
-		}
 
 		val = *rtsx_get_cmd_data(chip);
-		if (val & DCMPS_ERROR) {
+		if (val & DCMPS_ERROR)
 			TRACE_GOTO(chip, Fail);
-		}
-		if ((val & DCMPS_CURRENT_PHASE) != sample_point) {
+
+		if ((val & DCMPS_CURRENT_PHASE) != sample_point)
 			TRACE_GOTO(chip, Fail);
-		}
+
 		RTSX_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-		if (ddr_rx) {
+		if (ddr_rx)
 			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE, 0);
-		} else {
+		else
 			RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
-		}
+
 		udelay(50);
 	}
 
@@ -978,9 +949,8 @@ static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
 	u8 cmd[5], buf[8];
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	cmd[0] = 0x40 | SEND_SCR;
 	cmd[1] = 0;
@@ -996,9 +966,8 @@ static int sd_check_spec(struct rtsx_chip *chip, u8 bus_width)
 
 	memcpy(sd_card->raw_scr, buf, 8);
 
-	if ((buf[0] & 0x0F) == 0) {
+	if ((buf[0] & 0x0F) == 0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1172,13 +1141,12 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 		 */
 		u16 cc = ((u16)buf[0] << 8) | buf[1];
 		RTSX_DEBUGP("Maximum current consumption: %dmA\n", cc);
-		if ((cc == 0) || (cc > 800)) {
+		if ((cc == 0) || (cc > 800))
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = sd_query_switch_result(chip, func_group, func_to_switch, buf, 64);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, 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);
@@ -1192,13 +1160,12 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 static u8 downgrade_switch_mode(u8 func_group, u8 func_to_switch)
 {
 	if (func_group == SD_FUNC_GROUP_1) {
-		if (func_to_switch > HS_SUPPORT) {
+		if (func_to_switch > HS_SUPPORT)
 			func_to_switch--;
-		}
+
 	} else if (func_group == SD_FUNC_GROUP_4) {
-		if (func_to_switch > CURRENT_LIMIT_200) {
+		if (func_to_switch > CURRENT_LIMIT_200)
 			func_to_switch--;
-		}
 	}
 
 	return func_to_switch;
@@ -1241,9 +1208,8 @@ static int sd_check_switch(struct rtsx_chip *chip,
 		wait_timeout(20);
 	}
 
-	if (!switch_good) {
+	if (!switch_good)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1258,9 +1224,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
 
@@ -1289,9 +1254,9 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 			break;
 
 		case HS_SUPPORT:
-			if (sd_card->func_group1_mask & HS_SUPPORT_MASK) {
+			if (sd_card->func_group1_mask & HS_SUPPORT_MASK)
 				func_to_switch = HS_SUPPORT;
-			}
+
 			break;
 
 		default:
@@ -1299,9 +1264,9 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 		}
 
 
-		if (func_to_switch) {
+		if (func_to_switch)
 			break;
-		}
+
 	}
 	RTSX_DEBUGP("SD_FUNC_GROUP_1: func_to_switch = 0x%02x", func_to_switch);
 
@@ -1330,23 +1295,21 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 
-		if (func_to_switch == SDR104_SUPPORT) {
+		if (func_to_switch == SDR104_SUPPORT)
 			SET_SD_SDR104(sd_card);
-		} else if (func_to_switch == DDR50_SUPPORT) {
+		else if (func_to_switch == DDR50_SUPPORT)
 			SET_SD_DDR50(sd_card);
-		} else if (func_to_switch == SDR50_SUPPORT) {
+		else if (func_to_switch == SDR50_SUPPORT)
 			SET_SD_SDR50(sd_card);
-		} else {
+		else
 			SET_SD_HS(sd_card);
-		}
 	}
 
 	if (CHK_SD_DDR50(sd_card)) {
 		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0x04);
 		retval = sd_set_sample_push_timing(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	if (!func_to_switch || (func_to_switch == HS_SUPPORT)) {
@@ -1362,36 +1325,35 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 	for (i = 0; i < 4; i++) {
 		switch ((u8)(chip->sd_current_prior >> (i*8))) {
 		case CURRENT_LIMIT_800:
-			if (sd_card->func_group4_mask & CURRENT_LIMIT_800_MASK) {
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_800_MASK)
 				func_to_switch = CURRENT_LIMIT_800;
-			}
+
 			break;
 
 		case CURRENT_LIMIT_600:
-			if (sd_card->func_group4_mask & CURRENT_LIMIT_600_MASK) {
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_600_MASK)
 				func_to_switch = CURRENT_LIMIT_600;
-			}
+
 			break;
 
 		case CURRENT_LIMIT_400:
-			if (sd_card->func_group4_mask & CURRENT_LIMIT_400_MASK) {
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_400_MASK)
 				func_to_switch = CURRENT_LIMIT_400;
-			}
+
 			break;
 
 		case CURRENT_LIMIT_200:
-			if (sd_card->func_group4_mask & CURRENT_LIMIT_200_MASK) {
+			if (sd_card->func_group4_mask & CURRENT_LIMIT_200_MASK)
 				func_to_switch = CURRENT_LIMIT_200;
-			}
+
 			break;
 
 		default:
 			continue;
 		}
 
-		if (func_to_switch != 0xFF) {
+		if (func_to_switch != 0xFF)
 			break;
-		}
 	}
 
 	RTSX_DEBUGP("SD_FUNC_GROUP_4: func_to_switch = 0x%02x", func_to_switch);
@@ -1399,16 +1361,14 @@ static int sd_switch_function(struct rtsx_chip *chip, u8 bus_width)
 	if (func_to_switch <= CURRENT_LIMIT_800) {
 		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)) {
+			if (sd_check_err_code(chip, SD_NO_CARD))
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 		RTSX_DEBUGP("Switch current limit finished! (%d)\n", retval);
 	}
 
-	if (CHK_SD_DDR50(sd_card)) {
+	if (CHK_SD_DDR50(sd_card))
 		RTSX_WRITE_REG(chip, SD_PUSH_POINT_CTL, 0x06, 0);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1438,9 +1398,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
 	cmd[1] = 0;
@@ -1467,16 +1426,14 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("sd ddr tuning rx\n");
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	cmd[0] = 0x40 | SD_STATUS;
 	cmd[1] = 0;
@@ -1502,18 +1459,16 @@ static int mmc_ddr_tunning_rx_cmd(struct rtsx_chip *chip, u8 sample_point)
 	int retval;
 	u8 cmd[5], bus_width;
 
-	if (CHK_MMC_8BIT(sd_card)) {
+	if (CHK_MMC_8BIT(sd_card))
 		bus_width = SD_BUS_WIDTH_8;
-	} else if (CHK_MMC_4BIT(sd_card)) {
+	else if (CHK_MMC_4BIT(sd_card))
 		bus_width = SD_BUS_WIDTH_4;
-	} else {
+	else
 		bus_width = SD_BUS_WIDTH_1;
-	}
 
 	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("mmc ddr tuning rx\n");
 
@@ -1541,9 +1496,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, SD_RSP_80CLK_TIMEOUT_EN);
 
@@ -1568,26 +1522,23 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHK_SD(sd_card)) {
 		bus_width = SD_BUS_WIDTH_4;
 	} else {
-		if (CHK_MMC_8BIT(sd_card)) {
+		if (CHK_MMC_8BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_8;
-		} else if (CHK_MMC_4BIT(sd_card)) {
+		else if (CHK_MMC_4BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_4;
-		} else {
+		else
 			bus_width = SD_BUS_WIDTH_1;
-		}
 	}
 
 	retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, SD_RSP_80CLK_TIMEOUT_EN);
 
@@ -1621,11 +1572,10 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map, u8 tune_d
 	u8 final_phase = 0xFF;
 
 	if (phase_map == 0xFFFFFFFF) {
-		if (tune_dir == TUNE_RX) {
+		if (tune_dir == TUNE_RX)
 			final_phase = (u8)chip->sd_default_rx_phase;
-		} else {
+		else
 			final_phase = (u8)chip->sd_default_tx_phase;
-		}
 
 		goto Search_Finish;
 	}
@@ -1666,9 +1616,9 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map, u8 tune_d
 		path[0].start = path[cont_path_cnt - 1].start - MAX_PHASE - 1;
 		path[0].len += path[cont_path_cnt - 1].len;
 		path[0].mid = path[0].start + path[0].len / 2;
-		if (path[0].mid < 0) {
+		if (path[0].mid < 0)
 			path[0].mid += MAX_PHASE + 1;
-		}
+
 		cont_path_cnt--;
 	}
 
@@ -1696,11 +1646,10 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map, u8 tune_d
 				int temp_final_phase =
 					path[final_path_idx].end - (max_len - (6 + temp_mid));
 
-				if (temp_final_phase < 0) {
+				if (temp_final_phase < 0)
 					final_phase = (u8)(temp_final_phase + MAX_PHASE + 1);
-				} else {
+				else
 					final_phase = (u8)temp_final_phase;
-				}
 			}
 		} else if (CHK_SD_SDR50(sd_card)) {
 			if (max_len > 12) {
@@ -1708,11 +1657,10 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map, u8 tune_d
 				int temp_final_phase =
 					path[final_path_idx].end - (max_len - (3 + temp_mid));
 
-				if (temp_final_phase < 0) {
+				if (temp_final_phase < 0)
 					final_phase = (u8)(temp_final_phase + MAX_PHASE + 1);
-				} else {
+				else
 					final_phase = (u8)temp_final_phase;
-				}
 			}
 		}
 	}
@@ -1732,17 +1680,16 @@ static int sd_tuning_rx(struct rtsx_chip *chip)
 	int (*tuning_cmd)(struct rtsx_chip *chip, u8 sample_point);
 
 	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
 			tuning_cmd = sd_ddr_tuning_rx_cmd;
-		} else {
+		else
 			tuning_cmd = sd_sdr_tuning_rx_cmd;
-		}
+
 	} else {
-		if (CHK_MMC_DDR52(sd_card)) {
+		if (CHK_MMC_DDR52(sd_card))
 			tuning_cmd = mmc_ddr_tunning_rx_cmd;
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	for (i = 0; i < 3; i++) {
@@ -1754,27 +1701,24 @@ static int sd_tuning_rx(struct rtsx_chip *chip)
 			}
 
 			retval = tuning_cmd(chip, (u8)j);
-			if (retval == STATUS_SUCCESS) {
+			if (retval == STATUS_SUCCESS)
 				raw_phase_map[i] |= 1 << j;
-			}
 		}
 	}
 
 	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++) {
+	for (i = 0; i < 3; i++)
 		RTSX_DEBUGP("RX raw_phase_map[%d] = 0x%08x\n", i, raw_phase_map[i]);
-	}
+
 	RTSX_DEBUGP("RX phase_map = 0x%08x\n", phase_map);
 
 	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
-	if (final_phase == 0xFF) {
+	if (final_phase == 0xFF)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_RX);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1799,15 +1743,13 @@ static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
 		}
 
 		retval = sd_change_phase(chip, (u8)i, TUNE_TX);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			continue;
-		}
 
 		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0);
-		if ((retval == STATUS_SUCCESS) || !sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
+		if ((retval == STATUS_SUCCESS) || !sd_check_err_code(chip, SD_RSP_TIMEOUT))
 			phase_map |= 1 << i;
-		}
 	}
 
 	RTSX_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
@@ -1815,14 +1757,12 @@ static int sd_ddr_pre_tuning_tx(struct rtsx_chip *chip)
 	RTSX_DEBUGP("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) {
+	if (final_phase == 0xFF)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_TX);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("DDR TX pre tune phase: %d\n", (int)final_phase);
 
@@ -1839,17 +1779,16 @@ static int sd_tuning_tx(struct rtsx_chip *chip)
 	int (*tuning_cmd)(struct rtsx_chip *chip, u8 sample_point);
 
 	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
 			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		} else {
+		else
 			tuning_cmd = sd_sdr_tuning_tx_cmd;
-		}
+
 	} else {
-		if (CHK_MMC_DDR52(sd_card)) {
+		if (CHK_MMC_DDR52(sd_card))
 			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		} else {
+		else
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	for (i = 0; i < 3; i++) {
@@ -1863,27 +1802,24 @@ static int sd_tuning_tx(struct rtsx_chip *chip)
 			}
 
 			retval = tuning_cmd(chip, (u8)j);
-			if (retval == STATUS_SUCCESS) {
+			if (retval == STATUS_SUCCESS)
 				raw_phase_map[i] |= 1 << j;
-			}
 		}
 	}
 
 	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++) {
+	for (i = 0; i < 3; i++)
 		RTSX_DEBUGP("TX raw_phase_map[%d] = 0x%08x\n", i, raw_phase_map[i]);
-	}
+
 	RTSX_DEBUGP("TX phase_map = 0x%08x\n", phase_map);
 
 	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
-	if (final_phase == 0xFF) {
+	if (final_phase == 0xFF)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_change_phase(chip, final_phase, TUNE_TX);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1893,14 +1829,12 @@ static int sd_sdr_tuning(struct rtsx_chip *chip)
 	int retval;
 
 	retval = sd_tuning_tx(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -1911,26 +1845,22 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		retval = sd_change_phase(chip, (u8)chip->sd_ddr_tx_phase, TUNE_TX);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (!(chip->sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1942,26 +1872,22 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		retval = sd_change_phase(chip, (u8)chip->mmc_ddr_tx_phase, TUNE_TX);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (!(chip->sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
 		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -1974,9 +1900,8 @@ int sd_switch_clock(struct rtsx_chip *chip)
 	int re_tuning = 0;
 
 	retval = select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHECK_PID(chip, 0x5209) &&
 			(CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card))) {
@@ -1987,26 +1912,22 @@ int sd_switch_clock(struct rtsx_chip *chip)
 	}
 
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (re_tuning) {
 		if (CHK_SD(sd_card)) {
-			if (CHK_SD_DDR50(sd_card)) {
+			if (CHK_SD_DDR50(sd_card))
 				retval = sd_ddr_tuning(chip);
-			} else {
+			else
 				retval = sd_sdr_tuning(chip);
-			}
 		} else {
-			if (CHK_MMC_DDR52(sd_card)) {
+			if (CHK_MMC_DDR52(sd_card))
 				retval = mmc_ddr_tuning(chip);
-			}
 		}
 
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	return STATUS_SUCCESS;
@@ -2017,11 +1938,10 @@ static int sd_prepare_reset(struct rtsx_chip *chip)
 	struct sd_info *sd_card = &(chip->sd_card);
 	int retval;
 
-	if (chip->asic_code) {
+	if (chip->asic_code)
 		sd_card->sd_clock = 29;
-	} else {
+	else
 		sd_card->sd_clock = CLK_30;
-	}
 
 	sd_card->sd_type = 0;
 	sd_card->seq_mode = 0;
@@ -2037,9 +1957,8 @@ static int sd_prepare_reset(struct rtsx_chip *chip)
 	chip->sd_io = 0;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
-	}
 
 	if (CHECK_PID(chip, 0x5209)) {
 		RTSX_WRITE_REG(chip, REG_SD_CFG1, 0xFF,
@@ -2053,9 +1972,8 @@ static int sd_prepare_reset(struct rtsx_chip *chip)
 	RTSX_WRITE_REG(chip, CARD_STOP, SD_STOP | SD_CLR_ERR, SD_STOP | SD_CLR_ERR);
 
 	retval = select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2122,9 +2040,8 @@ int sd_pull_ctl_enable(struct rtsx_chip *chip)
 	}
 
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
-	if (retval < 0) {
+	if (retval < 0)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2133,42 +2050,37 @@ static int sd_init_power(struct rtsx_chip *chip)
 {
 	int retval;
 
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_OFF);
-	}
 
 	retval = sd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if (!chip->ft2_fast_mode) {
+	if (!chip->ft2_fast_mode)
 		wait_timeout(250);
-	}
 
 	retval = enable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (chip->asic_code) {
 		retval = sd_pull_ctl_enable(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		RTSX_WRITE_REG(chip, FPGA_PULL_CTL, FPGA_SD_PULL_CTL_BIT | 0x20, 0);
 	}
 
 	if (chip->ft2_fast_mode) {
-		if (CHECK_PID(chip, 0x5209)) {
+		if (CHECK_PID(chip, 0x5209))
 			RTSX_WRITE_REG(chip, PWR_GATE_CTRL, LDO3318_PWR_MASK, LDO_ON);
-		}
+
 	} else {
 		retval = card_power_on(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		wait_timeout(260);
 
 #ifdef SUPPORT_OCP
@@ -2214,13 +2126,12 @@ static int sd_read_lba0(struct rtsx_chip *chip)
 	if (CHK_SD(sd_card)) {
 		bus_width = SD_BUS_WIDTH_4;
 	} else {
-		if (CHK_MMC_8BIT(sd_card)) {
+		if (CHK_MMC_8BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_8;
-		} else if (CHK_MMC_4BIT(sd_card)) {
+		else if (CHK_MMC_4BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_4;
-		} else {
+		else
 			bus_width = SD_BUS_WIDTH_1;
-		}
 	}
 
 	retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
@@ -2243,9 +2154,8 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	cmd[0] = 0x40 | SD_STATUS;
 	cmd[1] = 0;
@@ -2273,9 +2183,8 @@ static int sd_check_wp_state(struct rtsx_chip *chip)
 
 	/* Check SD Machanical Write-Protect Switch */
 	val = rtsx_readl(chip, RTSX_BIPR);
-	if (val & SD_WRITE_PROTECT) {
+	if (val & SD_WRITE_PROTECT)
 		chip->card_wp |= SD_CARD;
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -2307,14 +2216,12 @@ Switch_Fail:
 #endif
 
 	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_dummy_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) && try_sdio) {
 		int rty_cnt = 0;
@@ -2348,9 +2255,8 @@ Switch_Fail:
 	/* Start Initialization Process of SD Card */
 RTY_SD_RST:
 	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 	       TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	wait_timeout(20);
 
@@ -2377,9 +2283,8 @@ RTY_SD_RST:
 		voltage = SUPPORT_VOLTAGE;
 
 		retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 		       TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		wait_timeout(20);
 	}
@@ -2393,42 +2298,38 @@ RTY_SD_RST:
 			}
 
 			j++;
-			if (j < 3) {
+			if (j < 3)
 				goto RTY_SD_RST;
-			} else {
+			else
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 
 		retval = sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage, SD_RSP_TYPE_R3, rsp, 5);
 		if (retval != STATUS_SUCCESS) {
 			k++;
-			if (k < 3) {
+			if (k < 3)
 				goto RTY_SD_RST;
-			} else {
+			else
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 
 		i++;
 		wait_timeout(20);
 	} while (!(rsp[1] & 0x80) && (i < 255));
 
-	if (i == 255) {
+	if (i == 255)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (hi_cap_flow) {
-		if (rsp[1] & 0x40) {
+		if (rsp[1] & 0x40)
 			SET_SD_HCXC(sd_card);
-		} else {
+		else
 			CLR_SD_HCXC(sd_card);
-		}
-		if (CHECK_PID(chip, 0x5209) && CHK_SD_HCXC(sd_card) && !sd20_mode) {
+
+		if (CHECK_PID(chip, 0x5209) && CHK_SD_HCXC(sd_card) && !sd20_mode)
 			support_1v8 = (rsp[1] & 0x01) ? 1 : 0;
-		} else {
+		else
 			support_1v8 = 0;
-		}
 	} else {
 		CLR_SD_HCXC(sd_card);
 		support_1v8 = 0;
@@ -2437,46 +2338,39 @@ RTY_SD_RST:
 
 	if (support_1v8) {
 		retval = sd_voltage_switch(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		sd_card->sd_addr = (u32)rsp[1] << 24;
 		sd_card->sd_addr += (u32)rsp[2] << 16;
 
-		if (sd_card->sd_addr) {
+		if (sd_card->sd_addr)
 			break;
-		}
 	}
 
 	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 #ifdef SUPPORT_SD_LOCK
 SD_UNLOCK_ENTRY:
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (sd_card->sd_lock_status & SD_LOCKED) {
 		sd_card->sd_lock_status |= (SD_LOCK_1BIT_MODE | SD_PWD_EXIST);
@@ -2487,23 +2381,21 @@ SD_UNLOCK_ENTRY:
 #endif
 
 	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	retval = sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		switch_bus_width = SD_BUS_WIDTH_4;
 	} else {
@@ -2511,14 +2403,12 @@ SD_UNLOCK_ENTRY:
 	}
 
 	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (!(sd_card->raw_csd[4] & 0x40))
 		sd_dont_switch = 1;
@@ -2537,9 +2427,9 @@ SD_UNLOCK_ENTRY:
 		if (retval == STATUS_SUCCESS) {
 			retval = sd_switch_function(chip, switch_bus_width);
 			if (retval != STATUS_SUCCESS) {
-				if (CHECK_PID(chip, 0x5209)) {
+				if (CHECK_PID(chip, 0x5209))
 					sd_change_bank_voltage(chip, SD_IO_3V3);
-				}
+
 				sd_init_power(chip);
 				sd_dont_switch = 1;
 				try_sdio = 0;
@@ -2548,9 +2438,9 @@ SD_UNLOCK_ENTRY:
 			}
 		} else {
 			if (support_1v8) {
-				if (CHECK_PID(chip, 0x5209)) {
+				if (CHECK_PID(chip, 0x5209))
 					sd_change_bank_voltage(chip, SD_IO_3V3);
-				}
+
 				sd_init_power(chip);
 				sd_dont_switch = 1;
 				try_sdio = 0;
@@ -2562,13 +2452,12 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
+
 		retval = sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	}
 
 #ifdef SUPPORT_SD_LOCK
@@ -2581,24 +2470,22 @@ SD_UNLOCK_ENTRY:
 		RTSX_WRITE_REG(chip, SD30_DRIVE_SEL, 0x07, chip->sd30_drive_sel_1v8);
 
 		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
-		if (CHK_SD_DDR50(sd_card)) {
+		if (CHK_SD_DDR50(sd_card))
 			retval = sd_ddr_tuning(chip);
-		} else {
+		else
 			retval = sd_sdr_tuning(chip);
-		}
 
 		if (retval != STATUS_SUCCESS) {
 			if (sd20_mode) {
 				TRACE_RET(chip, STATUS_FAIL);
 			} else {
 				retval = sd_init_power(chip);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 				try_sdio = 0;
 				sd20_mode = 1;
 				goto Switch_Fail;
@@ -2609,9 +2496,8 @@ SD_UNLOCK_ENTRY:
 
 		if (CHK_SD_DDR50(sd_card)) {
 			retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				read_lba0 = 0;
-			}
 		}
 
 		if (read_lba0) {
@@ -2621,9 +2507,9 @@ SD_UNLOCK_ENTRY:
 					TRACE_RET(chip, STATUS_FAIL);
 				} else {
 					retval = sd_init_power(chip);
-					if (retval != STATUS_SUCCESS) {
+					if (retval != STATUS_SUCCESS)
 						TRACE_RET(chip, STATUS_FAIL);
-					}
+
 					try_sdio = 0;
 					sd20_mode = 1;
 					goto Switch_Fail;
@@ -2633,9 +2519,8 @@ SD_UNLOCK_ENTRY:
 	}
 
 	retval = sd_check_wp_state(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
 
@@ -2659,9 +2544,8 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 	int len;
 
 	retval = sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, SWITCH_FAIL);
-	}
 
 	if (width == MMC_8BIT_BUS) {
 		buf[0] = 0x55;
@@ -2690,9 +2574,8 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 			rtsx_read_register(chip, REG_SD_STAT1, &val1);
 			rtsx_read_register(chip, REG_SD_STAT2, &val2);
 			rtsx_clear_sd_error(chip);
-			if ((val1 & 0xE0) || val2) {
+			if ((val1 & 0xE0) || val2)
 				TRACE_RET(chip, SWITCH_ERR);
-			}
 		} else {
 			rtsx_clear_sd_error(chip);
 			rtsx_write_register(chip, REG_SD_CFG3, 0x02, 0);
@@ -2712,11 +2595,10 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CMD0, 0xFF, 0x40 | BUSTEST_R);
 
-	if (width == MMC_8BIT_BUS) {
+	if (width == MMC_8BIT_BUS)
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF, 0x08);
-	} else {
+	else
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BYTE_CNT_L, 0xFF, 0x04);
-	}
 
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_L, 0xFF, 1);
 	rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_BLOCK_CNT_H, 0xFF, 0);
@@ -2729,9 +2611,8 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END, SD_TRANSFER_END);
 
 	rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2, 0, 0);
-	if (width == MMC_8BIT_BUS) {
+	if (width == MMC_8BIT_BUS)
 		rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 1, 0, 0);
-	}
 
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
 	if (retval < 0) {
@@ -2747,15 +2628,14 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 			u8 rsp[5];
 			u32 arg;
 
-			if (CHK_MMC_DDR52(sd_card)) {
+			if (CHK_MMC_DDR52(sd_card))
 				arg = 0x03B70600;
-			} else {
+			else
 				arg = 0x03B70200;
-			}
+
 			retval = sd_send_cmd_get_rsp(chip, SWITCH, arg, SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS) && !(rsp[4] & MMC_SWITCH_ERR)) {
+			if ((retval == STATUS_SUCCESS) && !(rsp[4] & MMC_SWITCH_ERR))
 				return SWITCH_SUCCESS;
-			}
 		}
 	} else {
 		RTSX_DEBUGP("BUSTEST_R [4bits]: 0x%02x\n", ptr[0]);
@@ -2763,15 +2643,14 @@ static int mmc_test_switch_bus(struct rtsx_chip *chip, u8 width)
 			u8 rsp[5];
 			u32 arg;
 
-			if (CHK_MMC_DDR52(sd_card)) {
+			if (CHK_MMC_DDR52(sd_card))
 				arg = 0x03B70500;
-			} else {
+			else
 				arg = 0x03B70100;
-			}
+
 			retval = sd_send_cmd_get_rsp(chip, SWITCH, arg, SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS) && !(rsp[4] & MMC_SWITCH_ERR)) {
+			if ((retval == STATUS_SUCCESS) && !(rsp[4] & MMC_SWITCH_ERR))
 				return SWITCH_SUCCESS;
-			}
 		}
 	}
 
@@ -2845,11 +2724,10 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 			card_type_mask = 0x03;
 		}
 #else
-		if (chip->sd_ctl & SUPPORT_MMC_DDR_MODE) {
+		if (chip->sd_ctl & SUPPORT_MMC_DDR_MODE)
 			card_type_mask = 0x07;
-		} else {
+		else
 			card_type_mask = 0x03;
-		}
 #endif
 	} else {
 		card_type_mask = 0x03;
@@ -2859,11 +2737,10 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 		u8 rsp[5];
 
 		if (card_type & 0x04) {
-			if (switch_ddr) {
+			if (switch_ddr)
 				SET_MMC_DDR52(sd_card);
-			} else {
+			else
 				SET_MMC_52M(sd_card);
-			}
 		} else if (card_type & 0x02) {
 			SET_MMC_52M(sd_card);
 		} else {
@@ -2872,16 +2749,14 @@ static int mmc_switch_timing_bus(struct rtsx_chip *chip, int switch_ddr)
 
 		retval = sd_send_cmd_get_rsp(chip, SWITCH,
 				0x03B90100, SD_RSP_TYPE_R1b, rsp, 5);
-		if ((retval != STATUS_SUCCESS) || (rsp[4] & MMC_SWITCH_ERR)) {
+		if ((retval != STATUS_SUCCESS) || (rsp[4] & MMC_SWITCH_ERR))
 			CLR_MMC_HS(sd_card);
-		}
 	}
 
 	sd_choose_proper_clock(chip);
 	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	/* Test Bus Procedure */
 	retval = mmc_test_switch_bus(chip, MMC_8BIT_BUS);
@@ -2929,17 +2804,15 @@ static int reset_mmc(struct rtsx_chip *chip)
 
 Switch_Fail:
 	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
-	}
 
 	SET_MMC(sd_card);
 
 RTY_MMC_RST:
 	retval = sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 	       TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	do {
 		if (detect_card_cd(chip, SD_CARD) != STATUS_SUCCESS) {
@@ -2973,56 +2846,47 @@ RTY_MMC_RST:
 		i++;
 	} while (!(rsp[1] & 0x80) && (i < 255));
 
-	if (i == 255) {
+	if (i == 255)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
-	if ((rsp[1] & 0x60) == 0x40) {
+	if ((rsp[1] & 0x60) == 0x40)
 		SET_MMC_SECTOR_MODE(sd_card);
-	} else {
+	else
 		CLR_MMC_SECTOR_MODE(sd_card);
-	}
 
 	retval = sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
 
 	retval = sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 #ifdef SUPPORT_SD_LOCK
 MMC_UNLOCK_ENTRY:
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 #endif
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
 
@@ -3039,22 +2903,20 @@ MMC_UNLOCK_ENTRY:
 			}
 		}
 
-		if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0)) {
+		if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0))
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (switch_ddr && CHK_MMC_DDR52(sd_card)) {
 			retval = sd_set_init_para(chip);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 
 			retval = mmc_ddr_tuning(chip);
 			if (retval != STATUS_SUCCESS) {
 				retval = sd_init_power(chip);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 				switch_ddr = 0;
 				TRACE_GOTO(chip, Switch_Fail);
 			}
@@ -3064,9 +2926,9 @@ MMC_UNLOCK_ENTRY:
 				retval = sd_read_lba0(chip);
 				if (retval != STATUS_SUCCESS) {
 					retval = sd_init_power(chip);
-					if (retval != STATUS_SUCCESS) {
+					if (retval != STATUS_SUCCESS)
 						TRACE_RET(chip, STATUS_FAIL);
-					}
+
 					switch_ddr = 0;
 					TRACE_GOTO(chip, Switch_Fail);
 				}
@@ -3082,9 +2944,8 @@ MMC_UNLOCK_ENTRY:
 #endif
 
 	temp = rtsx_readl(chip, RTSX_BIPR);
-	if (temp & SD_WRITE_PROTECT) {
+	if (temp & SD_WRITE_PROTECT)
 		chip->card_wp |= SD_CARD;
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -3100,36 +2961,31 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		} else {
 			retval = rtsx_write_register(chip, FPGA_PULL_CTL,
 						     FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-			if (retval != STATUS_SUCCESS) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 		retval = card_share_mode(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		chip->sd_io = 1;
 		TRACE_RET(chip, STATUS_FAIL);
 	}
 
 	retval = sd_init_power(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (chip->sd_ctl & RESET_MMC_FIRST) {
 		retval = reset_mmc(chip);
@@ -3168,18 +3024,17 @@ int reset_sd_card(struct rtsx_chip *chip)
 	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
 	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
 
 	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("sd_card->sd_type = 0x%x\n", sd_card->sd_type);
 
@@ -3205,33 +3060,29 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_init_power(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = reset_mmc(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	retval = sd_set_clock_divider(chip, SD_CLK_DIVIDE_0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
+
 	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_L, 0xFF, 0);
 	RTSX_WRITE_REG(chip, REG_SD_BYTE_CNT_H, 0xFF, 2);
 
 	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
 
 	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_DEBUGP("In reset_mmc_only, sd_card->sd_type = 0x%x\n", sd_card->sd_type);
 
@@ -3255,9 +3106,8 @@ static int wait_data_buf_ready(struct rtsx_chip *chip)
 
 		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
 				sd_card->sd_addr, SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		if (sd_card->sd_data_buf_ready) {
 			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
@@ -3277,19 +3127,18 @@ void sd_stop_seq_mode(struct rtsx_chip *chip)
 
 	if (sd_card->seq_mode) {
 		retval = sd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			return;
-		}
 
 		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
 				SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			sd_set_err_code(chip, SD_STS_ERR);
-		}
+
 		retval = sd_wait_state_data_ready(chip, 0x08, 1, 1000);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			sd_set_err_code(chip, SD_STS_ERR);
-		}
+
 		sd_card->seq_mode = 0;
 
 		rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
@@ -3302,9 +3151,8 @@ static inline int sd_auto_tune_clock(struct rtsx_chip *chip)
 	int retval;
 
 	if (chip->asic_code) {
-		if (sd_card->sd_clock > 30) {
+		if (sd_card->sd_clock > 30)
 			sd_card->sd_clock -= 20;
-		}
 	} else {
 		switch (sd_card->sd_clock) {
 		case CLK_200:
@@ -3337,9 +3185,8 @@ static inline int sd_auto_tune_clock(struct rtsx_chip *chip)
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -3377,11 +3224,10 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 		}
 	}
 
-	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card)) {
+	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card))
 		data_addr = start_sector << 9;
-	} else {
+	else
 		data_addr = start_sector;
-	}
 
 	sd_clr_err_code(chip);
 
@@ -3436,21 +3282,19 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 
 	rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01, RING_BUFFER);
 
-	if (CHK_MMC_8BIT(sd_card)) {
+	if (CHK_MMC_8BIT(sd_card))
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_8);
-	} else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card)) {
+	else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card))
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-	} else {
+	else
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_1);
-	}
 
 	if (sd_card->seq_mode) {
 		cfg2 = SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
 				SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
 		if (CHECK_PID(chip, 0x5209)) {
-			if (!CHK_SD30_SPEED(sd_card)) {
+			if (!CHK_SD30_SPEED(sd_card))
 				cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
-			}
 		}
 		rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, cfg2);
 
@@ -3480,9 +3324,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 			cfg2 = SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
 					SD_CHECK_CRC7 | SD_RSP_LEN_6;
 			if (CHECK_PID(chip, 0x5209)) {
-				if (!CHK_SD30_SPEED(sd_card)) {
+				if (!CHK_SD30_SPEED(sd_card))
 					cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
-				}
 			}
 			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, cfg2);
 
@@ -3523,9 +3366,8 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 			cfg2 = SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END |
 					SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
 			if (CHECK_PID(chip, 0x5209)) {
-				if (!CHK_SD30_SPEED(sd_card)) {
+				if (!CHK_SD30_SPEED(sd_card))
 					cfg2 |= SD_NO_CHECK_WAIT_CRC_TO;
-				}
 			}
 			rtsx_add_cmd(chip, WRITE_REG_CMD, REG_SD_CFG2, 0xFF, cfg2);
 
@@ -3550,11 +3392,10 @@ int sd_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip, u32 start_sector, u16 s
 
 		sd_card->seq_mode = 0;
 
-		if (retval == -ETIMEDOUT) {
+		if (retval == -ETIMEDOUT)
 			err = STATUS_TIMEDOUT;
-		} else {
+		else
 			err = STATUS_FAIL;
-		}
 
 		rtsx_read_register(chip, REG_SD_STAT1, &stat);
 		rtsx_clear_sd_error(chip);
@@ -3640,9 +3481,8 @@ int ext_sd_send_cmd_get_rsp(struct rtsx_chip *chip, u8 cmd_idx,
 
 	RTSX_DEBUGP("EXT SD/MMC CMD %d\n", cmd_idx);
 
-	if (rsp_type == SD_RSP_TYPE_R1b) {
+	if (rsp_type == SD_RSP_TYPE_R1b)
 		timeout = 3000;
-	}
 
 RTY_SEND_CMD:
 
@@ -3662,14 +3502,14 @@ RTY_SEND_CMD:
 	rtsx_add_cmd(chip, CHECK_REG_CMD, REG_SD_TRANSFER, SD_TRANSFER_END, SD_TRANSFER_END);
 
 	if (rsp_type == SD_RSP_TYPE_R2) {
-		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-		}
+
 		stat_idx = 17;
 	} else if (rsp_type != SD_RSP_TYPE_R0) {
-		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-		}
+
 		stat_idx = 6;
 	}
 	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_CMD5, 0, 0);
@@ -3683,9 +3523,8 @@ RTY_SEND_CMD:
 
 			if (rsp_type & SD_WAIT_BUSY_END) {
 				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS) {
+				if (retval != STATUS_SUCCESS)
 					TRACE_RET(chip, retval);
-				}
 			} else {
 				sd_set_err_code(chip, SD_TO_ERR);
 			}
@@ -3693,9 +3532,8 @@ RTY_SEND_CMD:
 		TRACE_RET(chip, STATUS_FAIL);
 	}
 
-	if (rsp_type == SD_RSP_TYPE_R0) {
+	if (rsp_type == SD_RSP_TYPE_R0)
 		return STATUS_SUCCESS;
-	}
 
 	ptr = rtsx_get_cmd_data(chip) + 1;
 
@@ -3724,9 +3562,8 @@ RTY_SEND_CMD:
 	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) {
+			if (ptr[1] & 0x80)
 				TRACE_RET(chip, STATUS_FAIL);
-			}
 		}
 #ifdef SUPPORT_SD_LOCK
 		if (ptr[1] & 0x7D)
@@ -3736,26 +3573,23 @@ RTY_SEND_CMD:
 		{
 			TRACE_RET(chip, STATUS_FAIL);
 		}
-		if (ptr[2] & 0xF8) {
+		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) {
+				if ((ptr[3] & 0x1E) != 0x04)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
+
 			} else if (rsp_type == SD_RSP_TYPE_R0) {
-				if ((ptr[3] & 0x1E) != 0x03) {
+				if ((ptr[3] & 0x1E) != 0x03)
 					TRACE_RET(chip, STATUS_FAIL);
-				}
 			}
 		}
 	}
 
-	if (rsp && rsp_len) {
+	if (rsp && rsp_len)
 		memcpy(rsp, ptr, rsp_len);
-	}
 
 	return STATUS_SUCCESS;
 }
@@ -3765,29 +3599,27 @@ int ext_sd_get_rsp(struct rtsx_chip *chip, int len, u8 *rsp, u8 rsp_type)
 	int retval, rsp_len;
 	u16 reg_addr;
 
-	if (rsp_type == SD_RSP_TYPE_R0) {
+	if (rsp_type == SD_RSP_TYPE_R0)
 		return STATUS_SUCCESS;
-	}
 
 	rtsx_init_cmd(chip);
 
 	if (rsp_type == SD_RSP_TYPE_R2) {
-		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		}
+
 		rsp_len = 17;
 	} else if (rsp_type != SD_RSP_TYPE_R0) {
-		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++) {
+		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		}
+
 		rsp_len = 6;
 	}
 	rtsx_add_cmd(chip, READ_REG_CMD, REG_SD_CMD5, 0xFF, 0);
 
 	retval = rtsx_send_cmd(chip, SD_CARD, 100);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (rsp) {
 		int min_len = (rsp_len < len) ? rsp_len : len;
@@ -3858,9 +3690,8 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	buf[5] = (1 == CHK_SD(sd_card)) ?  0x01 : 0x02;
-	if (chip->card_wp & SD_CARD) {
+	if (chip->card_wp & SD_CARD)
 		buf[5] |= 0x80;
-	}
 
 	buf[6] = (u8)(sd_card->sd_addr >> 16);
 	buf[7] = (u8)(sd_card->sd_addr >> 24);
@@ -3875,9 +3706,8 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 static inline int get_rsp_type(struct scsi_cmnd *srb, u8 *rsp_type, int *rsp_len)
 {
-	if (!rsp_type || !rsp_len) {
+	if (!rsp_type || !rsp_len)
 		return STATUS_FAIL;
-	}
 
 	switch (srb->cmnd[10]) {
 	case 0x03:
@@ -3927,9 +3757,8 @@ int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
@@ -3938,12 +3767,11 @@ int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x02) {
+	if (srb->cmnd[1] & 0x02)
 		standby = 1;
-	}
-	if (srb->cmnd[1] & 0x01) {
+
+	if (srb->cmnd[1] & 0x01)
 		acmd = 1;
-	}
 
 	arg = ((u32)srb->cmnd[3] << 24) | ((u32)srb->cmnd[4] << 16) |
 		((u32)srb->cmnd[5] << 8) | srb->cmnd[6];
@@ -3956,64 +3784,56 @@ int sd_execute_no_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
 		}
 	}
 #else
 	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 #endif
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-		}
 	}
 
 	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
 			sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-		}
 	}
 
 #ifdef SUPPORT_SD_LOCK
 	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
 #endif
 
 	scsi_set_resid(srb, 0);
@@ -4024,9 +3844,8 @@ SD_Execute_Cmd_Failed:
 	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
 	release_sd_card(chip);
 	do_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD)) {
+	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-	}
 
 	TRACE_RET(chip, TRANSPORT_FAILED);
 }
@@ -4053,20 +3872,18 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04) {
+	if (srb->cmnd[1] & 0x04)
 		send_cmd12 = 1;
-	}
-	if (srb->cmnd[1] & 0x02) {
+
+	if (srb->cmnd[1] & 0x02)
 		standby = 1;
-	}
-	if (srb->cmnd[1] & 0x01) {
+
+	if (srb->cmnd[1] & 0x01)
 		acmd = 1;
-	}
 
 	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8] << 8) | srb->cmnd[9];
 
@@ -4078,19 +3895,17 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 
 #ifdef SUPPORT_SD_LOCK
 	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
-		if (CHK_MMC_8BIT(sd_card)) {
+		if (CHK_MMC_8BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_8;
-		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
+		else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card))
 			bus_width = SD_BUS_WIDTH_4;
-		} else {
+		else
 			bus_width = SD_BUS_WIDTH_1;
-		}
 	} else {
 		bus_width = SD_BUS_WIDTH_4;
 	}
@@ -4102,24 +3917,21 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 	}
 
 	if (data_len <= 512) {
@@ -4138,9 +3950,8 @@ 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) {
+		if (buf == NULL)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, byte_cnt,
 				       blk_cnt, bus_width, buf, data_len, 2000);
@@ -4195,56 +4006,48 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	retval = ext_sd_get_rsp(chip, rsp_len, sd_card->rsp, rsp_type);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 	}
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
 				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
+
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
 	}
 
-	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04)) {
+	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
 		cmd13_checkbit = 1;
-	}
 
 	for (i = 0; i < 3; i++) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 			SD_RSP_TYPE_R1, NULL, 0, cmd13_checkbit);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		}
 	}
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
 
 	scsi_set_resid(srb, 0);
 	return TRANSPORT_GOOD;
@@ -4252,14 +4055,13 @@ int sd_execute_read_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 SD_Execute_Read_Cmd_Failed:
 	sd_card->pre_cmd_err = 1;
 	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (read_err) {
+	if (read_err)
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	}
+
 	release_sd_card(chip);
 	do_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD)) {
+	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-	}
 
 	TRACE_RET(chip, TRANSPORT_FAILED);
 }
@@ -4291,20 +4093,18 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	}
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
 
 	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04) {
+	if (srb->cmnd[1] & 0x04)
 		send_cmd12 = 1;
-	}
-	if (srb->cmnd[1] & 0x02) {
+
+	if (srb->cmnd[1] & 0x02)
 		standby = 1;
-	}
-	if (srb->cmnd[1] & 0x01) {
+
+	if (srb->cmnd[1] & 0x01)
 		acmd = 1;
-	}
 
 	data_len = ((u32)srb->cmnd[7] << 16) | ((u32)srb->cmnd[8] << 8) | srb->cmnd[9];
 	arg = ((u32)srb->cmnd[3] << 24) | ((u32)srb->cmnd[4] << 16) |
@@ -4325,75 +4125,66 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	sd_card->last_rsp_type = rsp_type;
 
 	retval = sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, 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) {
+			if (retval != STATUS_SUCCESS)
 				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
 		}
 	}
 #else
 	retval = rtsx_write_register(chip, REG_SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 	}
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 	}
 
 	if (acmd) {
 		retval = ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
 				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 	}
 
 	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
 			sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
 
 	if (data_len <= 512) {
 		u16 i;
 		u8 *buf;
 
 		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL) {
+		if (buf == NULL)
 			TRACE_RET(chip, TRANSPORT_ERROR);
-		}
 
 		rtsx_stor_get_xfer_buf(buf, data_len, srb);
 
 #ifdef SUPPORT_SD_LOCK
-		if (cmd_idx == LOCK_UNLOCK) {
+		if (cmd_idx == LOCK_UNLOCK)
 			lock_cmd_type = buf[0] & 0x0F;
-		}
 #endif
 
 		if (data_len > 256) {
@@ -4485,11 +4276,11 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		}
 
 		rtsx_init_cmd(chip);
-		if (CHECK_PID(chip, 0x5209)) {
+		if (CHECK_PID(chip, 0x5209))
 			rtsx_add_cmd(chip, CHECK_REG_CMD, SD_BUS_STAT, SD_DAT0_STATUS, SD_DAT0_STATUS);
-		} else {
+		else
 			rtsx_add_cmd(chip, CHECK_REG_CMD, 0xFD30, 0x02, 0x02);
-		}
+
 		rtsx_send_cmd(chip, SD_CARD, 250);
 
 		retval = sd_update_lock_status(chip);
@@ -4502,61 +4293,53 @@ 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, 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) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 	}
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
 				SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 
 		retval = rtsx_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
+
 		rtsx_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		}
 	}
 
-	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04)) {
+	if ((srb->cmnd[1] & 0x02) || (srb->cmnd[1] & 0x04))
 		cmd13_checkbit = 1;
-	}
 
 	for (i = 0; i < 3; i++) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
 			SD_RSP_TYPE_R1, NULL, 0, cmd13_checkbit);
-		if (retval == STATUS_SUCCESS) {
+		if (retval == STATUS_SUCCESS)
 			break;
-		}
 	}
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
 
 #ifdef SUPPORT_SD_LOCK
 	if (cmd_idx == LOCK_UNLOCK) {
 		if (!lock_cmd_fail) {
 			RTSX_DEBUGP("lock_cmd_type = 0x%x\n", lock_cmd_type);
-			if (lock_cmd_type & SD_CLR_PWD) {
+			if (lock_cmd_type & SD_CLR_PWD)
 				sd_card->sd_lock_status &= ~SD_PWD_EXIST;
-			}
-			if (lock_cmd_type & SD_SET_PWD) {
+
+			if (lock_cmd_type & SD_SET_PWD)
 				sd_card->sd_lock_status |= SD_PWD_EXIST;
-			}
 		}
 
 		RTSX_DEBUGP("sd_lock_state = 0x%x, sd_card->sd_lock_status = 0x%x\n",
@@ -4593,14 +4376,13 @@ int sd_execute_write_data(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 SD_Execute_Write_Cmd_Failed:
 	sd_card->pre_cmd_err = 1;
 	set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (write_err) {
+	if (write_err)
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-	}
+
 	release_sd_card(chip);
 	do_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD)) {
+	if (!(chip->card_ready & SD_CARD))
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-	}
 
 	TRACE_RET(chip, TRANSPORT_FAILED);
 }
@@ -4670,9 +4452,8 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	switch (srb->cmnd[1] & 0x0F) {
 	case 0:
 #ifdef SUPPORT_SD_LOCK
-		if (0x64 == srb->cmnd[9]) {
+		if (0x64 == srb->cmnd[9])
 			sd_card->sd_lock_status |= SD_SDR_RST;
-		}
 #endif
 		retval = reset_sd_card(chip);
 		if (retval != STATUS_SUCCESS) {
@@ -4723,26 +4504,23 @@ int sd_power_off_card3v3(struct rtsx_chip *chip)
 	int retval;
 
 	retval = disable_card_clock(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	RTSX_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, 0);
 
 	if (!chip->ft2_fast_mode) {
 		retval = card_power_off(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
 		wait_timeout(50);
 	}
 
 	if (chip->asic_code) {
 		retval = sd_pull_ctl_disable(chip);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 	} else {
 		RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
 			FPGA_SD_PULL_CTL_BIT | 0x20, FPGA_SD_PULL_CTL_BIT);
@@ -4774,19 +4552,16 @@ 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) {
+	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-	}
 
 	if (CHECK_PID(chip, 0x5209)) {
 		retval = sd_change_bank_voltage(chip, SD_IO_3V3);
-		if (retval != STATUS_SUCCESS) {
+		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 
-		if (CHK_SD30_SPEED(sd_card)) {
+		if (CHK_SD30_SPEED(sd_card))
 			RTSX_WRITE_REG(chip, SD30_DRIVE_SEL, 0x07, chip->sd30_drive_sel_3v3);
-		}
 
 		RTSX_WRITE_REG(chip, OCPPARA2, SD_OCP_THD_MASK, chip->sd_400mA_ocp_thd);
 	}
diff --git a/drivers/staging/rts_pstor/trace.h b/drivers/staging/rts_pstor/trace.h
index bc83b49a4eb4..cf60a1b872b3 100644
--- a/drivers/staging/rts_pstor/trace.h
+++ b/drivers/staging/rts_pstor/trace.h
@@ -83,33 +83,9 @@ do {													\
 #endif
 
 #ifdef CONFIG_RTS_PSTOR_DEBUG
-static inline void rtsx_dump(u8 *buf, int buf_len)
-{
-	int i;
-	u8 tmp[16] = {0};
-	u8 *_ptr = buf;
-
-	for (i = 0; i < ((buf_len)/16); i++) {
-		RTSX_DEBUGP("%02x %02x %02x %02x %02x %02x %02x %02x "
-			"%02x %02x %02x %02x %02x %02x %02x %02x\n",
-			_ptr[0], _ptr[1], _ptr[2], _ptr[3], _ptr[4], _ptr[5],
-			_ptr[6], _ptr[7], _ptr[8], _ptr[9], _ptr[10], _ptr[11],
-			_ptr[12], _ptr[13], _ptr[14], _ptr[15]);
-		_ptr += 16;
-	}
-	if ((buf_len) % 16) {
-		memcpy(tmp, _ptr, (buf_len) % 16);
-		_ptr = tmp;
-		RTSX_DEBUGP("%02x %02x %02x %02x %02x %02x %02x %02x "
-			"%02x %02x %02x %02x %02x %02x %02x %02x\n",
-			_ptr[0], _ptr[1], _ptr[2], _ptr[3], _ptr[4], _ptr[5],
-			_ptr[6], _ptr[7], _ptr[8], _ptr[9], _ptr[10], _ptr[11],
-			_ptr[12], _ptr[13], _ptr[14], _ptr[15]);
-	}
-}
-
-#define RTSX_DUMP(buf, buf_len)		rtsx_dump((u8 *)(buf), (buf_len))
-
+#define RTSX_DUMP(buf, buf_len)					\
+	print_hex_dump(KERN_DEBUG, RTSX_STOR, DUMP_PREFIX_NONE,	\
+				16, 1, (buf), (buf_len), false)
 #else
 #define RTSX_DUMP(buf, buf_len)
 #endif
diff --git a/drivers/staging/sbe-2t3e3/2t3e3.h b/drivers/staging/sbe-2t3e3/2t3e3.h
index 383f2cfc1ad2..ccad049c1122 100644
--- a/drivers/staging/sbe-2t3e3/2t3e3.h
+++ b/drivers/staging/sbe-2t3e3/2t3e3.h
@@ -789,7 +789,6 @@ void dc_restart(struct channel *);
 void dc_receiver_onoff(struct channel *, u32);
 void dc_transmitter_onoff(struct channel *, u32);
 void dc_set_loopback(struct channel *, u32);
-u32 dc_init_descriptor_list(struct channel *);
 void dc_clear_descriptor_list(struct channel *);
 void dc_drop_descriptor_list(struct channel *);
 void dc_set_output_port(struct channel *);
diff --git a/drivers/staging/sbe-2t3e3/dc.c b/drivers/staging/sbe-2t3e3/dc.c
index 9e81d9036a33..daadd6ea4978 100644
--- a/drivers/staging/sbe-2t3e3/dc.c
+++ b/drivers/staging/sbe-2t3e3/dc.c
@@ -17,6 +17,8 @@
 #include "2t3e3.h"
 #include "ctrl.h"
 
+static int dc_init_descriptor_list(struct channel *sc);
+
 void dc_init(struct channel *sc)
 {
 	u32 val;
@@ -307,7 +309,7 @@ void dc_set_loopback(struct channel *sc, u32 mode)
 			      SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
 }
 
-u32 dc_init_descriptor_list(struct channel *sc)
+static int dc_init_descriptor_list(struct channel *sc)
 {
 	u32 i, j;
 	struct sk_buff *m;
@@ -317,7 +319,7 @@ u32 dc_init_descriptor_list(struct channel *sc)
 					    sizeof(t3e3_rx_desc_t), GFP_KERNEL);
 	if (sc->ether.rx_ring == NULL) {
 		dev_err(&sc->pdev->dev, "SBE 2T3E3: no buffer space for RX ring\n");
-		return ENOMEM;
+		return -ENOMEM;
 	}
 
 	if (sc->ether.tx_ring == NULL)
@@ -327,7 +329,7 @@ u32 dc_init_descriptor_list(struct channel *sc)
 		kfree(sc->ether.rx_ring);
 		sc->ether.rx_ring = NULL;
 		dev_err(&sc->pdev->dev, "SBE 2T3E3: no buffer space for RX ring\n");
-		return ENOMEM;
+		return -ENOMEM;
 	}
 
 
@@ -351,7 +353,7 @@ u32 dc_init_descriptor_list(struct channel *sc)
 				sc->ether.tx_ring = NULL;
 				dev_err(&sc->pdev->dev, "SBE 2T3E3: token_alloc err:"
 					" no buffer space for RX ring\n");
-				return ENOBUFS;
+				return -ENOBUFS;
 			}
 			sc->ether.rx_data[i] = m;
 		}
diff --git a/drivers/staging/sbe-2t3e3/module.c b/drivers/staging/sbe-2t3e3/module.c
index cd778b3a02b2..8adb17816ad9 100644
--- a/drivers/staging/sbe-2t3e3/module.c
+++ b/drivers/staging/sbe-2t3e3/module.c
@@ -67,6 +67,7 @@ static int __devinit t3e3_init_channel(struct channel *channel, struct pci_dev *
 	dev = alloc_hdlcdev(channel);
 	if (!dev) {
 		printk(KERN_ERR "SBE 2T3E3" ": Out of memory\n");
+		err = -ENOMEM;
 		goto free_regions;
 	}
 
@@ -82,8 +83,9 @@ static int __devinit t3e3_init_channel(struct channel *channel, struct pci_dev *
 	else
 		channel->h.slot = 0;
 
-	if (setup_device(dev, channel))
-		goto free_regions;
+	err = setup_device(dev, channel);
+	if (err)
+		goto free_dev;
 
 	pci_read_config_dword(channel->pdev, 0x40, &val); /* mask sleep mode */
 	pci_write_config_dword(channel->pdev, 0x40, val & 0x3FFFFFFF);
@@ -92,14 +94,19 @@ static int __devinit t3e3_init_channel(struct channel *channel, struct pci_dev *
 	pci_read_config_dword(channel->pdev, PCI_COMMAND, &channel->h.command);
 	t3e3_init(channel);
 
-	if (request_irq(dev->irq, &t3e3_intr, IRQF_SHARED, dev->name, dev)) {
+	err = request_irq(dev->irq, &t3e3_intr, IRQF_SHARED, dev->name, dev);
+	if (err) {
 		printk(KERN_WARNING "%s: could not get irq: %d\n", dev->name, dev->irq);
-		goto free_regions;
+		goto unregister_dev;
 	}
 
 	pci_set_drvdata(pdev, channel);
 	return 0;
 
+unregister_dev:
+	unregister_hdlc_device(dev);
+free_dev:
+	free_netdev(dev);
 free_regions:
 	pci_release_regions(pdev);
 disable:
diff --git a/drivers/staging/sbe-2t3e3/netdev.c b/drivers/staging/sbe-2t3e3/netdev.c
index c7b5e8bb04ff..180c96327b9a 100644
--- a/drivers/staging/sbe-2t3e3/netdev.c
+++ b/drivers/staging/sbe-2t3e3/netdev.c
@@ -21,13 +21,13 @@
 #include <linux/interrupt.h>
 #include "2t3e3.h"
 
-int t3e3_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+static int t3e3_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
 	struct channel *sc = dev_to_priv(dev);
 	int cmd_2t3e3, len, rlen;
 	t3e3_param_t param;
 	t3e3_resp_t  resp;
-	void *data = ifr->ifr_data + sizeof(cmd_2t3e3) + sizeof(len);
+	void __user *data = ifr->ifr_data + sizeof(cmd_2t3e3) + sizeof(len);
 
 	if (cmd == SIOCWANDEV)
 		return hdlc_ioctl(dev, ifr, cmd);
@@ -82,7 +82,7 @@ static struct net_device_stats* t3e3_get_stats(struct net_device *dev)
 	return nstats;
 }
 
-int t3e3_open(struct net_device *dev)
+static int t3e3_open(struct net_device *dev)
 {
 	struct channel *sc = dev_to_priv(dev);
 	int ret = hdlc_open(dev);
@@ -97,7 +97,7 @@ int t3e3_open(struct net_device *dev)
 	return 0;
 }
 
-int t3e3_close(struct net_device *dev)
+static int t3e3_close(struct net_device *dev)
 {
 	struct channel *sc = dev_to_priv(dev);
 	hdlc_close(dev);
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index ca8946acba60..a414e52dd082 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -719,7 +719,7 @@ static int sep_mmap(struct file *filp, struct vm_area_struct *vma)
 
 	if (remap_pfn_range(vma, vma->vm_start, bus_addr >> PAGE_SHIFT,
 		vma->vm_end - vma->vm_start, vma->vm_page_prot)) {
-		dev_dbg(&sep->pdev->dev, "[PID%d] remap_page_range failed\n",
+		dev_dbg(&sep->pdev->dev, "[PID%d] remap_pfn_range failed\n",
 						current->pid);
 		error = -EAGAIN;
 		goto end_function_with_error;
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 376269b2bbfc..099bc69ca00c 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -245,7 +245,6 @@ static void ProcessLineStatus(struct quatech_port *qt_port,
 	qt_port->shadowLSR =
 	    line_status & (SERIAL_LSR_OE | SERIAL_LSR_PE | SERIAL_LSR_FE |
 			   SERIAL_LSR_BI);
-	return;
 }
 
 static void ProcessModemStatus(struct quatech_port *qt_port,
@@ -254,7 +253,6 @@ static void ProcessModemStatus(struct quatech_port *qt_port,
 
 	qt_port->shadowMSR = modem_status;
 	wake_up_interruptible(&qt_port->wait);
-	return;
 }
 
 static void ProcessRxChar(struct tty_struct *tty, struct usb_serial_port *port,
@@ -358,7 +356,7 @@ static void qt_read_bulk_callback(struct urb *urb)
 		goto exit;
 	}
 
-	if (tty && RxCount) {
+	if (RxCount) {
 		flag_data = 0;
 		for (i = 0; i < RxCount; ++i) {
 			/* Look ahead code here */
@@ -422,7 +420,7 @@ static void qt_read_bulk_callback(struct urb *urb)
 		dev_dbg(&port->dev, "%s - failed resubmitting read urb, error %d",
 			__func__, result);
 	else {
-		if (tty && RxCount) {
+		if (RxCount) {
 			tty_flip_buffer_push(tty);
 			tty_schedule_flip(tty);
 		}
@@ -883,8 +881,6 @@ static int qt_open(struct tty_struct *tty,
 	 * Put this here to make it responsive to stty and defaults set by
 	 * the tty layer
 	 */
-	/* FIXME: is this needed? */
-	/* qt_set_termios(tty, port, NULL); */
 
 	/*  Check to see if we've set up our endpoint info yet */
 	if (port0->open_ports == 1) {
@@ -1175,7 +1171,7 @@ static void qt_set_termios(struct tty_struct *tty,
 			   struct usb_serial_port *port,
 			   struct ktermios *old_termios)
 {
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	unsigned char new_LCR = 0;
 	unsigned int cflag = termios->c_cflag;
 	unsigned int index;
@@ -1184,7 +1180,7 @@ static void qt_set_termios(struct tty_struct *tty,
 
 	index = tty->index - port->serial->minor;
 
-	switch (cflag) {
+	switch (cflag & CSIZE) {
 	case CS5:
 		new_LCR |= SERIAL_5_DATA;
 		break;
@@ -1195,6 +1191,8 @@ static void qt_set_termios(struct tty_struct *tty,
 		new_LCR |= SERIAL_7_DATA;
 		break;
 	default:
+		termios->c_cflag &= ~CSIZE;
+		termios->c_cflag |= CS8;
 	case CS8:
 		new_LCR |= SERIAL_8_DATA;
 		break;
@@ -1279,7 +1277,7 @@ static void qt_set_termios(struct tty_struct *tty,
 			dev_dbg(&port->dev, "BoxSetSW_FlowCtrl (diabling) failed\n");
 
 	}
-	tty->termios->c_cflag &= ~CMSPAR;
+	termios->c_cflag &= ~CMSPAR;
 	/* FIXME: Error cases should be returning the actual bits changed only */
 }
 
@@ -1390,7 +1388,7 @@ static int qt_tiocmget(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct usb_serial *serial = get_usb_serial(port, __func__);
 	struct quatech_port *qt_port = qt_get_port_private(port);
-	int retval = -ENODEV;
+	int retval;
 
 	if (!serial)
 		return -ENODEV;
@@ -1408,7 +1406,7 @@ static int qt_tiocmset(struct tty_struct *tty,
 	struct usb_serial_port *port = tty->driver_data;
 	struct usb_serial *serial = get_usb_serial(port, __func__);
 	struct quatech_port *qt_port = qt_get_port_private(port);
-	int retval = -ENODEV;
+	int retval;
 
 	if (!serial)
 		return -ENODEV;
@@ -1436,7 +1434,6 @@ static void qt_throttle(struct tty_struct *tty)
 	qt_port->RxHolding = 1;
 
 	mutex_unlock(&qt_port->lock);
-	return;
 }
 
 static void qt_unthrottle(struct tty_struct *tty)
@@ -1477,8 +1474,6 @@ static void qt_unthrottle(struct tty_struct *tty)
 		}
 	}
 	mutex_unlock(&qt_port->lock);
-	return;
-
 }
 
 static int qt_calc_num_ports(struct usb_serial *serial)
diff --git a/drivers/staging/silicom/Kconfig b/drivers/staging/silicom/Kconfig
new file mode 100644
index 000000000000..eda2e7d73645
--- /dev/null
+++ b/drivers/staging/silicom/Kconfig
@@ -0,0 +1,46 @@
+#
+# Silicom device configuration
+#
+
+config NET_VENDOR_SILICOM
+	bool "Silicom devices"
+	default y
+	depends on PCI
+	---help---
+	  If you have a network card (Ethernet) belonging to this class,
+	  say Y.
+
+	  Note that the answer to this question does not directly affect
+	  the kernel: saying N will just case the configurator to skip all
+	  the questions regarding Silicom chipsets. If you say Y, you will be asked
+	  for your specific chipset/driver in the following questions.
+
+if NET_VENDOR_SILICOM
+
+config SBYPASS
+	tristate "Silicom BypassCTL library support"
+	depends on PCI && NET
+	depends on m
+	---help---
+	  If you have a network (Ethernet) controller of this type, say Y
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called bypass.
+
+config BPCTL
+	tristate "Silicom BypassCTL net support"
+	depends on PCI && NET
+	depends on m
+	select SBYPASS
+	select NET_CORE
+	select MII
+	---help---
+	  If you have a network (Ethernet) controller of this type, say Y
+	  or M and read the Ethernet-HOWTO, available from
+	  <http://www.tldp.org/docs.html#howto>.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called bpctl_mod.
+
+
+endif # NET_VENDOR_SILICOM
diff --git a/drivers/staging/silicom/Makefile b/drivers/staging/silicom/Makefile
new file mode 100644
index 000000000000..80e6d12d156b
--- /dev/null
+++ b/drivers/staging/silicom/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the Bypass network device drivers.
+#
+
+obj-$(CONFIG_BPCTL) += bpctl_mod.o
+obj-$(CONFIG_SBYPASS) += bypasslib/
+
+
+bpctl_mod-objs := bp_mod.o  bp_proc.o
diff --git a/drivers/staging/silicom/README b/drivers/staging/silicom/README
new file mode 100644
index 000000000000..ae970b37fdc6
--- /dev/null
+++ b/drivers/staging/silicom/README
@@ -0,0 +1,14 @@
+
+Theory of Operation:
+
+The Silicom Bypass Network Interface Cards (NICs) are network cards with paired ports (2 or 4). 
+The pairs either act as a "wire" allowing the network packets to pass or insert the device in 
+between the two ports.  When paired with the on-board hardware watchdog or other failsafe, 
+they provide high availability for the network in the face of software outages or maintenance.
+
+The software requirements are for a kernel level driver that interfaces with the bypass and watchdog,
+as well as for control software. User control can be either the provided standalone executable 
+(/bin/bpctl) or the API exposed by the Silicom library.
+
+
+
diff --git a/drivers/staging/silicom/TODO b/drivers/staging/silicom/TODO
new file mode 100644
index 000000000000..09d07b0ea9c0
--- /dev/null
+++ b/drivers/staging/silicom/TODO
@@ -0,0 +1,8 @@
+TODO:
+	- checkpatch.pl cleanups
+	- locking audit
+	- single module with all functionality
+	- userland 
+	- fix monolithic build.
+
+
diff --git a/drivers/staging/silicom/bits.h b/drivers/staging/silicom/bits.h
new file mode 100644
index 000000000000..8c411d0d4ecd
--- /dev/null
+++ b/drivers/staging/silicom/bits.h
@@ -0,0 +1,56 @@
+/******************************************************************************/
+/*                                                                            */
+/* Broadcom BCM5700 Linux Network Driver, Copyright (c) 2000 - 2004 Broadcom  */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/* History:                                                                   */
+/*    02/25/00 Hav Khauv        Initial version.                              */
+/******************************************************************************/
+
+#ifndef BITS_H
+#define BITS_H
+
+/******************************************************************************/
+/* Bit Mask definitions */
+/******************************************************************************/
+
+#define BIT_NONE            0x00
+#define BIT_0               0x01
+#define BIT_1               0x02
+#define BIT_2               0x04
+#define BIT_3               0x08
+#define BIT_4               0x10
+#define BIT_5               0x20
+#define BIT_6               0x40
+#define BIT_7               0x80
+#define BIT_8               0x0100
+#define BIT_9               0x0200
+#define BIT_10              0x0400
+#define BIT_11              0x0800
+#define BIT_12              0x1000
+#define BIT_13              0x2000
+#define BIT_14              0x4000
+#define BIT_15              0x8000
+#define BIT_16              0x010000
+#define BIT_17              0x020000
+#define BIT_18              0x040000
+#define BIT_19              0x080000
+#define BIT_20              0x100000
+#define BIT_21              0x200000
+#define BIT_22              0x400000
+#define BIT_23              0x800000
+#define BIT_24              0x01000000
+#define BIT_25              0x02000000
+#define BIT_26              0x04000000
+#define BIT_27              0x08000000
+#define BIT_28              0x10000000
+#define BIT_29              0x20000000
+#define BIT_30              0x40000000
+#define BIT_31              0x80000000
+
+#endif				/* BITS_H */
diff --git a/drivers/staging/silicom/bp_ioctl.h b/drivers/staging/silicom/bp_ioctl.h
new file mode 100644
index 000000000000..57de34a69e8e
--- /dev/null
+++ b/drivers/staging/silicom/bp_ioctl.h
@@ -0,0 +1,140 @@
+/******************************************************************************/
+/*                                                                            */
+/* Silicom Bypass Control Utility, Copyright (c) 2005-2007 Silicom            */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/******************************************************************************/
+
+#ifndef BP_IOCTL_H
+#define BP_IOCTL_H
+
+#define BP_CAP                   0x01	/* BIT_0 */
+#define BP_STATUS_CAP            0x02
+#define BP_STATUS_CHANGE_CAP     0x04
+#define SW_CTL_CAP               0x08
+#define BP_DIS_CAP               0x10
+#define BP_DIS_STATUS_CAP        0x20
+#define STD_NIC_CAP              0x40
+#define BP_PWOFF_ON_CAP          0x80
+#define BP_PWOFF_OFF_CAP         0x0100
+#define BP_PWOFF_CTL_CAP         0x0200
+#define BP_PWUP_ON_CAP           0x0400
+#define BP_PWUP_OFF_CAP          0x0800
+#define BP_PWUP_CTL_CAP          0x1000
+#define WD_CTL_CAP               0x2000
+#define WD_STATUS_CAP            0x4000
+#define WD_TIMEOUT_CAP           0x8000
+#define TX_CTL_CAP               0x10000
+#define TX_STATUS_CAP            0x20000
+#define TAP_CAP                  0x40000
+#define TAP_STATUS_CAP           0x80000
+#define TAP_STATUS_CHANGE_CAP    0x100000
+#define TAP_DIS_CAP              0x200000
+#define TAP_DIS_STATUS_CAP       0x400000
+#define TAP_PWUP_ON_CAP          0x800000
+#define TAP_PWUP_OFF_CAP         0x1000000
+#define TAP_PWUP_CTL_CAP         0x2000000
+#define NIC_CAP_NEG              0x4000000
+#define TPL_CAP                  0x8000000
+#define DISC_CAP                 0x10000000
+#define DISC_DIS_CAP             0x20000000
+#define DISC_PWUP_CTL_CAP        0x40000000
+
+#define TPL2_CAP_EX              0x01
+#define DISC_PORT_CAP_EX         0x02
+
+#define WD_MIN_TIME_MASK(val)      (val & 0xf)
+#define WD_STEP_COUNT_MASK(val)    ((val & 0xf) << 5)
+#define WDT_STEP_TIME              0x10	/* BIT_4 */
+
+#define WD_MIN_TIME_GET(desc)   (desc & 0xf)
+#define WD_STEP_COUNT_GET(desc) ((desc>>5) & 0xf)
+
+typedef enum {
+	IF_SCAN,
+	GET_DEV_NUM,
+	IS_BYPASS,
+	GET_BYPASS_SLAVE,
+	GET_BYPASS_CAPS,
+	GET_WD_SET_CAPS,
+	SET_BYPASS,
+	GET_BYPASS,
+	GET_BYPASS_CHANGE,
+	SET_BYPASS_WD,
+	GET_BYPASS_WD,
+	GET_WD_EXPIRE_TIME,
+	RESET_BYPASS_WD_TIMER,
+	SET_DIS_BYPASS,
+	GET_DIS_BYPASS,
+	SET_BYPASS_PWOFF,
+	GET_BYPASS_PWOFF,
+	SET_BYPASS_PWUP,
+	GET_BYPASS_PWUP,
+	SET_STD_NIC,
+	GET_STD_NIC,
+	SET_TX,
+	GET_TX,
+	SET_TAP,
+	GET_TAP,
+	GET_TAP_CHANGE,
+	SET_DIS_TAP,
+	GET_DIS_TAP,
+	SET_TAP_PWUP,
+	GET_TAP_PWUP,
+	SET_WD_EXP_MODE,
+	GET_WD_EXP_MODE,
+	SET_WD_AUTORESET,
+	GET_WD_AUTORESET,
+	SET_TPL,
+	GET_TPL,
+	SET_DISC,
+	GET_DISC,
+	GET_DISC_CHANGE,
+	SET_DIS_DISC,
+	GET_DIS_DISC,
+	SET_DISC_PWUP,
+	GET_DISC_PWUP,
+	GET_BYPASS_INFO = 100,
+	GET_BP_WAIT_AT_PWUP,
+	SET_BP_WAIT_AT_PWUP,
+	GET_BP_HW_RESET,
+	SET_BP_HW_RESET,
+	SET_DISC_PORT,
+	GET_DISC_PORT,
+	SET_DISC_PORT_PWUP,
+	GET_DISC_PORT_PWUP,
+	SET_BP_FORCE_LINK,
+	GET_BP_FORCE_LINK,
+#ifdef BP_SELF_TEST
+	SET_BP_SELF_TEST = 200,
+	GET_BP_SELF_TEST,
+#endif
+
+} CMND_TYPE_SD;
+
+/*
+* The major device number. We can't rely on dynamic
+* registration any more, because ioctls need to know
+* it.
+*/
+
+#define MAGIC_NUM 'J'
+
+/* for passing single values */
+struct bpctl_cmd {
+	int status;
+	int data[8];
+	int in_param[8];
+	int out_param[8];
+};
+
+#define IOCTL_TX_MSG(cmd) _IOWR(MAGIC_NUM, cmd, struct bpctl_cmd)
+
+#define DEVICE_NAME "bpctl"
+
+#endif
diff --git a/drivers/staging/silicom/bp_mod.c b/drivers/staging/silicom/bp_mod.c
new file mode 100644
index 000000000000..3cfd0516adfa
--- /dev/null
+++ b/drivers/staging/silicom/bp_mod.c
@@ -0,0 +1,8931 @@
+/******************************************************************************/
+/*                                                                            */
+/* Bypass Control utility, Copyright (c) 2005-20011 Silicom                   */
+/*                                                                            */
+/* This program is free software; you can redistribute it and/or modify       */
+/* it under the terms of the GNU General Public License as published by       */
+/* the Free Software Foundation, located in the file LICENSE.                 */
+/*  Copyright(c) 2007 - 2009 Intel Corporation. All rights reserved.          */
+/*                                                                            */
+/*                                                                            */
+/******************************************************************************/
+#include <linux/version.h>
+
+#include <linux/kernel.h>	/* We're doing kernel work */
+#include <linux/module.h>	/* Specifically, a module */
+#include <linux/fs.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/rtnetlink.h>
+#include <linux/rcupdate.h>
+#include <linux/etherdevice.h>
+
+#include <linux/uaccess.h>	/* for get_user and put_user */
+#include <linux/sched.h>
+#include <linux/ethtool.h>
+#include <linux/proc_fs.h>
+
+#include "bp_ioctl.h"
+#include "bp_mod.h"
+#include "bypass.h"
+#include "libbp_sd.h"
+
+#define SUCCESS 0
+#define BP_MOD_VER  "9.0.4"
+#define BP_MOD_DESCR "Silicom Bypass-SD Control driver"
+#define BP_SYNC_FLAG 1
+
+static int Device_Open = 0;
+static int major_num = 0;
+
+MODULE_AUTHOR("Anna Lukin, annal@silicom.co.il");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(BP_MOD_DESCR);
+MODULE_VERSION(BP_MOD_VER);
+spinlock_t bpvm_lock;
+
+#define lock_bpctl() 					\
+if (down_interruptible(&bpctl_sema)) {			\
+	return -ERESTARTSYS;				\
+}							\
+
+#define unlock_bpctl() 					\
+	up(&bpctl_sema);
+
+/* Media Types */
+typedef enum {
+	bp_copper = 0,
+	bp_fiber,
+	bp_cx4,
+	bp_none,
+} bp_media_type;
+
+struct pfs_unit_sd {
+	struct proc_dir_entry *proc_entry;
+	char proc_name[32];
+};
+
+struct bypass_pfs_sd {
+	char dir_name[32];
+	struct proc_dir_entry *bypass_entry;
+	struct pfs_unit_sd bypass_info;
+	struct pfs_unit_sd bypass_slave;
+	struct pfs_unit_sd bypass_caps;
+	struct pfs_unit_sd wd_set_caps;
+	struct pfs_unit_sd bypass;
+	struct pfs_unit_sd bypass_change;
+	struct pfs_unit_sd bypass_wd;
+	struct pfs_unit_sd wd_expire_time;
+	struct pfs_unit_sd reset_bypass_wd;
+	struct pfs_unit_sd dis_bypass;
+	struct pfs_unit_sd bypass_pwup;
+	struct pfs_unit_sd bypass_pwoff;
+	struct pfs_unit_sd std_nic;
+	struct pfs_unit_sd tap;
+	struct pfs_unit_sd dis_tap;
+	struct pfs_unit_sd tap_pwup;
+	struct pfs_unit_sd tap_change;
+	struct pfs_unit_sd wd_exp_mode;
+	struct pfs_unit_sd wd_autoreset;
+	struct pfs_unit_sd tpl;
+
+};
+
+typedef struct _bpctl_dev {
+	char *name;
+	char *desc;
+	struct pci_dev *pdev;	/* PCI device */
+	struct net_device *ndev;	/* net device */
+	unsigned long mem_map;
+	uint8_t bus;
+	uint8_t slot;
+	uint8_t func;
+	u_int32_t device;
+	u_int32_t vendor;
+	u_int32_t subvendor;
+	u_int32_t subdevice;
+	int ifindex;
+	uint32_t bp_caps;
+	uint32_t bp_caps_ex;
+	uint8_t bp_fw_ver;
+	int bp_ext_ver;
+	int wdt_status;
+	unsigned long bypass_wdt_on_time;
+	uint32_t bypass_timer_interval;
+	struct timer_list bp_timer;
+	uint32_t reset_time;
+	uint8_t bp_status_un;
+	atomic_t wdt_busy;
+	bp_media_type media_type;
+	int bp_tpl_flag;
+	struct timer_list bp_tpl_timer;
+	spinlock_t bypass_wr_lock;
+	int bp_10g;
+	int bp_10gb;
+	int bp_fiber5;
+	int bp_10g9;
+	int bp_i80;
+	int bp_540;
+	int (*hard_start_xmit_save) (struct sk_buff *skb,
+				     struct net_device *dev);
+	const struct net_device_ops *old_ops;
+	struct net_device_ops new_ops;
+	int bp_self_test_flag;
+	char *bp_tx_data;
+	struct bypass_pfs_sd bypass_pfs_set;
+
+} bpctl_dev_t;
+
+static bpctl_dev_t *bpctl_dev_arr;
+
+static struct semaphore bpctl_sema;
+static int device_num = 0;
+
+static int get_dev_idx(int ifindex);
+static bpctl_dev_t *get_master_port_fn(bpctl_dev_t *pbpctl_dev);
+static int disc_status(bpctl_dev_t *pbpctl_dev);
+static int bypass_status(bpctl_dev_t *pbpctl_dev);
+static int wdt_timer(bpctl_dev_t *pbpctl_dev, int *time_left);
+static bpctl_dev_t *get_status_port_fn(bpctl_dev_t *pbpctl_dev);
+static void if_scan_init(void);
+
+int bypass_proc_create_dev_sd(bpctl_dev_t *pbp_device_block);
+int bypass_proc_remove_dev_sd(bpctl_dev_t *pbp_device_block);
+int bp_proc_create(void);
+
+int is_bypass_fn(bpctl_dev_t *pbpctl_dev);
+int get_dev_idx_bsf(int bus, int slot, int func);
+
+static unsigned long str_to_hex(char *p);
+static int bp_device_event(struct notifier_block *unused,
+			   unsigned long event, void *ptr)
+{
+	struct net_device *dev = ptr;
+	static bpctl_dev_t *pbpctl_dev = NULL, *pbpctl_dev_m = NULL;
+	int dev_num = 0, ret = 0, ret_d = 0, time_left = 0;
+	/* printk("BP_PROC_SUPPORT event =%d %s %d\n", event,dev->name, dev->ifindex ); */
+	/* return NOTIFY_DONE; */
+	if (!dev)
+		return NOTIFY_DONE;
+	if (event == NETDEV_REGISTER) {
+		{
+			struct ethtool_drvinfo drvinfo;
+			char cbuf[32];
+			char *buf = NULL;
+			char res[10];
+			int i = 0, ifindex, idx_dev = 0;
+			int bus = 0, slot = 0, func = 0;
+			ifindex = dev->ifindex;
+
+			memset(res, 0, 10);
+			memset(&drvinfo, 0, sizeof(struct ethtool_drvinfo));
+
+			if (dev->ethtool_ops && dev->ethtool_ops->get_drvinfo) {
+				memset(&drvinfo, 0, sizeof(drvinfo));
+				dev->ethtool_ops->get_drvinfo(dev, &drvinfo);
+			} else
+				return NOTIFY_DONE;
+			if (!drvinfo.bus_info)
+				return NOTIFY_DONE;
+			if (!strcmp(drvinfo.bus_info, "N/A"))
+				return NOTIFY_DONE;
+			memcpy(&cbuf, drvinfo.bus_info, 32);
+			buf = &cbuf[0];
+
+			while (*buf++ != ':') ;
+			for (i = 0; i < 10; i++, buf++) {
+				if (*buf == ':')
+					break;
+				res[i] = *buf;
+
+			}
+			buf++;
+			bus = str_to_hex(res);
+			memset(res, 0, 10);
+
+			for (i = 0; i < 10; i++, buf++) {
+				if (*buf == '.')
+					break;
+				res[i] = *buf;
+
+			}
+			buf++;
+			slot = str_to_hex(res);
+			func = str_to_hex(buf);
+			idx_dev = get_dev_idx_bsf(bus, slot, func);
+
+			if (idx_dev != -1) {
+
+				bpctl_dev_arr[idx_dev].ifindex = ifindex;
+				bpctl_dev_arr[idx_dev].ndev = dev;
+
+				bypass_proc_remove_dev_sd(&bpctl_dev_arr
+							  [idx_dev]);
+				bypass_proc_create_dev_sd(&bpctl_dev_arr
+							  [idx_dev]);
+
+			}
+
+		}
+		return NOTIFY_DONE;
+
+	}
+	if (event == NETDEV_UNREGISTER) {
+		int idx_dev = 0;
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if (bpctl_dev_arr[idx_dev].ndev == dev) {
+				bypass_proc_remove_dev_sd(&bpctl_dev_arr
+							  [idx_dev]);
+				bpctl_dev_arr[idx_dev].ndev = NULL;
+
+				return NOTIFY_DONE;
+
+			}
+
+		}
+		return NOTIFY_DONE;
+	}
+	if (event == NETDEV_CHANGENAME) {
+		int idx_dev = 0;
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if (bpctl_dev_arr[idx_dev].ndev == dev) {
+				bypass_proc_remove_dev_sd(&bpctl_dev_arr
+							  [idx_dev]);
+				bypass_proc_create_dev_sd(&bpctl_dev_arr
+							  [idx_dev]);
+
+				return NOTIFY_DONE;
+
+			}
+
+		}
+		return NOTIFY_DONE;
+
+	}
+
+	switch (event) {
+
+	case NETDEV_CHANGE:{
+			if (netif_carrier_ok(dev))
+				return NOTIFY_DONE;
+
+			if (((dev_num = get_dev_idx(dev->ifindex)) == -1) ||
+			    (!(pbpctl_dev = &bpctl_dev_arr[dev_num])))
+				return NOTIFY_DONE;
+
+			if ((is_bypass_fn(pbpctl_dev)) == 1)
+				pbpctl_dev_m = pbpctl_dev;
+			else
+				pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+			if (!pbpctl_dev_m)
+				return NOTIFY_DONE;
+			ret = bypass_status(pbpctl_dev_m);
+			if (ret == 1)
+				printk("bpmod: %s is in the Bypass mode now",
+				       dev->name);
+			ret_d = disc_status(pbpctl_dev_m);
+			if (ret_d == 1)
+				printk
+				    ("bpmod: %s is in the Disconnect mode now",
+				     dev->name);
+			if (ret || ret_d) {
+				wdt_timer(pbpctl_dev_m, &time_left);
+				if (time_left == -1)
+					printk("; WDT has expired");
+				printk(".\n");
+
+			}
+			return NOTIFY_DONE;
+
+		}
+
+	default:
+		return NOTIFY_DONE;
+
+	}
+	return NOTIFY_DONE;
+
+}
+
+static struct notifier_block bp_notifier_block = {
+	.notifier_call = bp_device_event,
+};
+
+static int device_open(struct inode *inode, struct file *file)
+{
+#ifdef DEBUG
+	printk("device_open(%p)\n", file);
+#endif
+	Device_Open++;
+/*
+* Initialize the message
+*/
+	return SUCCESS;
+}
+
+static int device_release(struct inode *inode, struct file *file)
+{
+#ifdef DEBUG
+	printk("device_release(%p,%p)\n", inode, file);
+#endif
+	Device_Open--;
+	return SUCCESS;
+}
+
+int is_bypass_fn(bpctl_dev_t *pbpctl_dev);
+int wdt_time_left(bpctl_dev_t *pbpctl_dev);
+
+static void write_pulse(bpctl_dev_t *pbpctl_dev,
+			unsigned int ctrl_ext,
+			unsigned char value, unsigned char len)
+{
+	unsigned char ctrl_val = 0;
+	unsigned int i = len;
+	unsigned int ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_c = NULL;
+
+	if (pbpctl_dev->bp_i80)
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	if (pbpctl_dev->bp_540)
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+
+	if (pbpctl_dev->bp_10g9) {
+		if (!(pbpctl_dev_c = get_status_port_fn(pbpctl_dev)))
+			return;
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+	}
+
+	while (i--) {
+		ctrl_val = (value >> i) & 0x1;
+		if (ctrl_val) {
+			if (pbpctl_dev->bp_10g9) {
+
+				/* To start management : MCLK 1, MDIO 1, output */
+				/* DATA 1 CLK 1 */
+				/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext|BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9)); */
+				BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+						ctrl_ext |
+						BP10G_MDIO_DATA_OUT9);
+				BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+						(ctrl | BP10G_MCLK_DATA_OUT9 |
+						 BP10G_MCLK_DIR_OUT9));
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, (ctrl_ext |
+								      BPCTLI_CTRL_EXT_MCLK_DIR5
+								      |
+								      BPCTLI_CTRL_EXT_MDIO_DIR5
+								      |
+								      BPCTLI_CTRL_EXT_MDIO_DATA5
+								      |
+								      BPCTLI_CTRL_EXT_MCLK_DATA5));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, (ctrl_ext |
+								      BPCTLI_CTRL_EXT_MDIO_DIR80
+								      |
+								      BPCTLI_CTRL_EXT_MDIO_DATA80));
+
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, (ctrl |
+									  BPCTLI_CTRL_EXT_MCLK_DIR80
+									  |
+									  BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+			} else if (pbpctl_dev->bp_540) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP, (ctrl |
+								   BP540_MDIO_DIR
+								   |
+								   BP540_MDIO_DATA
+								   |
+								   BP540_MCLK_DIR
+								   |
+								   BP540_MCLK_DATA));
+
+			} else if (pbpctl_dev->bp_10gb) {
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+						 (ctrl_ext | BP10GB_MDIO_SET |
+						  BP10GB_MCLK_SET) &
+						 ~(BP10GB_MCLK_DIR |
+						   BP10GB_MDIO_DIR |
+						   BP10GB_MDIO_CLR |
+						   BP10GB_MCLK_CLR));
+
+			} else if (!pbpctl_dev->bp_10g)
+				/* To start management : MCLK 1, MDIO 1, output */
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   (ctrl_ext |
+						    BPCTLI_CTRL_EXT_MCLK_DIR |
+						    BPCTLI_CTRL_EXT_MDIO_DIR |
+						    BPCTLI_CTRL_EXT_MDIO_DATA |
+						    BPCTLI_CTRL_EXT_MCLK_DATA));
+			else {
+
+				/* To start management : MCLK 1, MDIO 1, output*/
+				BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+						(ctrl_ext | BP10G_MCLK_DATA_OUT
+						 | BP10G_MDIO_DATA_OUT));
+
+			}
+
+			usec_delay(PULSE_TIME);
+			if (pbpctl_dev->bp_10g9) {
+
+				/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, ((ctrl_ext|BP10G_MDIO_DATA_OUT9)&~(BP10G_MCLK_DATA_OUT9))); */
+				/* DATA 1 CLK 0 */
+				BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+						ctrl_ext |
+						BP10G_MDIO_DATA_OUT9);
+				BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+						(ctrl | BP10G_MCLK_DIR_OUT9) &
+						~BP10G_MCLK_DATA_OUT9);
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR5 |
+						     BPCTLI_CTRL_EXT_MDIO_DIR5 |
+						     BPCTLI_CTRL_EXT_MDIO_DATA5)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MCLK_DATA5)));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, (ctrl_ext |
+								      BPCTLI_CTRL_EXT_MDIO_DIR80
+								      |
+								      BPCTLI_CTRL_EXT_MDIO_DATA80));
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl |
+						     BPCTLI_CTRL_EXT_MCLK_DIR80)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MCLK_DATA80)));
+
+			} else if (pbpctl_dev->bp_540) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						(ctrl | BP540_MDIO_DIR |
+						 BP540_MDIO_DATA |
+						 BP540_MCLK_DIR) &
+						~(BP540_MCLK_DATA));
+
+			} else if (pbpctl_dev->bp_10gb) {
+
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+						 (ctrl_ext | BP10GB_MDIO_SET |
+						  BP10GB_MCLK_CLR) &
+						 ~(BP10GB_MCLK_DIR |
+						   BP10GB_MDIO_DIR |
+						   BP10GB_MDIO_CLR |
+						   BP10GB_MCLK_SET));
+
+			} else if (!pbpctl_dev->bp_10g)
+
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR |
+						     BPCTLI_CTRL_EXT_MDIO_DIR |
+						     BPCTLI_CTRL_EXT_MDIO_DATA)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MCLK_DATA)));
+			else {
+
+				BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+						((ctrl_ext |
+						  BP10G_MDIO_DATA_OUT) &
+						 ~(BP10G_MCLK_DATA_OUT)));
+			}
+
+			usec_delay(PULSE_TIME);
+
+		} else {
+			if (pbpctl_dev->bp_10g9) {
+				/* DATA 0 CLK 1 */
+				/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, ((ctrl_ext|BP10G_MCLK_DATA_OUT9)&~BP10G_MDIO_DATA_OUT9)); */
+				BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+						(ctrl_ext &
+						 ~BP10G_MDIO_DATA_OUT9));
+				BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+						(ctrl | BP10G_MCLK_DATA_OUT9 |
+						 BP10G_MCLK_DIR_OUT9));
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR5 |
+						     BPCTLI_CTRL_EXT_MDIO_DIR5 |
+						     BPCTLI_CTRL_EXT_MCLK_DATA5)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MDIO_DATA5)));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MDIO_DIR80)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MDIO_DATA80)));
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   (ctrl |
+						    BPCTLI_CTRL_EXT_MCLK_DIR80 |
+						    BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+			} else if (pbpctl_dev->bp_540) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP540_MCLK_DIR |
+						  BP540_MCLK_DATA |
+						  BP540_MDIO_DIR) &
+						 ~(BP540_MDIO_DATA)));
+
+			} else if (pbpctl_dev->bp_10gb) {
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+						 (ctrl_ext | BP10GB_MDIO_CLR |
+						  BP10GB_MCLK_SET) &
+						 ~(BP10GB_MCLK_DIR |
+						   BP10GB_MDIO_DIR |
+						   BP10GB_MDIO_SET |
+						   BP10GB_MCLK_CLR));
+
+			} else if (!pbpctl_dev->bp_10g)
+
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR |
+						     BPCTLI_CTRL_EXT_MDIO_DIR |
+						     BPCTLI_CTRL_EXT_MCLK_DATA)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MDIO_DATA)));
+			else {
+
+				BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+						((ctrl_ext |
+						  BP10G_MCLK_DATA_OUT) &
+						 ~BP10G_MDIO_DATA_OUT));
+
+			}
+			usec_delay(PULSE_TIME);
+			if (pbpctl_dev->bp_10g9) {
+				/* DATA 0 CLK 0 */
+				/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+				BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+						(ctrl_ext &
+						 ~BP10G_MDIO_DATA_OUT9));
+				BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+						((ctrl | BP10G_MCLK_DIR_OUT9) &
+						 ~(BP10G_MCLK_DATA_OUT9)));
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR5 |
+						     BPCTLI_CTRL_EXT_MDIO_DIR5)
+						    &
+						    ~(BPCTLI_CTRL_EXT_MCLK_DATA5
+						      |
+						      BPCTLI_CTRL_EXT_MDIO_DATA5)));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MDIO_DIR80)
+						    &
+						    ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl |
+						     BPCTLI_CTRL_EXT_MCLK_DIR80)
+						    &
+						    ~
+						    (BPCTLI_CTRL_EXT_MCLK_DATA80)));
+
+			} else if (pbpctl_dev->bp_540) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP540_MCLK_DIR |
+						  BP540_MDIO_DIR) &
+						 ~(BP540_MDIO_DATA |
+						   BP540_MCLK_DATA)));
+			} else if (pbpctl_dev->bp_10gb) {
+
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+						 (ctrl_ext | BP10GB_MDIO_CLR |
+						  BP10GB_MCLK_CLR) &
+						 ~(BP10GB_MCLK_DIR |
+						   BP10GB_MDIO_DIR |
+						   BP10GB_MDIO_SET |
+						   BP10GB_MCLK_SET));
+
+			} else if (!pbpctl_dev->bp_10g)
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl_ext |
+						     BPCTLI_CTRL_EXT_MCLK_DIR |
+						     BPCTLI_CTRL_EXT_MDIO_DIR) &
+						    ~(BPCTLI_CTRL_EXT_MCLK_DATA
+						      |
+						      BPCTLI_CTRL_EXT_MDIO_DATA)));
+			else {
+
+				BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+						(ctrl_ext &
+						 ~(BP10G_MCLK_DATA_OUT |
+						   BP10G_MDIO_DATA_OUT)));
+			}
+
+			usec_delay(PULSE_TIME);
+		}
+
+	}
+}
+
+static int read_pulse(bpctl_dev_t *pbpctl_dev, unsigned int ctrl_ext,
+		      unsigned char len)
+{
+	unsigned char ctrl_val = 0;
+	unsigned int i = len;
+	unsigned int ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_c = NULL;
+
+	if (pbpctl_dev->bp_i80)
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	if (pbpctl_dev->bp_540)
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+	if (pbpctl_dev->bp_10g9) {
+		if (!(pbpctl_dev_c = get_status_port_fn(pbpctl_dev)))
+			return -1;
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+	}
+
+
+	while (i--) {
+		if (pbpctl_dev->bp_10g9) {
+			/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, ((ctrl_ext|BP10G_MDIO_DATA_OUT9)&~BP10G_MCLK_DATA_OUT9)); */
+			/* DATA ? CLK 0 */
+			BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+					((ctrl | BP10G_MCLK_DIR_OUT9) &
+					 ~(BP10G_MCLK_DATA_OUT9)));
+
+		} else if (pbpctl_dev->bp_fiber5) {
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+							       BPCTLI_CTRL_EXT_MCLK_DIR5)
+							      &
+							      ~
+							      (BPCTLI_CTRL_EXT_MDIO_DIR5
+							       |
+							       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+
+		} else if (pbpctl_dev->bp_i80) {
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+					   (ctrl_ext &
+					    ~BPCTLI_CTRL_EXT_MDIO_DIR80));
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+					   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80)
+					    & ~(BPCTLI_CTRL_EXT_MCLK_DATA80)));
+
+		} else if (pbpctl_dev->bp_540) {
+			BP10G_WRITE_REG(pbpctl_dev, ESDP,
+					((ctrl | BP540_MCLK_DIR) &
+					 ~(BP540_MDIO_DIR | BP540_MCLK_DATA)));
+
+		} else if (pbpctl_dev->bp_10gb) {
+
+			BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+					 (ctrl_ext | BP10GB_MDIO_DIR |
+					  BP10GB_MCLK_CLR) & ~(BP10GB_MCLK_DIR |
+							       BP10GB_MDIO_CLR |
+							       BP10GB_MDIO_SET |
+							       BP10GB_MCLK_SET));
+
+		} else if (!pbpctl_dev->bp_10g)
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+								   BPCTLI_CTRL_EXT_MCLK_DIR)
+								  &
+								  ~
+								  (BPCTLI_CTRL_EXT_MDIO_DIR
+								   |
+								   BPCTLI_CTRL_EXT_MCLK_DATA)));
+		else {
+
+			BP10G_WRITE_REG(pbpctl_dev, EODSDP, ((ctrl_ext | BP10G_MDIO_DATA_OUT) & ~BP10G_MCLK_DATA_OUT));	/* ? */
+			/*    printk("0x28=0x%x\n",BP10G_READ_REG(pbpctl_dev,EODSDP);); */
+
+		}
+
+		usec_delay(PULSE_TIME);
+		if (pbpctl_dev->bp_10g9) {
+			/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext|BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9)); */
+			/* DATA ? CLK 1 */
+			BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+					(ctrl | BP10G_MCLK_DATA_OUT9 |
+					 BP10G_MCLK_DIR_OUT9));
+
+		} else if (pbpctl_dev->bp_fiber5) {
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+							       BPCTLI_CTRL_EXT_MCLK_DIR5
+							       |
+							       BPCTLI_CTRL_EXT_MCLK_DATA5)
+							      &
+							      ~
+							      (BPCTLI_CTRL_EXT_MDIO_DIR5)));
+
+		} else if (pbpctl_dev->bp_i80) {
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+					   (ctrl_ext &
+					    ~(BPCTLI_CTRL_EXT_MDIO_DIR80)));
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+					   (ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80 |
+					    BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+		} else if (pbpctl_dev->bp_540) {
+			BP10G_WRITE_REG(pbpctl_dev, ESDP,
+					((ctrl | BP540_MCLK_DIR |
+					  BP540_MCLK_DATA) &
+					 ~(BP540_MDIO_DIR)));
+
+		} else if (pbpctl_dev->bp_10gb) {
+			BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+					 (ctrl_ext | BP10GB_MDIO_DIR |
+					  BP10GB_MCLK_SET) & ~(BP10GB_MCLK_DIR |
+							       BP10GB_MDIO_CLR |
+							       BP10GB_MDIO_SET |
+							       BP10GB_MCLK_CLR));
+
+		} else if (!pbpctl_dev->bp_10g)
+			BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+								   BPCTLI_CTRL_EXT_MCLK_DIR
+								   |
+								   BPCTLI_CTRL_EXT_MCLK_DATA)
+								  &
+								  ~
+								  (BPCTLI_CTRL_EXT_MDIO_DIR)));
+		else {
+
+			BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+					(ctrl_ext | BP10G_MCLK_DATA_OUT |
+					 BP10G_MDIO_DATA_OUT));
+
+		}
+		if (pbpctl_dev->bp_10g9) {
+			ctrl_ext = BP10G_READ_REG(pbpctl_dev, I2CCTL);
+
+		} else if ((pbpctl_dev->bp_fiber5) || (pbpctl_dev->bp_i80)) {
+			ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		} else if (pbpctl_dev->bp_540) {
+			ctrl_ext = BP10G_READ_REG(pbpctl_dev, ESDP);
+		} else if (pbpctl_dev->bp_10gb)
+			ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+
+		else if (!pbpctl_dev->bp_10g)
+			ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		else
+			ctrl_ext = BP10G_READ_REG(pbpctl_dev, EODSDP);
+
+		usec_delay(PULSE_TIME);
+		if (pbpctl_dev->bp_10g9) {
+			if (ctrl_ext & BP10G_MDIO_DATA_IN9)
+				ctrl_val |= 1 << i;
+
+		} else if (pbpctl_dev->bp_fiber5) {
+			if (ctrl_ext & BPCTLI_CTRL_EXT_MDIO_DATA5)
+				ctrl_val |= 1 << i;
+		} else if (pbpctl_dev->bp_i80) {
+			if (ctrl_ext & BPCTLI_CTRL_EXT_MDIO_DATA80)
+				ctrl_val |= 1 << i;
+		} else if (pbpctl_dev->bp_540) {
+			if (ctrl_ext & BP540_MDIO_DATA)
+				ctrl_val |= 1 << i;
+		} else if (pbpctl_dev->bp_10gb) {
+			if (ctrl_ext & BP10GB_MDIO_DATA)
+				ctrl_val |= 1 << i;
+
+		} else if (!pbpctl_dev->bp_10g) {
+
+			if (ctrl_ext & BPCTLI_CTRL_EXT_MDIO_DATA)
+				ctrl_val |= 1 << i;
+		} else {
+
+			if (ctrl_ext & BP10G_MDIO_DATA_IN)
+				ctrl_val |= 1 << i;
+		}
+
+	}
+
+	return ctrl_val;
+}
+
+static void write_reg(bpctl_dev_t *pbpctl_dev, unsigned char value,
+		      unsigned char addr)
+{
+	uint32_t ctrl_ext = 0, ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_c = NULL;
+	unsigned long flags;
+	if (pbpctl_dev->bp_10g9) {
+		if (!(pbpctl_dev_c = get_status_port_fn(pbpctl_dev)))
+			return;
+	}
+	if ((pbpctl_dev->wdt_status == WDT_STATUS_EN) &&
+	    (pbpctl_dev->bp_ext_ver < PXG4BPFI_VER))
+		wdt_time_left(pbpctl_dev);
+
+#ifdef BP_SYNC_FLAG
+	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 1);
+#endif
+	if (pbpctl_dev->bp_10g9) {
+
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, I2CCTL);
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+		/* DATA 0 CLK 0 */
+		/* BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+	} else if (pbpctl_dev->bp_540) {
+		ctrl = ctrl_ext = BP10G_READ_REG(pbpctl_dev, ESDP);
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl |
+						    BP540_MDIO_DIR |
+						    BP540_MCLK_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+
+	} else if (!pbpctl_dev->bp_10g) {
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)));
+	} else {
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, EODSDP);
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+	}
+	usec_delay(CMND_INTERVAL);
+
+	/*send sync cmd */
+	write_pulse(pbpctl_dev, ctrl_ext, SYNC_CMD_VAL, SYNC_CMD_LEN);
+	/*send wr cmd */
+	write_pulse(pbpctl_dev, ctrl_ext, WR_CMD_VAL, WR_CMD_LEN);
+	write_pulse(pbpctl_dev, ctrl_ext, addr, ADDR_CMD_LEN);
+
+	/*write data */
+	write_pulse(pbpctl_dev, ctrl_ext, value, WR_DATA_LEN);
+	if (pbpctl_dev->bp_10g9) {
+		/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		/* DATA 0 CLK 0 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+	} else if (pbpctl_dev->bp_540) {
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl |
+						    BP540_MDIO_DIR |
+						    BP540_MCLK_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+	} else if (pbpctl_dev->bp_10gb) {
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+
+	} else if (!pbpctl_dev->bp_10g)
+
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)));
+	else {
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+
+	}
+
+	usec_delay(CMND_INTERVAL * 4);
+
+	if ((pbpctl_dev->wdt_status == WDT_STATUS_EN) &&
+	    (pbpctl_dev->bp_ext_ver < PXG4BPFI_VER) && (addr == CMND_REG_ADDR))
+		pbpctl_dev->bypass_wdt_on_time = jiffies;
+#ifdef BP_SYNC_FLAG
+	spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 0);
+#endif
+
+}
+
+static void write_data(bpctl_dev_t *pbpctl_dev, unsigned char value)
+{
+	write_reg(pbpctl_dev, value, CMND_REG_ADDR);
+}
+
+static int read_reg(bpctl_dev_t *pbpctl_dev, unsigned char addr)
+{
+	uint32_t ctrl_ext = 0, ctrl = 0, ctrl_value = 0;
+	bpctl_dev_t *pbpctl_dev_c = NULL;
+
+#ifdef BP_SYNC_FLAG
+	unsigned long flags;
+	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 1);
+#endif
+	if (pbpctl_dev->bp_10g9) {
+		if (!(pbpctl_dev_c = get_status_port_fn(pbpctl_dev)))
+			return -1;
+	}
+
+	if (pbpctl_dev->bp_10g9) {
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, I2CCTL);
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+
+		/* BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		/* DATA 0 CLK 0 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+	} else if (pbpctl_dev->bp_540) {
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, ESDP);
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl | BP540_MCLK_DIR |
+						    BP540_MDIO_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+#if 0
+
+		/*BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO, (ctrl_ext | BP10GB_MCLK_DIR | BP10GB_MDIO_DIR|
+		   BP10GB_MCLK_CLR|BP10GB_MDIO_CLR));
+		   ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+		   printk("1reg=%x\n", ctrl_ext); */
+
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO, ((ctrl_ext |
+							      BP10GB_MCLK_SET |
+							      BP10GB_MDIO_CLR))
+				 & ~(BP10GB_MCLK_CLR | BP10GB_MDIO_SET |
+				     BP10GB_MCLK_DIR | BP10GB_MDIO_DIR));
+
+		/*   bnx2x_set_spio(pbpctl_dev, 5, MISC_REGISTERS_SPIO_OUTPUT_LOW);
+		   bnx2x_set_spio(pbpctl_dev, 4, MISC_REGISTERS_SPIO_OUTPUT_LOW);
+		   bnx2x_set_spio(pbpctl_dev, 4, MISC_REGISTERS_SPIO_INPUT_HI_Z); */
+
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+
+		printk("2reg=%x\n", ctrl_ext);
+
+#ifdef BP_SYNC_FLAG
+		spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+		atomic_set(&pbpctl_dev->wdt_busy, 0);
+#endif
+
+		return 0;
+
+#endif
+
+	} else if (!pbpctl_dev->bp_10g) {
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)));
+	} else {
+
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, EODSDP);
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+
+	}
+
+	usec_delay(CMND_INTERVAL);
+
+	/*send sync cmd */
+	write_pulse(pbpctl_dev, ctrl_ext, SYNC_CMD_VAL, SYNC_CMD_LEN);
+	/*send rd cmd */
+	write_pulse(pbpctl_dev, ctrl_ext, RD_CMD_VAL, RD_CMD_LEN);
+	/*send addr */
+	write_pulse(pbpctl_dev, ctrl_ext, addr, ADDR_CMD_LEN);
+	/*read data */
+	/* zero */
+	if (pbpctl_dev->bp_10g9) {
+		/* DATA 0 CLK 1 */
+		/*BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext|BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9)); */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext | BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				(ctrl | BP10G_MCLK_DATA_OUT9 |
+				 BP10G_MCLK_DIR_OUT9));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DATA5)));
+
+	} else if (pbpctl_dev->bp_i80) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+				   (ctrl_ext &
+				    ~(BPCTLI_CTRL_EXT_MDIO_DATA80 |
+				      BPCTLI_CTRL_EXT_MDIO_DIR80)));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   (ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80 |
+				    BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+	} else if (pbpctl_dev->bp_540) {
+		BP10G_WRITE_REG(pbpctl_dev, ESDP,
+				(((ctrl | BP540_MDIO_DIR | BP540_MCLK_DIR |
+				   BP540_MCLK_DATA) & ~BP540_MDIO_DATA)));
+
+	} else if (pbpctl_dev->bp_10gb) {
+
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_DIR | BP10GB_MCLK_SET)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_SET |
+				     BP10GB_MDIO_CLR | BP10GB_MCLK_CLR));
+
+	} else if (!pbpctl_dev->bp_10g)
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DATA)));
+	else {
+
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext | BP10G_MCLK_DATA_OUT |
+				 BP10G_MDIO_DATA_OUT));
+
+
+	}
+	usec_delay(PULSE_TIME);
+
+	ctrl_value = read_pulse(pbpctl_dev, ctrl_ext, RD_DATA_LEN);
+
+	if (pbpctl_dev->bp_10g9) {
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, I2CCTL);
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+
+		/* BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		/* DATA 0 CLK 0 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+	} else if (pbpctl_dev->bp_540) {
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl | BP540_MCLK_DIR |
+						    BP540_MDIO_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+
+	} else if (!pbpctl_dev->bp_10g) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)));
+	} else {
+
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, EODSDP);
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+
+	}
+
+	usec_delay(CMND_INTERVAL * 4);
+#ifdef BP_SYNC_FLAG
+	spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 0);
+#endif
+
+	return ctrl_value;
+}
+
+static int wdt_pulse(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0, ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_c = NULL;
+
+#ifdef BP_SYNC_FLAG
+	unsigned long flags;
+
+	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+
+	if ((atomic_read(&pbpctl_dev->wdt_busy)) == 1)
+		return -1;
+#endif
+	if (pbpctl_dev->bp_10g9) {
+		if (!(pbpctl_dev_c = get_status_port_fn(pbpctl_dev)))
+			return -1;
+	}
+
+	if (pbpctl_dev->bp_10g9) {
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, I2CCTL);
+		ctrl = BP10G_READ_REG(pbpctl_dev_c, ESDP);
+
+		/* BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		/* DATA 0 CLK 0 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+	} else if (pbpctl_dev->bp_540) {
+		ctrl_ext = ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl | BP540_MCLK_DIR |
+						    BP540_MDIO_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+
+	} else if (!pbpctl_dev->bp_10g) {
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)));
+	} else {
+
+		ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+		ctrl_ext = BP10G_READ_REG(pbpctl_dev, EODSDP);
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+
+	}
+	if (pbpctl_dev->bp_10g9) {
+		/*   BP10G_WRITE_REG(pbpctl_dev, I2CCTL, ((ctrl_ext|BP10G_MCLK_DATA_OUT9)&~BP10G_MDIO_DATA_OUT9)); */
+		/* DATA 0 CLK 1 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				(ctrl | BP10G_MCLK_DATA_OUT9 |
+				 BP10G_MCLK_DIR_OUT9));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MCLK_DATA5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MDIO_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   (ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80 |
+				    BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+	} else if (pbpctl_dev->bp_540) {
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl |
+						    BP540_MDIO_DIR |
+						    BP540_MCLK_DIR |
+						    BP540_MCLK_DATA) &
+						   ~BP540_MDIO_DATA));
+
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_SET)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_CLR));
+
+	} else if (!pbpctl_dev->bp_10g)
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MCLK_DATA)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MDIO_DATA)));
+	else {
+
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				((ctrl_ext | BP10G_MCLK_DATA_OUT) &
+				 ~BP10G_MDIO_DATA_OUT));
+
+	}
+
+	usec_delay(WDT_INTERVAL);
+	if (pbpctl_dev->bp_10g9) {
+		/* BP10G_WRITE_REG(pbpctl_dev, I2CCTL, (ctrl_ext&~(BP10G_MCLK_DATA_OUT9|BP10G_MDIO_DATA_OUT9))); */
+		/* DATA 0 CLK 0 */
+		BP10G_WRITE_REG(pbpctl_dev, I2CCTL,
+				(ctrl_ext & ~BP10G_MDIO_DATA_OUT9));
+		BP10G_WRITE_REG(pbpctl_dev_c, ESDP,
+				((ctrl | BP10G_MCLK_DIR_OUT9) &
+				 ~(BP10G_MCLK_DATA_OUT9)));
+
+	} else if (pbpctl_dev->bp_fiber5) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MCLK_DIR5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DIR5)
+						      &
+						      ~
+						      (BPCTLI_CTRL_EXT_MCLK_DATA5
+						       |
+						       BPCTLI_CTRL_EXT_MDIO_DATA5)));
+	} else if (pbpctl_dev->bp_i80) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, ((ctrl_ext |
+						       BPCTLI_CTRL_EXT_MDIO_DIR80)
+						      &
+						      ~BPCTLI_CTRL_EXT_MDIO_DATA80));
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+				   ((ctrl | BPCTLI_CTRL_EXT_MCLK_DIR80) &
+				    ~BPCTLI_CTRL_EXT_MCLK_DATA80));
+
+	} else if (pbpctl_dev->bp_540) {
+		BP10G_WRITE_REG(pbpctl_dev, ESDP, ((ctrl | BP540_MCLK_DIR |
+						    BP540_MDIO_DIR) &
+						   ~(BP540_MDIO_DATA |
+						     BP540_MCLK_DATA)));
+
+	} else if (pbpctl_dev->bp_10gb) {
+		ctrl_ext = BP10GB_READ_REG(pbpctl_dev, MISC_REG_SPIO);
+		BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_SPIO,
+				 (ctrl_ext | BP10GB_MDIO_CLR | BP10GB_MCLK_CLR)
+				 & ~(BP10GB_MCLK_DIR | BP10GB_MDIO_DIR |
+				     BP10GB_MDIO_SET | BP10GB_MCLK_SET));
+
+	} else if (!pbpctl_dev->bp_10g)
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MCLK_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DATA)));
+	else {
+
+		BP10G_WRITE_REG(pbpctl_dev, EODSDP,
+				(ctrl_ext &
+				 ~(BP10G_MCLK_DATA_OUT | BP10G_MDIO_DATA_OUT)));
+	}
+	if ((pbpctl_dev->wdt_status == WDT_STATUS_EN)	/*&&
+							   (pbpctl_dev->bp_ext_ver<PXG4BPFI_VER) */ )
+		pbpctl_dev->bypass_wdt_on_time = jiffies;
+#ifdef BP_SYNC_FLAG
+	spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#endif
+	usec_delay(CMND_INTERVAL * 4);
+	return 0;
+}
+
+static void data_pulse(bpctl_dev_t *pbpctl_dev, unsigned char value)
+{
+
+	uint32_t ctrl_ext = 0;
+#ifdef BP_SYNC_FLAG
+	unsigned long flags;
+#endif
+	wdt_time_left(pbpctl_dev);
+#ifdef BP_SYNC_FLAG
+	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 1);
+#endif
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+						   BPCTLI_CTRL_EXT_SDP6_DIR |
+						   BPCTLI_CTRL_EXT_SDP7_DIR) &
+						  ~(BPCTLI_CTRL_EXT_SDP6_DATA |
+						    BPCTLI_CTRL_EXT_SDP7_DATA)));
+
+	usec_delay(INIT_CMND_INTERVAL);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+						   BPCTLI_CTRL_EXT_SDP6_DIR |
+						   BPCTLI_CTRL_EXT_SDP7_DIR |
+						   BPCTLI_CTRL_EXT_SDP6_DATA) &
+						  ~
+						  (BPCTLI_CTRL_EXT_SDP7_DATA)));
+	usec_delay(INIT_CMND_INTERVAL);
+
+	while (value) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ctrl_ext |
+				   BPCTLI_CTRL_EXT_SDP6_DIR |
+				   BPCTLI_CTRL_EXT_SDP7_DIR |
+				   BPCTLI_CTRL_EXT_SDP6_DATA |
+				   BPCTLI_CTRL_EXT_SDP7_DATA);
+		usec_delay(PULSE_INTERVAL);
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_SDP6_DIR
+							   |
+							   BPCTLI_CTRL_EXT_SDP7_DIR
+							   |
+							   BPCTLI_CTRL_EXT_SDP6_DATA)
+							  &
+							  ~BPCTLI_CTRL_EXT_SDP7_DATA));
+		usec_delay(PULSE_INTERVAL);
+		value--;
+
+	}
+	usec_delay(INIT_CMND_INTERVAL - PULSE_INTERVAL);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+						   BPCTLI_CTRL_EXT_SDP6_DIR |
+						   BPCTLI_CTRL_EXT_SDP7_DIR) &
+						  ~(BPCTLI_CTRL_EXT_SDP6_DATA |
+						    BPCTLI_CTRL_EXT_SDP7_DATA)));
+	usec_delay(WDT_TIME_CNT);
+	if (pbpctl_dev->wdt_status == WDT_STATUS_EN)
+		pbpctl_dev->bypass_wdt_on_time = jiffies;
+#ifdef BP_SYNC_FLAG
+	spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+	atomic_set(&pbpctl_dev->wdt_busy, 0);
+#endif
+
+}
+
+static int send_wdt_pulse(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+
+#ifdef BP_SYNC_FLAG
+	unsigned long flags;
+
+	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
+#else
+
+	if ((atomic_read(&pbpctl_dev->wdt_busy)) == 1)
+		return -1;
+#endif
+	wdt_time_left(pbpctl_dev);
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ctrl_ext |	/* 1 */
+			   BPCTLI_CTRL_EXT_SDP7_DIR |
+			   BPCTLI_CTRL_EXT_SDP7_DATA);
+	usec_delay(PULSE_INTERVAL);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |	/* 0 */
+						   BPCTLI_CTRL_EXT_SDP7_DIR) &
+						  ~BPCTLI_CTRL_EXT_SDP7_DATA));
+
+	usec_delay(PULSE_INTERVAL);
+	if (pbpctl_dev->wdt_status == WDT_STATUS_EN)
+		pbpctl_dev->bypass_wdt_on_time = jiffies;
+#ifdef BP_SYNC_FLAG
+	spin_unlock_irqrestore(&pbpctl_dev->bypass_wr_lock, flags);
+#endif
+
+	return 0;
+}
+
+void send_bypass_clear_pulse(bpctl_dev_t *pbpctl_dev, unsigned int value)
+{
+	uint32_t ctrl_ext = 0;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |	/* 0 */
+						   BPCTLI_CTRL_EXT_SDP6_DIR) &
+						  ~BPCTLI_CTRL_EXT_SDP6_DATA));
+
+	usec_delay(PULSE_INTERVAL);
+	while (value) {
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ctrl_ext |	/* 1 */
+				   BPCTLI_CTRL_EXT_SDP6_DIR |
+				   BPCTLI_CTRL_EXT_SDP6_DATA);
+		usec_delay(PULSE_INTERVAL);
+		value--;
+	}
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |	/* 0 */
+						   BPCTLI_CTRL_EXT_SDP6_DIR) &
+						  ~BPCTLI_CTRL_EXT_SDP6_DATA));
+	usec_delay(PULSE_INTERVAL);
+}
+
+/*  #endif  OLD_FW */
+#ifdef BYPASS_DEBUG
+
+int pulse_set_fn(bpctl_dev_t *pbpctl_dev, unsigned int counter)
+{
+	uint32_t ctrl_ext = 0;
+
+	if (!pbpctl_dev)
+		return -1;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	write_pulse_1(pbpctl_dev, ctrl_ext, counter, counter);
+
+	pbpctl_dev->bypass_wdt_status = 0;
+	if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+		write_pulse_1(pbpctl_dev, ctrl_ext, counter, counter);
+	} else {
+		wdt_time_left(pbpctl_dev);
+		if (pbpctl_dev->wdt_status == WDT_STATUS_EN) {
+			pbpctl_dev->wdt_status = 0;
+			data_pulse(pbpctl_dev, counter);
+			pbpctl_dev->wdt_status = WDT_STATUS_EN;
+			pbpctl_dev->bypass_wdt_on_time = jiffies;
+
+		} else
+			data_pulse(pbpctl_dev, counter);
+	}
+
+	return 0;
+}
+
+int zero_set_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0, ctrl_value = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+		printk("zero_set");
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+
+		BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+							   BPCTLI_CTRL_EXT_MCLK_DIR)
+							  &
+							  ~
+							  (BPCTLI_CTRL_EXT_MCLK_DATA
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DIR
+							   |
+							   BPCTLI_CTRL_EXT_MDIO_DATA)));
+
+	}
+	return ctrl_value;
+}
+
+int pulse_get2_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0, ctrl_value = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+		printk("pulse_get_fn\n");
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		ctrl_value = read_pulse_2(pbpctl_dev, ctrl_ext);
+		printk("read:%d\n", ctrl_value);
+	}
+	return ctrl_value;
+}
+
+int pulse_get1_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0, ctrl_value = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+
+		printk("pulse_get_fn\n");
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		ctrl_value = read_pulse_1(pbpctl_dev, ctrl_ext);
+		printk("read:%d\n", ctrl_value);
+	}
+	return ctrl_value;
+}
+
+int gpio6_set_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ctrl_ext |
+			   BPCTLI_CTRL_EXT_SDP6_DIR |
+			   BPCTLI_CTRL_EXT_SDP6_DATA);
+	return 0;
+}
+
+int gpio7_set_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ctrl_ext |
+			   BPCTLI_CTRL_EXT_SDP7_DIR |
+			   BPCTLI_CTRL_EXT_SDP7_DATA);
+	return 0;
+}
+
+int gpio7_clear_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+						   BPCTLI_CTRL_EXT_SDP7_DIR) &
+						  ~BPCTLI_CTRL_EXT_SDP7_DATA));
+	return 0;
+}
+
+int gpio6_clear_fn(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+
+	ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, ((ctrl_ext |
+						   BPCTLI_CTRL_EXT_SDP6_DIR) &
+						  ~BPCTLI_CTRL_EXT_SDP6_DATA));
+	return 0;
+}
+#endif				/*BYPASS_DEBUG */
+
+static bpctl_dev_t *get_status_port_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int idx_dev = 0;
+
+	if (pbpctl_dev == NULL)
+		return NULL;
+
+	if ((pbpctl_dev->func == 0) || (pbpctl_dev->func == 2)) {
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus)
+			    && (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot)
+			    && ((bpctl_dev_arr[idx_dev].func == 1)
+				&& (pbpctl_dev->func == 0))) {
+
+				return &(bpctl_dev_arr[idx_dev]);
+			}
+			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus) &&
+			    (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot) &&
+			    ((bpctl_dev_arr[idx_dev].func == 3)
+			     && (pbpctl_dev->func == 2))) {
+
+				return &(bpctl_dev_arr[idx_dev]);
+			}
+		}
+	}
+	return NULL;
+}
+
+static bpctl_dev_t *get_master_port_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int idx_dev = 0;
+
+	if (pbpctl_dev == NULL)
+		return NULL;
+
+	if ((pbpctl_dev->func == 1) || (pbpctl_dev->func == 3)) {
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus)
+			    && (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot)
+			    && ((bpctl_dev_arr[idx_dev].func == 0)
+				&& (pbpctl_dev->func == 1))) {
+
+				return &(bpctl_dev_arr[idx_dev]);
+			}
+			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus) &&
+			    (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot) &&
+			    ((bpctl_dev_arr[idx_dev].func == 2)
+			     && (pbpctl_dev->func == 3))) {
+
+				return &(bpctl_dev_arr[idx_dev]);
+			}
+		}
+	}
+	return NULL;
+}
+
+/**************************************/
+/**************INTEL API***************/
+/**************************************/
+
+static void write_data_port_int(bpctl_dev_t *pbpctl_dev,
+				unsigned char ctrl_value)
+{
+	uint32_t value;
+
+	value = BPCTL_READ_REG(pbpctl_dev, CTRL);
+/* Make SDP0 Pin Directonality to Output */
+	value |= BPCTLI_CTRL_SDP0_DIR;
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, value);
+
+	value &= ~BPCTLI_CTRL_SDP0_DATA;
+	value |= ((ctrl_value & 0x1) << BPCTLI_CTRL_SDP0_SHIFT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL, value);
+
+	value = (BPCTL_READ_REG(pbpctl_dev, CTRL_EXT));
+/* Make SDP2 Pin Directonality to Output */
+	value |= BPCTLI_CTRL_EXT_SDP6_DIR;
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, value);
+
+	value &= ~BPCTLI_CTRL_EXT_SDP6_DATA;
+	value |= (((ctrl_value & 0x2) >> 1) << BPCTLI_CTRL_EXT_SDP6_SHIFT);
+	BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT, value);
+
+}
+
+static int write_data_int(bpctl_dev_t *pbpctl_dev, unsigned char value)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+		return -1;
+	atomic_set(&pbpctl_dev->wdt_busy, 1);
+	write_data_port_int(pbpctl_dev, value & 0x3);
+	write_data_port_int(pbpctl_dev_b, ((value & 0xc) >> 2));
+	atomic_set(&pbpctl_dev->wdt_busy, 0);
+
+	return 0;
+}
+
+static int wdt_pulse_int(bpctl_dev_t *pbpctl_dev)
+{
+
+	if ((atomic_read(&pbpctl_dev->wdt_busy)) == 1)
+		return -1;
+
+	if ((write_data_int(pbpctl_dev, RESET_WDT_INT)) < 0)
+		return -1;
+	msec_delay_bp(CMND_INTERVAL_INT);
+	if ((write_data_int(pbpctl_dev, CMND_OFF_INT)) < 0)
+		return -1;
+	msec_delay_bp(CMND_INTERVAL_INT);
+
+	if (pbpctl_dev->wdt_status == WDT_STATUS_EN)
+		pbpctl_dev->bypass_wdt_on_time = jiffies;
+
+	return 0;
+}
+
+/*************************************/
+/************* COMMANDS **************/
+/*************************************/
+
+/* CMND_ON  0x4 (100)*/
+int cmnd_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+			return 0;
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER)
+			write_data(pbpctl_dev, CMND_ON);
+		else
+			data_pulse(pbpctl_dev, CMND_ON);
+		ret = 0;
+	}
+	return ret;
+}
+
+/* CMND_OFF  0x2 (10)*/
+int cmnd_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, CMND_OFF_INT);
+			msec_delay_bp(CMND_INTERVAL_INT);
+		} else if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER)
+			write_data(pbpctl_dev, CMND_OFF);
+		else
+			data_pulse(pbpctl_dev, CMND_OFF);
+		ret = 0;
+	};
+	return ret;
+}
+
+/* BYPASS_ON (0xa)*/
+int bypass_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & BP_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, BYPASS_ON_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			pbpctl_dev->bp_status_un = 0;
+		} else if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			write_data(pbpctl_dev, BYPASS_ON);
+			if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+				msec_delay_bp(LATCH_DELAY);
+		} else
+			data_pulse(pbpctl_dev, BYPASS_ON);
+		ret = 0;
+	};
+	return ret;
+}
+
+/* BYPASS_OFF (0x8 111)*/
+int bypass_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & BP_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, DIS_BYPASS_CAP_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			write_data_int(pbpctl_dev, PWROFF_BYPASS_ON_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			pbpctl_dev->bp_status_un = 0;
+		} else if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			write_data(pbpctl_dev, BYPASS_OFF);
+			if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+				msec_delay_bp(LATCH_DELAY);
+		} else
+			data_pulse(pbpctl_dev, BYPASS_OFF);
+		ret = 0;
+	}
+	return ret;
+}
+
+/* TAP_OFF (0x9)*/
+int tap_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	if ((pbpctl_dev->bp_caps & TAP_CAP)
+	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
+		write_data(pbpctl_dev, TAP_OFF);
+		msec_delay_bp(LATCH_DELAY);
+		ret = 0;
+	};
+	return ret;
+}
+
+/* TAP_ON (0xb)*/
+int tap_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	if ((pbpctl_dev->bp_caps & TAP_CAP)
+	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
+		write_data(pbpctl_dev, TAP_ON);
+		msec_delay_bp(LATCH_DELAY);
+		ret = 0;
+	};
+	return ret;
+}
+
+/* DISC_OFF (0x9)*/
+int disc_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
+		write_data(pbpctl_dev, DISC_OFF);
+		msec_delay_bp(LATCH_DELAY);
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+/* DISC_ON (0xb)*/
+int disc_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
+		write_data(pbpctl_dev, /*DISC_ON */ 0x85);
+		msec_delay_bp(LATCH_DELAY);
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+/* DISC_PORT_ON */
+int disc_port_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1) {
+
+			write_data(pbpctl_dev_m, TX_DISA);
+		} else {
+
+			write_data(pbpctl_dev_m, TX_DISB);
+		}
+
+		msec_delay_bp(LATCH_DELAY);
+
+	}
+	return ret;
+}
+
+/* DISC_PORT_OFF */
+int disc_port_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1)
+			write_data(pbpctl_dev_m, TX_ENA);
+		else
+			write_data(pbpctl_dev_m, TX_ENB);
+
+		msec_delay_bp(LATCH_DELAY);
+
+	}
+	return ret;
+}
+
+/*TWO_PORT_LINK_HW_EN (0xe)*/
+int tpl_hw_on(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0, ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps_ex & TPL2_CAP_EX) {
+		cmnd_on(pbpctl_dev);
+		write_data(pbpctl_dev, TPL2_ON);
+		msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		cmnd_off(pbpctl_dev);
+		return ret;
+	}
+
+	if (TPL_IF_SERIES(pbpctl_dev->subdevice)) {
+		ctrl = BPCTL_READ_REG(pbpctl_dev_b, CTRL);
+		BPCTL_BP_WRITE_REG(pbpctl_dev_b, CTRL,
+				   ((ctrl | BPCTLI_CTRL_SWDPIO0) &
+				    ~BPCTLI_CTRL_SWDPIN0));
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+/*TWO_PORT_LINK_HW_DIS (0xc)*/
+int tpl_hw_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0, ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+		return BP_NOT_CAP;
+	if (pbpctl_dev->bp_caps_ex & TPL2_CAP_EX) {
+		cmnd_on(pbpctl_dev);
+		write_data(pbpctl_dev, TPL2_OFF);
+		msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		cmnd_off(pbpctl_dev);
+		return ret;
+	}
+	if (TPL_IF_SERIES(pbpctl_dev->subdevice)) {
+		ctrl = BPCTL_READ_REG(pbpctl_dev_b, CTRL);
+		BPCTL_BP_WRITE_REG(pbpctl_dev_b, CTRL,
+				   (ctrl | BPCTLI_CTRL_SWDPIO0 |
+				    BPCTLI_CTRL_SWDPIN0));
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+/* WDT_OFF (0x6 110)*/
+int wdt_off(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			bypass_off(pbpctl_dev);
+		} else if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER)
+			write_data(pbpctl_dev, WDT_OFF);
+		else
+			data_pulse(pbpctl_dev, WDT_OFF);
+		pbpctl_dev->wdt_status = WDT_STATUS_DIS;
+		ret = 0;
+	};
+	return ret;
+}
+
+/* WDT_ON (0x10)*/
+
+/***Global***/
+static unsigned int
+    wdt_val_array[] = { 1000, 1500, 2000, 3000, 4000, 8000, 16000, 32000, 0 };
+
+int wdt_on(bpctl_dev_t *pbpctl_dev, unsigned int timeout)
+{
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		unsigned int pulse = 0, temp_value = 0, temp_cnt = 0;
+		pbpctl_dev->wdt_status = 0;
+
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			for (; wdt_val_array[temp_cnt]; temp_cnt++)
+				if (timeout <= wdt_val_array[temp_cnt])
+					break;
+
+			if (!wdt_val_array[temp_cnt])
+				temp_cnt--;
+
+			timeout = wdt_val_array[temp_cnt];
+			temp_cnt += 0x7;
+
+			write_data_int(pbpctl_dev, DIS_BYPASS_CAP_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			pbpctl_dev->bp_status_un = 0;
+			write_data_int(pbpctl_dev, temp_cnt);
+			pbpctl_dev->bypass_wdt_on_time = jiffies;
+			msec_delay_bp(CMND_INTERVAL_INT);
+			pbpctl_dev->bypass_timer_interval = timeout;
+		} else {
+			timeout =
+			    (timeout <
+			     TIMEOUT_UNIT ? TIMEOUT_UNIT : (timeout >
+							    WDT_TIMEOUT_MAX ?
+							    WDT_TIMEOUT_MAX :
+							    timeout));
+			temp_value = timeout / 100;
+			while ((temp_value >>= 1))
+				temp_cnt++;
+			if (timeout > ((1 << temp_cnt) * 100))
+				temp_cnt++;
+			pbpctl_dev->bypass_wdt_on_time = jiffies;
+			pulse = (WDT_ON | temp_cnt);
+			if (pbpctl_dev->bp_ext_ver == OLD_IF_VER)
+				data_pulse(pbpctl_dev, pulse);
+			else
+				write_data(pbpctl_dev, pulse);
+			pbpctl_dev->bypass_timer_interval =
+			    (1 << temp_cnt) * 100;
+		}
+		pbpctl_dev->wdt_status = WDT_STATUS_EN;
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+void bp75_put_hw_semaphore_generic(bpctl_dev_t *pbpctl_dev)
+{
+	u32 swsm;
+
+	swsm = BPCTL_READ_REG(pbpctl_dev, SWSM);
+
+	swsm &= ~(BPCTLI_SWSM_SMBI | BPCTLI_SWSM_SWESMBI);
+
+	BPCTL_WRITE_REG(pbpctl_dev, SWSM, swsm);
+}
+
+s32 bp75_get_hw_semaphore_generic(bpctl_dev_t *pbpctl_dev)
+{
+	u32 swsm;
+	s32 ret_val = 0;
+	s32 timeout = 8192 + 1;
+	s32 i = 0;
+
+	/* Get the SW semaphore */
+	while (i < timeout) {
+		swsm = BPCTL_READ_REG(pbpctl_dev, SWSM);
+		if (!(swsm & BPCTLI_SWSM_SMBI))
+			break;
+
+		usec_delay(50);
+		i++;
+	}
+
+	if (i == timeout) {
+		printk
+		    ("bpctl_mod: Driver can't access device - SMBI bit is set.\n");
+		ret_val = -1;
+		goto out;
+	}
+
+	/* Get the FW semaphore. */
+	for (i = 0; i < timeout; i++) {
+		swsm = BPCTL_READ_REG(pbpctl_dev, SWSM);
+		BPCTL_WRITE_REG(pbpctl_dev, SWSM, swsm | BPCTLI_SWSM_SWESMBI);
+
+		/* Semaphore acquired if bit latched */
+		if (BPCTL_READ_REG(pbpctl_dev, SWSM) & BPCTLI_SWSM_SWESMBI)
+			break;
+
+		usec_delay(50);
+	}
+
+	if (i == timeout) {
+		/* Release semaphores */
+		bp75_put_hw_semaphore_generic(pbpctl_dev);
+		printk("bpctl_mod: Driver can't access the NVM\n");
+		ret_val = -1;
+		goto out;
+	}
+
+ out:
+	return ret_val;
+}
+
+static void bp75_release_phy(bpctl_dev_t *pbpctl_dev)
+{
+	u16 mask = BPCTLI_SWFW_PHY0_SM;
+	u32 swfw_sync;
+
+	if ((pbpctl_dev->func == 1) || (pbpctl_dev->func == 3))
+		mask = BPCTLI_SWFW_PHY1_SM;
+
+	while (bp75_get_hw_semaphore_generic(pbpctl_dev) != 0) ;
+	/* Empty */
+
+	swfw_sync = BPCTL_READ_REG(pbpctl_dev, SW_FW_SYNC);
+	swfw_sync &= ~mask;
+	BPCTL_WRITE_REG(pbpctl_dev, SW_FW_SYNC, swfw_sync);
+
+	bp75_put_hw_semaphore_generic(pbpctl_dev);
+}
+
+static s32 bp75_acquire_phy(bpctl_dev_t *pbpctl_dev)
+{
+	u16 mask = BPCTLI_SWFW_PHY0_SM;
+	u32 swfw_sync;
+	u32 swmask;
+	u32 fwmask;
+	s32 ret_val = 0;
+	s32 i = 0, timeout = 200;
+
+	if ((pbpctl_dev->func == 1) || (pbpctl_dev->func == 3))
+		mask = BPCTLI_SWFW_PHY1_SM;
+
+	swmask = mask;
+	fwmask = mask << 16;
+
+	while (i < timeout) {
+		if (bp75_get_hw_semaphore_generic(pbpctl_dev)) {
+			ret_val = -1;
+			goto out;
+		}
+
+		swfw_sync = BPCTL_READ_REG(pbpctl_dev, SW_FW_SYNC);
+		if (!(swfw_sync & (fwmask | swmask)))
+			break;
+
+		bp75_put_hw_semaphore_generic(pbpctl_dev);
+		mdelay(5);
+		i++;
+	}
+
+	if (i == timeout) {
+		printk
+		    ("bpctl_mod: Driver can't access resource, SW_FW_SYNC timeout.\n");
+		ret_val = -1;
+		goto out;
+	}
+
+	swfw_sync |= swmask;
+	BPCTL_WRITE_REG(pbpctl_dev, SW_FW_SYNC, swfw_sync);
+
+	bp75_put_hw_semaphore_generic(pbpctl_dev);
+
+ out:
+	return ret_val;
+}
+
+s32 bp75_read_phy_reg_mdic(bpctl_dev_t *pbpctl_dev, u32 offset, u16 *data)
+{
+	u32 i, mdic = 0;
+	s32 ret_val = 0;
+	u32 phy_addr = 1;
+
+	mdic = ((offset << BPCTLI_MDIC_REG_SHIFT) |
+		(phy_addr << BPCTLI_MDIC_PHY_SHIFT) | (BPCTLI_MDIC_OP_READ));
+
+	BPCTL_WRITE_REG(pbpctl_dev, MDIC, mdic);
+
+	for (i = 0; i < (BPCTLI_GEN_POLL_TIMEOUT * 3); i++) {
+		usec_delay(50);
+		mdic = BPCTL_READ_REG(pbpctl_dev, MDIC);
+		if (mdic & BPCTLI_MDIC_READY)
+			break;
+	}
+	if (!(mdic & BPCTLI_MDIC_READY)) {
+		printk("bpctl_mod: MDI Read did not complete\n");
+		ret_val = -1;
+		goto out;
+	}
+	if (mdic & BPCTLI_MDIC_ERROR) {
+		printk("bpctl_mod: MDI Error\n");
+		ret_val = -1;
+		goto out;
+	}
+	*data = (u16) mdic;
+
+ out:
+	return ret_val;
+}
+
+s32 bp75_write_phy_reg_mdic(bpctl_dev_t *pbpctl_dev, u32 offset, u16 data)
+{
+	u32 i, mdic = 0;
+	s32 ret_val = 0;
+	u32 phy_addr = 1;
+
+	mdic = (((u32) data) |
+		(offset << BPCTLI_MDIC_REG_SHIFT) |
+		(phy_addr << BPCTLI_MDIC_PHY_SHIFT) | (BPCTLI_MDIC_OP_WRITE));
+
+	BPCTL_WRITE_REG(pbpctl_dev, MDIC, mdic);
+
+	for (i = 0; i < (BPCTLI_GEN_POLL_TIMEOUT * 3); i++) {
+		usec_delay(50);
+		mdic = BPCTL_READ_REG(pbpctl_dev, MDIC);
+		if (mdic & BPCTLI_MDIC_READY)
+			break;
+	}
+	if (!(mdic & BPCTLI_MDIC_READY)) {
+		printk("bpctl_mod: MDI Write did not complete\n");
+		ret_val = -1;
+		goto out;
+	}
+	if (mdic & BPCTLI_MDIC_ERROR) {
+		printk("bpctl_mod: MDI Error\n");
+		ret_val = -1;
+		goto out;
+	}
+
+ out:
+	return ret_val;
+}
+
+static s32 bp75_read_phy_reg(bpctl_dev_t *pbpctl_dev, u32 offset, u16 *data)
+{
+	s32 ret_val = 0;
+
+	ret_val = bp75_acquire_phy(pbpctl_dev);
+	if (ret_val)
+		goto out;
+
+	if (offset > BPCTLI_MAX_PHY_MULTI_PAGE_REG) {
+		ret_val = bp75_write_phy_reg_mdic(pbpctl_dev,
+						  BPCTLI_IGP01E1000_PHY_PAGE_SELECT,
+						  (u16) offset);
+		if (ret_val)
+			goto release;
+	}
+
+	ret_val =
+	    bp75_read_phy_reg_mdic(pbpctl_dev,
+				   BPCTLI_MAX_PHY_REG_ADDRESS & offset, data);
+
+ release:
+	bp75_release_phy(pbpctl_dev);
+ out:
+	return ret_val;
+}
+
+static s32 bp75_write_phy_reg(bpctl_dev_t *pbpctl_dev, u32 offset, u16 data)
+{
+	s32 ret_val = 0;
+
+	ret_val = bp75_acquire_phy(pbpctl_dev);
+	if (ret_val)
+		goto out;
+
+	if (offset > BPCTLI_MAX_PHY_MULTI_PAGE_REG) {
+		ret_val = bp75_write_phy_reg_mdic(pbpctl_dev,
+						  BPCTLI_IGP01E1000_PHY_PAGE_SELECT,
+						  (u16) offset);
+		if (ret_val)
+			goto release;
+	}
+
+	ret_val =
+	    bp75_write_phy_reg_mdic(pbpctl_dev,
+				    BPCTLI_MAX_PHY_REG_ADDRESS & offset, data);
+
+ release:
+	bp75_release_phy(pbpctl_dev);
+
+ out:
+	return ret_val;
+}
+
+/* SET_TX  (non-Bypass command :)) */
+static int set_tx(bpctl_dev_t *pbpctl_dev, int tx_state)
+{
+	int ret = 0, ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		if (!tx_state) {
+			if (pbpctl_dev->bp_540) {
+				ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						(ctrl | BP10G_SDP1_DIR |
+						 BP10G_SDP1_DATA));
+
+			} else {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   (ctrl | BPCTLI_CTRL_SDP1_DIR
+						    | BPCTLI_CTRL_SWDPIN1));
+			}
+		} else {
+			if (pbpctl_dev->bp_540) {
+				ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP10G_SDP1_DIR) &
+						 ~BP10G_SDP1_DATA));
+			} else {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl |
+						     BPCTLI_CTRL_SDP1_DIR) &
+						    ~BPCTLI_CTRL_SWDPIN1));
+			}
+			return ret;
+
+		}
+	} else if (pbpctl_dev->bp_caps & TX_CTL_CAP) {
+		if (PEG5_IF_SERIES(pbpctl_dev->subdevice)) {
+			if (tx_state) {
+				uint16_t mii_reg;
+				if (!
+				    (ret =
+				     bp75_read_phy_reg(pbpctl_dev,
+						       BPCTLI_PHY_CONTROL,
+						       &mii_reg))) {
+					if (mii_reg & BPCTLI_MII_CR_POWER_DOWN) {
+						ret =
+						    bp75_write_phy_reg
+						    (pbpctl_dev,
+						     BPCTLI_PHY_CONTROL,
+						     mii_reg &
+						     ~BPCTLI_MII_CR_POWER_DOWN);
+					}
+				}
+			} else {
+				uint16_t mii_reg;
+				if (!
+				    (ret =
+				     bp75_read_phy_reg(pbpctl_dev,
+						       BPCTLI_PHY_CONTROL,
+						       &mii_reg))) {
+
+					mii_reg |= BPCTLI_MII_CR_POWER_DOWN;
+					ret =
+					    bp75_write_phy_reg(pbpctl_dev,
+							       BPCTLI_PHY_CONTROL,
+							       mii_reg);
+				}
+			}
+
+		}
+		if (pbpctl_dev->bp_fiber5) {
+			ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+
+		} else if (pbpctl_dev->bp_10gb)
+			ctrl = BP10GB_READ_REG(pbpctl_dev, MISC_REG_GPIO);
+
+		else if (!pbpctl_dev->bp_10g)
+			ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		else
+			ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+
+		if (!tx_state)
+			if (pbpctl_dev->bp_10g9) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						(ctrl | BP10G_SDP3_DATA |
+						 BP10G_SDP3_DIR));
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   (ctrl |
+						    BPCTLI_CTRL_EXT_SDP6_DIR |
+						    BPCTLI_CTRL_EXT_SDP6_DATA));
+
+			} else if (pbpctl_dev->bp_10gb) {
+				if ((pbpctl_dev->func == 1)
+				    || (pbpctl_dev->func == 3))
+					BP10GB_WRITE_REG(pbpctl_dev,
+							 MISC_REG_GPIO,
+							 (ctrl |
+							  BP10GB_GPIO0_SET_P1) &
+							 ~(BP10GB_GPIO0_CLR_P1 |
+							   BP10GB_GPIO0_OE_P1));
+				else
+					BP10GB_WRITE_REG(pbpctl_dev,
+							 MISC_REG_GPIO,
+							 (ctrl |
+							  BP10GB_GPIO0_OE_P0 |
+							  BP10GB_GPIO0_SET_P0));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   (ctrl | BPCTLI_CTRL_SDP1_DIR
+						    | BPCTLI_CTRL_SWDPIN1));
+
+			} else if (pbpctl_dev->bp_540) {
+				ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						(ctrl | BP10G_SDP1_DIR |
+						 BP10G_SDP1_DATA));
+
+			}
+
+			else if (!pbpctl_dev->bp_10g)
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   (ctrl | BPCTLI_CTRL_SWDPIO0 |
+						    BPCTLI_CTRL_SWDPIN0));
+
+			else
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						(ctrl | BP10G_SDP0_DATA |
+						 BP10G_SDP0_DIR));
+
+		else {
+			if (pbpctl_dev->bp_10g9) {
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP10G_SDP3_DIR) &
+						 ~BP10G_SDP3_DATA));
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL_EXT,
+						   ((ctrl |
+						     BPCTLI_CTRL_EXT_SDP6_DIR) &
+						    ~BPCTLI_CTRL_EXT_SDP6_DATA));
+
+			} else if (pbpctl_dev->bp_10gb) {
+				if ((bpctl_dev_arr->func == 1)
+				    || (bpctl_dev_arr->func == 3))
+					BP10GB_WRITE_REG(pbpctl_dev,
+							 MISC_REG_GPIO,
+							 (ctrl |
+							  BP10GB_GPIO0_CLR_P1) &
+							 ~(BP10GB_GPIO0_SET_P1 |
+							   BP10GB_GPIO0_OE_P1));
+				else
+					BP10GB_WRITE_REG(pbpctl_dev,
+							 MISC_REG_GPIO,
+							 (ctrl |
+							  BP10GB_GPIO0_OE_P0 |
+							  BP10GB_GPIO0_CLR_P0));
+
+			} else if (pbpctl_dev->bp_i80) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl |
+						     BPCTLI_CTRL_SDP1_DIR) &
+						    ~BPCTLI_CTRL_SWDPIN1));
+			} else if (pbpctl_dev->bp_540) {
+				ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP10G_SDP1_DIR) &
+						 ~BP10G_SDP1_DATA));
+			}
+
+			else if (!pbpctl_dev->bp_10g) {
+				BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+						   ((ctrl | BPCTLI_CTRL_SWDPIO0)
+						    & ~BPCTLI_CTRL_SWDPIN0));
+				if (!PEGF_IF_SERIES(pbpctl_dev->subdevice)) {
+					BPCTL_BP_WRITE_REG(pbpctl_dev, CTRL,
+							   (ctrl &
+							    ~
+							    (BPCTLI_CTRL_SDP0_DATA
+							     |
+							     BPCTLI_CTRL_SDP0_DIR)));
+				}
+			} else
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP10G_SDP0_DIR) &
+						 ~BP10G_SDP0_DATA));
+
+		}
+
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+
+}
+
+/* SET_FORCE_LINK  (non-Bypass command :)) */
+static int set_bp_force_link(bpctl_dev_t *pbpctl_dev, int tx_state)
+{
+	int ret = 0, ctrl = 0;
+
+	if (DBI_IF_SERIES(pbpctl_dev->subdevice)) {
+
+		if ((pbpctl_dev->bp_10g) || (pbpctl_dev->bp_10g9)) {
+
+			ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL);
+			if (!tx_state)
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						ctrl & ~BP10G_SDP1_DIR);
+			else
+				BP10G_WRITE_REG(pbpctl_dev, ESDP,
+						((ctrl | BP10G_SDP1_DIR) &
+						 ~BP10G_SDP1_DATA));
+			return ret;
+		}
+
+	}
+	return BP_NOT_CAP;
+}
+
+/*RESET_CONT 0x20 */
+int reset_cont(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+			return BP_NOT_CAP;
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER)
+			write_data(pbpctl_dev, RESET_CONT);
+		else
+			data_pulse(pbpctl_dev, RESET_CONT);
+		ret = 0;
+	};
+	return ret;
+}
+
+/*DIS_BYPASS_CAP 0x22 */
+int dis_bypass_cap(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & BP_DIS_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, DIS_BYPASS_CAP_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+		} else {
+			write_data(pbpctl_dev, BYPASS_OFF);
+			msec_delay_bp(LATCH_DELAY);
+			write_data(pbpctl_dev, DIS_BYPASS_CAP);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+		}
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/*EN_BYPASS_CAP 0x24 */
+int en_bypass_cap(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & BP_DIS_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, PWROFF_BYPASS_ON_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+		} else {
+			write_data(pbpctl_dev, EN_BYPASS_CAP);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+		}
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/* BYPASS_STATE_PWRON 0x26*/
+int bypass_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & BP_PWUP_CTL_CAP) {
+		write_data(pbpctl_dev, BYPASS_STATE_PWRON);
+		if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+			msec_delay_bp(DFLT_PWRON_DELAY);
+		else
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/* NORMAL_STATE_PWRON 0x28*/
+int normal_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	if ((pbpctl_dev->bp_caps & BP_PWUP_CTL_CAP)
+	    || (pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP)) {
+		write_data(pbpctl_dev, NORMAL_STATE_PWRON);
+		if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+			msec_delay_bp(DFLT_PWRON_DELAY);
+		else
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/* BYPASS_STATE_PWROFF 0x27*/
+int bypass_state_pwroff(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & BP_PWOFF_CTL_CAP) {
+		write_data(pbpctl_dev, BYPASS_STATE_PWROFF);
+		msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/* NORMAL_STATE_PWROFF 0x29*/
+int normal_state_pwroff(bpctl_dev_t *pbpctl_dev)
+{
+	if ((pbpctl_dev->bp_caps & BP_PWOFF_CTL_CAP)) {
+		write_data(pbpctl_dev, NORMAL_STATE_PWROFF);
+		msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/*TAP_STATE_PWRON 0x2a*/
+int tap_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP) {
+		write_data(pbpctl_dev, TAP_STATE_PWRON);
+		msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/*DIS_TAP_CAP 0x2c*/
+int dis_tap_cap(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_DIS_CAP) {
+		write_data(pbpctl_dev, DIS_TAP_CAP);
+		msec_delay_bp(BYPASS_CAP_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/*EN_TAP_CAP 0x2e*/
+int en_tap_cap(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_DIS_CAP) {
+		write_data(pbpctl_dev, EN_TAP_CAP);
+		msec_delay_bp(BYPASS_CAP_DELAY);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+/*DISC_STATE_PWRON 0x2a*/
+int disc_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & DISC_PWUP_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			write_data(pbpctl_dev, DISC_STATE_PWRON);
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/*DIS_DISC_CAP 0x2c*/
+int dis_disc_cap(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & DISC_DIS_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			write_data(pbpctl_dev, DIS_DISC_CAP);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/*DISC_STATE_PWRON 0x2a*/
+int disc_port_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+
+	return BP_NOT_CAP;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1)
+			write_data(pbpctl_dev_m, TX_DISA_PWRUP);
+		else
+			write_data(pbpctl_dev_m, TX_DISB_PWRUP);
+
+		msec_delay_bp(LATCH_DELAY);
+
+	}
+	return ret;
+}
+
+int normal_port_state_pwron(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+	return BP_NOT_CAP;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1)
+			write_data(pbpctl_dev_m, TX_ENA_PWRUP);
+		else
+			write_data(pbpctl_dev_m, TX_ENB_PWRUP);
+
+		msec_delay_bp(LATCH_DELAY);
+
+	}
+	return ret;
+}
+
+/*EN_TAP_CAP 0x2e*/
+int en_disc_cap(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & DISC_DIS_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			write_data(pbpctl_dev, EN_DISC_CAP);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int std_nic_on(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & STD_NIC_CAP) {
+
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, DIS_BYPASS_CAP_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			pbpctl_dev->bp_status_un = 0;
+			return BP_OK;
+		}
+
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			write_data(pbpctl_dev, STD_NIC_ON);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+			return BP_OK;
+
+		}
+
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			wdt_off(pbpctl_dev);
+
+			if (pbpctl_dev->bp_caps & BP_CAP) {
+				write_data(pbpctl_dev, BYPASS_OFF);
+				msec_delay_bp(LATCH_DELAY);
+			}
+
+			if (pbpctl_dev->bp_caps & TAP_CAP) {
+				write_data(pbpctl_dev, TAP_OFF);
+				msec_delay_bp(LATCH_DELAY);
+			}
+
+			write_data(pbpctl_dev, NORMAL_STATE_PWRON);
+			if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+				msec_delay_bp(DFLT_PWRON_DELAY);
+			else
+				msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+
+			if (pbpctl_dev->bp_caps & BP_DIS_CAP) {
+				write_data(pbpctl_dev, DIS_BYPASS_CAP);
+				msec_delay_bp(BYPASS_CAP_DELAY);
+			}
+
+			if (pbpctl_dev->bp_caps & TAP_DIS_CAP) {
+				write_data(pbpctl_dev, DIS_TAP_CAP);
+				msec_delay_bp(BYPASS_CAP_DELAY);
+
+			}
+			return 0;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int std_nic_off(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & STD_NIC_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			write_data_int(pbpctl_dev, PWROFF_BYPASS_ON_INT);
+			msec_delay_bp(BYPASS_DELAY_INT);
+			return BP_OK;
+		}
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			write_data(pbpctl_dev, STD_NIC_OFF);
+			msec_delay_bp(BYPASS_CAP_DELAY);
+			return BP_OK;
+
+		}
+
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+
+			if (pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP) {
+				write_data(pbpctl_dev, TAP_STATE_PWRON);
+				msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+			}
+
+			if (pbpctl_dev->bp_caps & BP_PWUP_CTL_CAP) {
+				write_data(pbpctl_dev, BYPASS_STATE_PWRON);
+				if (pbpctl_dev->bp_ext_ver > PXG2BPI_VER)
+					msec_delay_bp(LATCH_DELAY +
+						      EEPROM_WR_DELAY);
+				else
+					msec_delay_bp(DFLT_PWRON_DELAY);
+			}
+
+			if (pbpctl_dev->bp_caps & TAP_DIS_CAP) {
+				write_data(pbpctl_dev, EN_TAP_CAP);
+				msec_delay_bp(BYPASS_CAP_DELAY);
+			}
+			if (pbpctl_dev->bp_caps & DISC_DIS_CAP) {
+				write_data(pbpctl_dev, EN_DISC_CAP);
+				msec_delay_bp(BYPASS_CAP_DELAY);
+			}
+
+			if (pbpctl_dev->bp_caps & BP_DIS_CAP) {
+				write_data(pbpctl_dev, EN_BYPASS_CAP);
+				msec_delay_bp(BYPASS_CAP_DELAY);
+			}
+
+			return 0;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int wdt_time_left(bpctl_dev_t *pbpctl_dev)
+{
+
+	/* unsigned long curr_time=((long long)(jiffies*1000))/HZ, delta_time=0,wdt_on_time=((long long)(pbpctl_dev->bypass_wdt_on_time*1000))/HZ; */
+	unsigned long curr_time = jiffies, delta_time = 0, wdt_on_time =
+	    pbpctl_dev->bypass_wdt_on_time, delta_time_msec = 0;
+	int time_left = 0;
+
+	switch (pbpctl_dev->wdt_status) {
+	case WDT_STATUS_DIS:
+		time_left = 0;
+		break;
+	case WDT_STATUS_EN:
+		delta_time =
+		    (curr_time >=
+		     wdt_on_time) ? (curr_time - wdt_on_time) : (~wdt_on_time +
+								 curr_time);
+		delta_time_msec = jiffies_to_msecs(delta_time);
+		time_left = pbpctl_dev->bypass_timer_interval - delta_time_msec;
+		if (time_left < 0) {
+			time_left = -1;
+			pbpctl_dev->wdt_status = WDT_STATUS_EXP;
+		}
+		break;
+	case WDT_STATUS_EXP:
+		time_left = -1;
+		break;
+	}
+
+	return time_left;
+}
+
+static int wdt_timer(bpctl_dev_t *pbpctl_dev, int *time_left)
+{
+	int ret = 0;
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		{
+			if (pbpctl_dev->wdt_status == WDT_STATUS_UNKNOWN)
+				ret = BP_NOT_CAP;
+			else
+				*time_left = wdt_time_left(pbpctl_dev);
+		}
+
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+static int wdt_timer_reload(bpctl_dev_t *pbpctl_dev)
+{
+
+	int ret = 0;
+
+	if ((pbpctl_dev->bp_caps & WD_CTL_CAP) &&
+	    (pbpctl_dev->wdt_status != WDT_STATUS_UNKNOWN)) {
+		if (pbpctl_dev->wdt_status == WDT_STATUS_DIS)
+			return 0;
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER)
+			ret = wdt_pulse(pbpctl_dev);
+		else if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+			ret = wdt_pulse_int(pbpctl_dev);
+		else
+			ret = send_wdt_pulse(pbpctl_dev);
+		/* if (ret==-1)
+		    mod_timer(&pbpctl_dev->bp_timer, jiffies+1);*/
+		return 1;
+	}
+	return BP_NOT_CAP;
+}
+
+static void wd_reset_timer(unsigned long param)
+{
+	bpctl_dev_t *pbpctl_dev = (bpctl_dev_t *) param;
+#ifdef BP_SELF_TEST
+	struct sk_buff *skb_tmp;
+#endif
+
+	if ((pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) &&
+	    ((atomic_read(&pbpctl_dev->wdt_busy)) == 1)) {
+		mod_timer(&pbpctl_dev->bp_timer, jiffies + 1);
+		return;
+	}
+#ifdef BP_SELF_TEST
+
+	if (pbpctl_dev->bp_self_test_flag == 1) {
+		skb_tmp = dev_alloc_skb(BPTEST_DATA_LEN + 2);
+		if ((skb_tmp) && (pbpctl_dev->ndev) && (pbpctl_dev->bp_tx_data)) {
+			memcpy(skb_put(skb_tmp, BPTEST_DATA_LEN),
+			       pbpctl_dev->bp_tx_data, BPTEST_DATA_LEN);
+			skb_tmp->dev = pbpctl_dev->ndev;
+			skb_tmp->protocol =
+			    eth_type_trans(skb_tmp, pbpctl_dev->ndev);
+			skb_tmp->ip_summed = CHECKSUM_UNNECESSARY;
+			netif_receive_skb(skb_tmp);
+			goto bp_timer_reload;
+			return;
+		}
+	}
+#endif
+
+	wdt_timer_reload(pbpctl_dev);
+#ifdef BP_SELF_TEST
+ bp_timer_reload:
+#endif
+	if (pbpctl_dev->reset_time) {
+		mod_timer(&pbpctl_dev->bp_timer,
+			  jiffies + (HZ * pbpctl_dev->reset_time) / 1000);
+	}
+}
+
+/*WAIT_AT_PWRUP 0x80   */
+int bp_wait_at_pwup_en(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+			write_data(pbpctl_dev, BP_WAIT_AT_PWUP_EN);
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/*DIS_WAIT_AT_PWRUP       0x81 */
+int bp_wait_at_pwup_dis(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+
+		if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+			write_data(pbpctl_dev, BP_WAIT_AT_PWUP_DIS);
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/*EN_HW_RESET  0x82   */
+
+int bp_hw_reset_en(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+			write_data(pbpctl_dev, BP_HW_RESET_EN);
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/*DIS_HW_RESET             0x83   */
+
+int bp_hw_reset_dis(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+			write_data(pbpctl_dev, BP_HW_RESET_DIS);
+			msec_delay_bp(LATCH_DELAY + EEPROM_WR_DELAY);
+
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+
+int wdt_exp_mode(bpctl_dev_t *pbpctl_dev, int mode)
+{
+	uint32_t status_reg = 0, status_reg1 = 0;
+
+	if ((pbpctl_dev->bp_caps & (TAP_STATUS_CAP | DISC_CAP)) &&
+	    (pbpctl_dev->bp_caps & BP_CAP)) {
+		if (pbpctl_dev->bp_ext_ver >= PXE2TBPI_VER) {
+
+			if ((pbpctl_dev->bp_ext_ver >= 0x8) &&
+			    (mode == 2) && (pbpctl_dev->bp_caps & DISC_CAP)) {
+				status_reg1 =
+				    read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR);
+				if (!(status_reg1 & WDTE_DISC_BPN_MASK))
+					write_reg(pbpctl_dev,
+						  status_reg1 |
+						  WDTE_DISC_BPN_MASK,
+						  STATUS_DISC_REG_ADDR);
+				return BP_OK;
+			}
+		}
+		status_reg = read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
+
+		if ((mode == 0) && (pbpctl_dev->bp_caps & BP_CAP)) {
+			if (pbpctl_dev->bp_ext_ver >= 0x8) {
+				status_reg1 =
+				    read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR);
+				if (status_reg1 & WDTE_DISC_BPN_MASK)
+					write_reg(pbpctl_dev,
+						  status_reg1 &
+						  ~WDTE_DISC_BPN_MASK,
+						  STATUS_DISC_REG_ADDR);
+			}
+			if (status_reg & WDTE_TAP_BPN_MASK)
+				write_reg(pbpctl_dev,
+					  status_reg & ~WDTE_TAP_BPN_MASK,
+					  STATUS_TAP_REG_ADDR);
+			return BP_OK;
+
+		} else if ((mode == 1) && (pbpctl_dev->bp_caps & TAP_CAP)) {
+			if (!(status_reg & WDTE_TAP_BPN_MASK))
+				write_reg(pbpctl_dev,
+					  status_reg | WDTE_TAP_BPN_MASK,
+					  STATUS_TAP_REG_ADDR);
+			/*else return BP_NOT_CAP; */
+			return BP_OK;
+		}
+
+	}
+	return BP_NOT_CAP;
+}
+
+int bypass_fw_ver(bpctl_dev_t *pbpctl_dev)
+{
+	if (is_bypass_fn(pbpctl_dev))
+		return read_reg(pbpctl_dev, VER_REG_ADDR);
+	else
+		return BP_NOT_CAP;
+}
+
+int bypass_sign_check(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (is_bypass_fn(pbpctl_dev))
+		return (((read_reg(pbpctl_dev, PIC_SIGN_REG_ADDR)) ==
+			 PIC_SIGN_VALUE) ? 1 : 0);
+	else
+		return BP_NOT_CAP;
+}
+
+static int tx_status(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl = 0;
+	bpctl_dev_t *pbpctl_dev_m;
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+
+		ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL);
+		if (pbpctl_dev->bp_i80)
+			return ((ctrl & BPCTLI_CTRL_SWDPIN1) != 0 ? 0 : 1);
+		if (pbpctl_dev->bp_540) {
+			ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+
+			return ((ctrl & BP10G_SDP1_DATA) != 0 ? 0 : 1);
+		}
+
+	}
+
+	if (pbpctl_dev->bp_caps & TX_CTL_CAP) {
+		if (PEG5_IF_SERIES(pbpctl_dev->subdevice)) {
+			uint16_t mii_reg;
+			if (!
+			    (bp75_read_phy_reg
+			     (pbpctl_dev, BPCTLI_PHY_CONTROL, &mii_reg))) {
+				if (mii_reg & BPCTLI_MII_CR_POWER_DOWN)
+					return 0;
+
+				else
+					return 1;
+			}
+			return -1;
+		}
+
+		if (pbpctl_dev->bp_10g9) {
+			return ((BP10G_READ_REG(pbpctl_dev, ESDP) &
+				 BP10G_SDP3_DATA) != 0 ? 0 : 1);
+
+		} else if (pbpctl_dev->bp_fiber5) {
+			ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+			if (ctrl & BPCTLI_CTRL_EXT_SDP6_DATA)
+				return 0;
+			return 1;
+		} else if (pbpctl_dev->bp_10gb) {
+			ctrl = BP10GB_READ_REG(pbpctl_dev, MISC_REG_GPIO);
+			BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_GPIO,
+					 (ctrl | BP10GB_GPIO0_OE_P1) &
+					 ~(BP10GB_GPIO0_SET_P1 |
+					   BP10GB_GPIO0_CLR_P1));
+
+			if ((pbpctl_dev->func == 1) || (pbpctl_dev->func == 3))
+				return (((BP10GB_READ_REG
+					  (pbpctl_dev,
+					   MISC_REG_GPIO)) & BP10GB_GPIO0_P1) !=
+					0 ? 0 : 1);
+			else
+				return (((BP10GB_READ_REG
+					  (pbpctl_dev,
+					   MISC_REG_GPIO)) & BP10GB_GPIO0_P0) !=
+					0 ? 0 : 1);
+		}
+
+		if (!pbpctl_dev->bp_10g) {
+
+			ctrl = BPCTL_READ_REG(pbpctl_dev, CTRL);
+			if (pbpctl_dev->bp_i80)
+				return ((ctrl & BPCTLI_CTRL_SWDPIN1) !=
+					0 ? 0 : 1);
+			if (pbpctl_dev->bp_540) {
+				ctrl = BP10G_READ_REG(pbpctl_dev, ESDP);
+
+				return ((ctrl & BP10G_SDP1_DATA) != 0 ? 0 : 1);
+			}
+
+			return ((ctrl & BPCTLI_CTRL_SWDPIN0) != 0 ? 0 : 1);
+		} else
+			return ((BP10G_READ_REG(pbpctl_dev, ESDP) &
+				 BP10G_SDP0_DATA) != 0 ? 0 : 1);
+
+	}
+	return BP_NOT_CAP;
+}
+
+static int bp_force_link_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (DBI_IF_SERIES(pbpctl_dev->subdevice)) {
+
+		if ((pbpctl_dev->bp_10g) || (pbpctl_dev->bp_10g9)) {
+			return ((BP10G_READ_REG(pbpctl_dev, ESDP) &
+				 BP10G_SDP1_DIR) != 0 ? 1 : 0);
+
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int bypass_from_last_read(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t ctrl_ext = 0;
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if ((pbpctl_dev->bp_caps & SW_CTL_CAP)
+	    && (pbpctl_dev_b = get_status_port_fn(pbpctl_dev))) {
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev_b, CTRL_EXT);
+		BPCTL_BP_WRITE_REG(pbpctl_dev_b, CTRL_EXT,
+				   (ctrl_ext & ~BPCTLI_CTRL_EXT_SDP7_DIR));
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev_b, CTRL_EXT);
+		if (ctrl_ext & BPCTLI_CTRL_EXT_SDP7_DATA)
+			return 0;
+		return 1;
+	} else
+		return BP_NOT_CAP;
+}
+
+int bypass_status_clear(bpctl_dev_t *pbpctl_dev)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if ((pbpctl_dev->bp_caps & SW_CTL_CAP)
+	    && (pbpctl_dev_b = get_status_port_fn(pbpctl_dev))) {
+
+		send_bypass_clear_pulse(pbpctl_dev_b, 1);
+		return 0;
+	} else
+		return BP_NOT_CAP;
+}
+
+int bypass_flag_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if ((pbpctl_dev->bp_caps & BP_CAP)) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+				  BYPASS_FLAG_MASK) ==
+				 BYPASS_FLAG_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int bypass_flag_status_clear(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & BP_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			uint32_t status_reg = 0;
+			status_reg = read_reg(pbpctl_dev, STATUS_REG_ADDR);
+			write_reg(pbpctl_dev, status_reg & ~BYPASS_FLAG_MASK,
+				  STATUS_REG_ADDR);
+			return 0;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int bypass_change_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & BP_STATUS_CHANGE_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			ret = bypass_flag_status(pbpctl_dev);
+			bypass_flag_status_clear(pbpctl_dev);
+		} else if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			ret = bypass_flag_status(pbpctl_dev);
+			bypass_flag_status_clear(pbpctl_dev);
+		} else {
+			ret = bypass_from_last_read(pbpctl_dev);
+			bypass_status_clear(pbpctl_dev);
+		}
+	}
+	return ret;
+}
+
+int bypass_off_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & BP_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+				  BYPASS_OFF_MASK) == BYPASS_OFF_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+static int bypass_status(bpctl_dev_t *pbpctl_dev)
+{
+	u32 ctrl_ext = 0;
+	if (pbpctl_dev->bp_caps & BP_CAP) {
+
+		bpctl_dev_t *pbpctl_dev_b = NULL;
+
+		if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+			return BP_NOT_CAP;
+
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+
+			if (!pbpctl_dev->bp_status_un)
+				return (((BPCTL_READ_REG
+					  (pbpctl_dev_b,
+					   CTRL_EXT)) &
+					 BPCTLI_CTRL_EXT_SDP7_DATA) !=
+					0 ? 1 : 0);
+			else
+				return BP_NOT_CAP;
+		}
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+
+			if (pbpctl_dev->bp_10g9) {
+				ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, I2CCTL);
+				BP10G_WRITE_REG(pbpctl_dev_b, I2CCTL,
+						(ctrl_ext | BP10G_I2C_CLK_OUT));
+				return ((BP10G_READ_REG(pbpctl_dev_b, I2CCTL) &
+					 BP10G_I2C_CLK_IN) != 0 ? 0 : 1);
+
+			} else if (pbpctl_dev->bp_540) {
+				return (((BP10G_READ_REG(pbpctl_dev_b, ESDP)) &
+					 BP10G_SDP0_DATA) != 0 ? 0 : 1);
+			}
+
+			else if ((pbpctl_dev->bp_fiber5)
+				 || (pbpctl_dev->bp_i80)) {
+				return (((BPCTL_READ_REG(pbpctl_dev_b, CTRL)) &
+					 BPCTLI_CTRL_SWDPIN0) != 0 ? 0 : 1);
+			} else if (pbpctl_dev->bp_10gb) {
+				ctrl_ext =
+				    BP10GB_READ_REG(pbpctl_dev, MISC_REG_GPIO);
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_GPIO,
+						 (ctrl_ext | BP10GB_GPIO3_OE_P0)
+						 & ~(BP10GB_GPIO3_SET_P0 |
+						     BP10GB_GPIO3_CLR_P0));
+
+				return (((BP10GB_READ_REG
+					  (pbpctl_dev,
+					   MISC_REG_GPIO)) & BP10GB_GPIO3_P0) !=
+					0 ? 0 : 1);
+			}
+
+			else if (!pbpctl_dev->bp_10g)
+				return (((BPCTL_READ_REG
+					  (pbpctl_dev_b,
+					   CTRL_EXT)) &
+					 BPCTLI_CTRL_EXT_SDP7_DATA) !=
+					0 ? 0 : 1);
+
+			else {
+				ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, EODSDP);
+				BP10G_WRITE_REG(pbpctl_dev_b, EODSDP,
+						(ctrl_ext |
+						 BP10G_SDP7_DATA_OUT));
+				return ((BP10G_READ_REG(pbpctl_dev_b, EODSDP) &
+					 BP10G_SDP7_DATA_IN) != 0 ? 0 : 1);
+			}
+
+		} else if (pbpctl_dev->media_type == bp_copper) {
+
+			return (((BPCTL_READ_REG(pbpctl_dev_b, CTRL)) &
+				 BPCTLI_CTRL_SWDPIN1) != 0 ? 1 : 0);
+		} else {
+			if ((bypass_status_clear(pbpctl_dev)) >= 0)
+				return bypass_from_last_read(pbpctl_dev);
+		}
+
+	}
+	return BP_NOT_CAP;
+}
+
+int default_pwron_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_caps & BP_PWUP_CTL_CAP) {
+			if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+				return ((((read_reg
+					   (pbpctl_dev,
+					    STATUS_REG_ADDR)) & DFLT_PWRON_MASK)
+					 == DFLT_PWRON_MASK) ? 0 : 1);
+			}
+		}		/*else if ((!pbpctl_dev->bp_caps&BP_DIS_CAP)&&
+				   (pbpctl_dev->bp_caps&BP_PWUP_ON_CAP))
+				   return 1; */
+	}
+	return BP_NOT_CAP;
+}
+
+static int default_pwroff_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	/*if ((!pbpctl_dev->bp_caps&BP_DIS_CAP)&&
+	   (pbpctl_dev->bp_caps&BP_PWOFF_ON_CAP))
+	   return 1; */
+	if ((pbpctl_dev->bp_caps & SW_CTL_CAP)
+	    && (pbpctl_dev->bp_caps & BP_PWOFF_CTL_CAP)) {
+		return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+			  DFLT_PWROFF_MASK) == DFLT_PWROFF_MASK) ? 0 : 1);
+	}
+	return BP_NOT_CAP;
+}
+
+int dis_bypass_cap_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & BP_DIS_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+				  DIS_BYPASS_CAP_MASK) ==
+				 DIS_BYPASS_CAP_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int cmd_en_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+				  CMND_EN_MASK) == CMND_EN_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int wdt_en_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			return ((((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+				  WDT_EN_MASK) == WDT_EN_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int wdt_programmed(bpctl_dev_t *pbpctl_dev, int *timeout)
+{
+	int ret = 0;
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			if ((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+			    WDT_EN_MASK) {
+				u8 wdt_val;
+				wdt_val = read_reg(pbpctl_dev, WDT_REG_ADDR);
+				*timeout = (1 << wdt_val) * 100;
+			} else
+				*timeout = 0;
+		} else {
+			int curr_wdt_status = pbpctl_dev->wdt_status;
+			if (curr_wdt_status == WDT_STATUS_UNKNOWN)
+				*timeout = -1;
+			else
+				*timeout =
+				    curr_wdt_status ==
+				    0 ? 0 : pbpctl_dev->bypass_timer_interval;
+		};
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+int bypass_support(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+			ret =
+			    ((((read_reg(pbpctl_dev, PRODUCT_CAP_REG_ADDR)) &
+			       BYPASS_SUPPORT_MASK) ==
+			      BYPASS_SUPPORT_MASK) ? 1 : 0);
+		} else if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+			ret = 1;
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+int tap_support(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+			ret =
+			    ((((read_reg(pbpctl_dev, PRODUCT_CAP_REG_ADDR)) &
+			       TAP_SUPPORT_MASK) == TAP_SUPPORT_MASK) ? 1 : 0);
+		} else if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+			ret = 0;
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+int normal_support(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+			ret =
+			    ((((read_reg(pbpctl_dev, PRODUCT_CAP_REG_ADDR)) &
+			       NORMAL_UNSUPPORT_MASK) ==
+			      NORMAL_UNSUPPORT_MASK) ? 0 : 1);
+		} else
+			ret = 1;
+	};
+	return ret;
+}
+
+int get_bp_prod_caps(bpctl_dev_t *pbpctl_dev)
+{
+	if ((pbpctl_dev->bp_caps & SW_CTL_CAP) &&
+	    (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER))
+		return read_reg(pbpctl_dev, PRODUCT_CAP_REG_ADDR);
+	return BP_NOT_CAP;
+
+}
+
+int tap_flag_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & TAP_STATUS_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  TAP_FLAG_MASK) == TAP_FLAG_MASK) ? 1 : 0);
+
+	}
+	return BP_NOT_CAP;
+}
+
+int tap_flag_status_clear(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t status_reg = 0;
+	if (pbpctl_dev->bp_caps & TAP_STATUS_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+			status_reg = read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
+			write_reg(pbpctl_dev, status_reg & ~TAP_FLAG_MASK,
+				  STATUS_TAP_REG_ADDR);
+			return 0;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int tap_change_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+		if (pbpctl_dev->bp_caps & TAP_CAP) {
+			if (pbpctl_dev->bp_caps & BP_CAP) {
+				ret = tap_flag_status(pbpctl_dev);
+				tap_flag_status_clear(pbpctl_dev);
+			} else {
+				ret = bypass_from_last_read(pbpctl_dev);
+				bypass_status_clear(pbpctl_dev);
+			}
+		}
+	}
+	return ret;
+}
+
+int tap_off_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  TAP_OFF_MASK) == TAP_OFF_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int tap_status(bpctl_dev_t *pbpctl_dev)
+{
+	u32 ctrl_ext = 0;
+
+	if (pbpctl_dev->bp_caps & TAP_CAP) {
+		bpctl_dev_t *pbpctl_dev_b = NULL;
+
+		if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+			return BP_NOT_CAP;
+
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			if (!pbpctl_dev->bp_10g)
+				return (((BPCTL_READ_REG
+					  (pbpctl_dev_b,
+					   CTRL_EXT)) &
+					 BPCTLI_CTRL_EXT_SDP6_DATA) !=
+					0 ? 0 : 1);
+			else {
+				ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, EODSDP);
+				BP10G_WRITE_REG(pbpctl_dev_b, EODSDP,
+						(ctrl_ext |
+						 BP10G_SDP6_DATA_OUT));
+				return ((BP10G_READ_REG(pbpctl_dev_b, EODSDP) &
+					 BP10G_SDP6_DATA_IN) != 0 ? 0 : 1);
+			}
+
+		} else if (pbpctl_dev->media_type == bp_copper)
+			return (((BPCTL_READ_REG(pbpctl_dev, CTRL)) &
+				 BPCTLI_CTRL_SWDPIN0) != 0 ? 1 : 0);
+		else {
+			if ((bypass_status_clear(pbpctl_dev)) >= 0)
+				return bypass_from_last_read(pbpctl_dev);
+		}
+
+	}
+	return BP_NOT_CAP;
+}
+
+int default_pwron_tap_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  DFLT_PWRON_TAP_MASK) ==
+				 DFLT_PWRON_TAP_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int dis_tap_cap_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  DIS_TAP_CAP_MASK) ==
+				 DIS_TAP_CAP_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int disc_flag_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & DISC_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8)
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  DISC_FLAG_MASK) == DISC_FLAG_MASK) ? 1 : 0);
+
+	}
+	return BP_NOT_CAP;
+}
+
+int disc_flag_status_clear(bpctl_dev_t *pbpctl_dev)
+{
+	uint32_t status_reg = 0;
+	if (pbpctl_dev->bp_caps & DISC_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8) {
+			status_reg = read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR);
+			write_reg(pbpctl_dev, status_reg & ~DISC_FLAG_MASK,
+				  STATUS_DISC_REG_ADDR);
+			return BP_OK;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int disc_change_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	if (pbpctl_dev->bp_caps & DISC_CAP) {
+		ret = disc_flag_status(pbpctl_dev);
+		disc_flag_status_clear(pbpctl_dev);
+		return ret;
+	}
+	return BP_NOT_CAP;
+}
+
+int disc_off_status(bpctl_dev_t *pbpctl_dev)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+	u32 ctrl_ext = 0;
+
+	if (pbpctl_dev->bp_caps & DISC_CAP) {
+		if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+			return BP_NOT_CAP;
+		if (DISCF_IF_SERIES(pbpctl_dev->subdevice))
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  DISC_OFF_MASK) == DISC_OFF_MASK) ? 1 : 0);
+
+		if (pbpctl_dev->bp_i80) {
+			return (((BPCTL_READ_REG(pbpctl_dev_b, CTRL_EXT)) &
+				 BPCTLI_CTRL_EXT_SDP6_DATA) != 0 ? 1 : 0);
+
+		}
+		if (pbpctl_dev->bp_540) {
+			ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, ESDP);
+			return ((BP10G_READ_REG(pbpctl_dev_b, ESDP) &
+				 BP10G_SDP2_DATA) != 0 ? 1 : 0);
+
+		}
+		if (pbpctl_dev->media_type == bp_copper) {
+
+#if 0
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  DISC_OFF_MASK) == DISC_OFF_MASK) ? 1 : 0);
+#endif
+			if (!pbpctl_dev->bp_10g)
+				return (((BPCTL_READ_REG(pbpctl_dev_b, CTRL)) &
+					 BPCTLI_CTRL_SWDPIN1) != 0 ? 1 : 0);
+			else
+				return ((BP10G_READ_REG(pbpctl_dev_b, ESDP) &
+					 BP10G_SDP1_DATA) != 0 ? 1 : 0);
+
+		} else {
+
+			if (pbpctl_dev->bp_10g9) {
+				ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, I2CCTL);
+				BP10G_WRITE_REG(pbpctl_dev_b, I2CCTL,
+						(ctrl_ext |
+						 BP10G_I2C_DATA_OUT));
+				return ((BP10G_READ_REG(pbpctl_dev_b, I2CCTL) &
+					 BP10G_I2C_DATA_IN) != 0 ? 1 : 0);
+
+			} else if (pbpctl_dev->bp_fiber5) {
+				return (((BPCTL_READ_REG(pbpctl_dev_b, CTRL)) &
+					 BPCTLI_CTRL_SWDPIN1) != 0 ? 1 : 0);
+			} else if (pbpctl_dev->bp_10gb) {
+				ctrl_ext =
+				    BP10GB_READ_REG(pbpctl_dev, MISC_REG_GPIO);
+				BP10GB_WRITE_REG(pbpctl_dev, MISC_REG_GPIO,
+						 (ctrl_ext | BP10GB_GPIO3_OE_P1)
+						 & ~(BP10GB_GPIO3_SET_P1 |
+						     BP10GB_GPIO3_CLR_P1));
+
+				return (((BP10GB_READ_REG
+					  (pbpctl_dev,
+					   MISC_REG_GPIO)) & BP10GB_GPIO3_P1) !=
+					0 ? 1 : 0);
+			}
+			if (!pbpctl_dev->bp_10g) {
+
+				return (((BPCTL_READ_REG
+					  (pbpctl_dev_b,
+					   CTRL_EXT)) &
+					 BPCTLI_CTRL_EXT_SDP6_DATA) !=
+					0 ? 1 : 0);
+			} else {
+				ctrl_ext = BP10G_READ_REG(pbpctl_dev_b, EODSDP);
+				BP10G_WRITE_REG(pbpctl_dev_b, EODSDP,
+						(ctrl_ext |
+						 BP10G_SDP6_DATA_OUT));
+				return (((BP10G_READ_REG(pbpctl_dev_b, EODSDP))
+					 & BP10G_SDP6_DATA_IN) != 0 ? 1 : 0);
+			}
+
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+static int disc_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ctrl = 0;
+	if (pbpctl_dev->bp_caps & DISC_CAP) {
+
+		if ((ctrl = disc_off_status(pbpctl_dev)) < 0)
+			return ctrl;
+		return ((ctrl == 0) ? 1 : 0);
+
+	}
+	return BP_NOT_CAP;
+}
+
+int default_pwron_disc_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & DISC_PWUP_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8)
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  DFLT_PWRON_DISC_MASK) ==
+				 DFLT_PWRON_DISC_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int dis_disc_cap_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & DIS_DISC_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8)
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  DIS_DISC_CAP_MASK) ==
+				 DIS_DISC_CAP_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int disc_port_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	bpctl_dev_t *pbpctl_dev_m;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1) {
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  TX_DISA_MASK) == TX_DISA_MASK) ? 1 : 0);
+		} else
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  TX_DISB_MASK) == TX_DISB_MASK) ? 1 : 0);
+
+	}
+	return ret;
+}
+
+int default_pwron_disc_port_status(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	bpctl_dev_t *pbpctl_dev_m;
+
+	if ((is_bypass_fn(pbpctl_dev)) == 1)
+		pbpctl_dev_m = pbpctl_dev;
+	else
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m == NULL)
+		return BP_NOT_CAP;
+
+	if (pbpctl_dev_m->bp_caps_ex & DISC_PORT_CAP_EX) {
+		if (is_bypass_fn(pbpctl_dev) == 1)
+			return ret;
+		/*  return((((read_reg(pbpctl_dev,STATUS_TAP_REG_ADDR)) & TX_DISA_MASK)==TX_DISA_MASK)?1:0); */
+		else
+			return ret;
+		/*   return((((read_reg(pbpctl_dev,STATUS_TAP_REG_ADDR)) & TX_DISA_MASK)==TX_DISA_MASK)?1:0); */
+
+	}
+	return ret;
+}
+
+int wdt_exp_mode_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver <= PXG2BPI_VER)
+			return 0;	/* bypass mode */
+		else if (pbpctl_dev->bp_ext_ver == PXG2TBPI_VER)
+			return 1;	/* tap mode */
+		else if (pbpctl_dev->bp_ext_ver >= PXE2TBPI_VER) {
+			if (pbpctl_dev->bp_ext_ver >= 0x8) {
+				if (((read_reg
+				      (pbpctl_dev,
+				       STATUS_DISC_REG_ADDR)) &
+				     WDTE_DISC_BPN_MASK) == WDTE_DISC_BPN_MASK)
+					return 2;
+			}
+			return ((((read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR)) &
+				  WDTE_TAP_BPN_MASK) ==
+				 WDTE_TAP_BPN_MASK) ? 1 : 0);
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+int tpl2_flag_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps_ex & TPL2_CAP_EX) {
+		return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+			  TPL2_FLAG_MASK) == TPL2_FLAG_MASK) ? 1 : 0);
+
+	}
+	return BP_NOT_CAP;
+}
+
+int tpl_hw_status(bpctl_dev_t *pbpctl_dev)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+		return BP_NOT_CAP;
+
+	if (TPL_IF_SERIES(pbpctl_dev->subdevice))
+		return (((BPCTL_READ_REG(pbpctl_dev, CTRL)) &
+			 BPCTLI_CTRL_SWDPIN0) != 0 ? 1 : 0);
+	return BP_NOT_CAP;
+}
+
+
+int bp_wait_at_pwup_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		if (pbpctl_dev->bp_ext_ver >= 0x8)
+			return ((((read_reg(pbpctl_dev, CONT_CONFIG_REG_ADDR)) &
+				  WAIT_AT_PWUP_MASK) ==
+				 WAIT_AT_PWUP_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+int bp_hw_reset_status(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+
+		if (pbpctl_dev->bp_ext_ver >= 0x8)
+			return ((((read_reg(pbpctl_dev, CONT_CONFIG_REG_ADDR)) &
+				  EN_HW_RESET_MASK) ==
+				 EN_HW_RESET_MASK) ? 1 : 0);
+	}
+	return BP_NOT_CAP;
+}
+
+
+int std_nic_status(bpctl_dev_t *pbpctl_dev)
+{
+	int status_val = 0;
+
+	if (pbpctl_dev->bp_caps & STD_NIC_CAP) {
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+			return BP_NOT_CAP;
+		if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+			return ((((read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR)) &
+				  STD_NIC_ON_MASK) == STD_NIC_ON_MASK) ? 1 : 0);
+		}
+
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			if (pbpctl_dev->bp_caps & BP_CAP) {
+				status_val =
+				    read_reg(pbpctl_dev, STATUS_REG_ADDR);
+				if (((!(status_val & WDT_EN_MASK))
+				     && ((status_val & STD_NIC_MASK) ==
+					 STD_NIC_MASK)))
+					status_val = 1;
+				else
+					return 0;
+			}
+			if (pbpctl_dev->bp_caps & TAP_CAP) {
+				status_val =
+				    read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
+				if ((status_val & STD_NIC_TAP_MASK) ==
+				    STD_NIC_TAP_MASK)
+					status_val = 1;
+				else
+					return 0;
+			}
+			if (pbpctl_dev->bp_caps & TAP_CAP) {
+				if ((disc_off_status(pbpctl_dev)))
+					status_val = 1;
+				else
+					return 0;
+			}
+
+			return status_val;
+		}
+	}
+	return BP_NOT_CAP;
+}
+
+/******************************************************/
+/**************SW_INIT*********************************/
+/******************************************************/
+void bypass_caps_init(bpctl_dev_t *pbpctl_dev)
+{
+	u_int32_t ctrl_ext = 0;
+	bpctl_dev_t *pbpctl_dev_m = NULL;
+
+#ifdef BYPASS_DEBUG
+	int ret = 0;
+	if (!(INTEL_IF_SERIES(adapter->bp_device_block.subdevice))) {
+		ret = read_reg(pbpctl_dev, VER_REG_ADDR);
+		printk("VER_REG reg1=%x\n", ret);
+		ret = read_reg(pbpctl_dev, PRODUCT_CAP_REG_ADDR);
+		printk("PRODUCT_CAP reg=%x\n", ret);
+		ret = read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
+		printk("STATUS_TAP reg1=%x\n", ret);
+		ret = read_reg(pbpctl_dev, 0x7);
+		printk("SIG_REG reg1=%x\n", ret);
+		ret = read_reg(pbpctl_dev, STATUS_REG_ADDR);
+		printk("STATUS_REG_ADDR=%x\n", ret);
+		ret = read_reg(pbpctl_dev, WDT_REG_ADDR);
+		printk("WDT_REG_ADDR=%x\n", ret);
+		ret = read_reg(pbpctl_dev, TMRL_REG_ADDR);
+		printk("TMRL_REG_ADDR=%x\n", ret);
+		ret = read_reg(pbpctl_dev, TMRH_REG_ADDR);
+		printk("TMRH_REG_ADDR=%x\n", ret);
+	}
+#endif
+	if ((pbpctl_dev->bp_fiber5) || (pbpctl_dev->bp_10g9)) {
+		pbpctl_dev->media_type = bp_fiber;
+	} else if (pbpctl_dev->bp_10gb) {
+		if (BP10GB_CX4_SERIES(pbpctl_dev->subdevice))
+			pbpctl_dev->media_type = bp_cx4;
+		else
+			pbpctl_dev->media_type = bp_fiber;
+
+	}
+
+	else if (pbpctl_dev->bp_540)
+		pbpctl_dev->media_type = bp_none;
+	else if (!pbpctl_dev->bp_10g) {
+
+		ctrl_ext = BPCTL_READ_REG(pbpctl_dev, CTRL_EXT);
+		if ((ctrl_ext & BPCTLI_CTRL_EXT_LINK_MODE_MASK) == 0x0)
+			pbpctl_dev->media_type = bp_copper;
+		else
+			pbpctl_dev->media_type = bp_fiber;
+
+	} else {
+		if (BP10G_CX4_SERIES(pbpctl_dev->subdevice))
+			pbpctl_dev->media_type = bp_cx4;
+		else
+			pbpctl_dev->media_type = bp_fiber;
+	}
+
+	if (is_bypass_fn(pbpctl_dev)) {
+
+		pbpctl_dev->bp_caps |= BP_PWOFF_ON_CAP;
+		if (pbpctl_dev->media_type == bp_fiber)
+			pbpctl_dev->bp_caps |=
+			    (TX_CTL_CAP | TX_STATUS_CAP | TPL_CAP);
+
+		if (TPL_IF_SERIES(pbpctl_dev->subdevice)) {
+			pbpctl_dev->bp_caps |= TPL_CAP;
+		}
+
+		if (INTEL_IF_SERIES(pbpctl_dev->subdevice)) {
+			pbpctl_dev->bp_caps |=
+			    (BP_CAP | BP_STATUS_CAP | SW_CTL_CAP |
+			     BP_PWUP_ON_CAP | BP_PWUP_OFF_CAP | BP_PWOFF_OFF_CAP
+			     | WD_CTL_CAP | WD_STATUS_CAP | STD_NIC_CAP |
+			     WD_TIMEOUT_CAP);
+
+			pbpctl_dev->bp_ext_ver = OLD_IF_VER;
+			return;
+		}
+
+		if ((pbpctl_dev->bp_fw_ver == 0xff) &&
+		    OLD_IF_SERIES(pbpctl_dev->subdevice)) {
+
+			pbpctl_dev->bp_caps |=
+			    (BP_CAP | BP_STATUS_CAP | BP_STATUS_CHANGE_CAP |
+			     SW_CTL_CAP | BP_PWUP_ON_CAP | WD_CTL_CAP |
+			     WD_STATUS_CAP | WD_TIMEOUT_CAP);
+
+			pbpctl_dev->bp_ext_ver = OLD_IF_VER;
+			return;
+		}
+
+		else {
+			switch (pbpctl_dev->bp_fw_ver) {
+			case BP_FW_VER_A0:
+			case BP_FW_VER_A1:{
+					pbpctl_dev->bp_ext_ver =
+					    (pbpctl_dev->
+					     bp_fw_ver & EXT_VER_MASK);
+					break;
+				}
+			default:{
+					if ((bypass_sign_check(pbpctl_dev)) !=
+					    1) {
+						pbpctl_dev->bp_caps = 0;
+						return;
+					}
+					pbpctl_dev->bp_ext_ver =
+					    (pbpctl_dev->
+					     bp_fw_ver & EXT_VER_MASK);
+				}
+			}
+		}
+
+		if (pbpctl_dev->bp_ext_ver == PXG2BPI_VER)
+			pbpctl_dev->bp_caps |=
+			    (BP_CAP | BP_STATUS_CAP | BP_STATUS_CHANGE_CAP |
+			     SW_CTL_CAP | BP_DIS_CAP | BP_DIS_STATUS_CAP |
+			     BP_PWUP_ON_CAP | BP_PWUP_OFF_CAP | BP_PWUP_CTL_CAP
+			     | WD_CTL_CAP | STD_NIC_CAP | WD_STATUS_CAP |
+			     WD_TIMEOUT_CAP);
+		else if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
+			int cap_reg;
+
+			pbpctl_dev->bp_caps |=
+			    (SW_CTL_CAP | WD_CTL_CAP | WD_STATUS_CAP |
+			     WD_TIMEOUT_CAP);
+			cap_reg = get_bp_prod_caps(pbpctl_dev);
+
+			if ((cap_reg & NORMAL_UNSUPPORT_MASK) ==
+			    NORMAL_UNSUPPORT_MASK)
+				pbpctl_dev->bp_caps |= NIC_CAP_NEG;
+			else
+				pbpctl_dev->bp_caps |= STD_NIC_CAP;
+
+			if ((normal_support(pbpctl_dev)) == 1)
+
+				pbpctl_dev->bp_caps |= STD_NIC_CAP;
+
+			else
+				pbpctl_dev->bp_caps |= NIC_CAP_NEG;
+			if ((cap_reg & BYPASS_SUPPORT_MASK) ==
+			    BYPASS_SUPPORT_MASK) {
+				pbpctl_dev->bp_caps |=
+				    (BP_CAP | BP_STATUS_CAP |
+				     BP_STATUS_CHANGE_CAP | BP_DIS_CAP |
+				     BP_DIS_STATUS_CAP | BP_PWUP_ON_CAP |
+				     BP_PWUP_OFF_CAP | BP_PWUP_CTL_CAP);
+				if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER7)
+					pbpctl_dev->bp_caps |=
+					    BP_PWOFF_ON_CAP | BP_PWOFF_OFF_CAP |
+					    BP_PWOFF_CTL_CAP;
+			}
+			if ((cap_reg & TAP_SUPPORT_MASK) == TAP_SUPPORT_MASK) {
+				pbpctl_dev->bp_caps |=
+				    (TAP_CAP | TAP_STATUS_CAP |
+				     TAP_STATUS_CHANGE_CAP | TAP_DIS_CAP |
+				     TAP_DIS_STATUS_CAP | TAP_PWUP_ON_CAP |
+				     TAP_PWUP_OFF_CAP | TAP_PWUP_CTL_CAP);
+			}
+			if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER8) {
+				if ((cap_reg & DISC_SUPPORT_MASK) ==
+				    DISC_SUPPORT_MASK)
+					pbpctl_dev->bp_caps |=
+					    (DISC_CAP | DISC_DIS_CAP |
+					     DISC_PWUP_CTL_CAP);
+				if ((cap_reg & TPL2_SUPPORT_MASK) ==
+				    TPL2_SUPPORT_MASK) {
+					pbpctl_dev->bp_caps_ex |= TPL2_CAP_EX;
+					pbpctl_dev->bp_caps |= TPL_CAP;
+					pbpctl_dev->bp_tpl_flag =
+					    tpl2_flag_status(pbpctl_dev);
+				}
+
+			}
+
+			if (pbpctl_dev->bp_ext_ver >= BP_FW_EXT_VER9) {
+				if ((cap_reg & DISC_PORT_SUPPORT_MASK) ==
+				    DISC_PORT_SUPPORT_MASK) {
+					pbpctl_dev->bp_caps_ex |=
+					    DISC_PORT_CAP_EX;
+					pbpctl_dev->bp_caps |=
+					    (TX_CTL_CAP | TX_STATUS_CAP);
+				}
+
+			}
+
+		}
+		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
+			if ((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
+			    WDT_EN_MASK)
+				pbpctl_dev->wdt_status = WDT_STATUS_EN;
+			else
+				pbpctl_dev->wdt_status = WDT_STATUS_DIS;
+		}
+
+	} else if ((P2BPFI_IF_SERIES(pbpctl_dev->subdevice)) ||
+		   (PEGF5_IF_SERIES(pbpctl_dev->subdevice)) ||
+		   (PEGF80_IF_SERIES(pbpctl_dev->subdevice)) ||
+		   (BP10G9_IF_SERIES(pbpctl_dev->subdevice))) {
+		pbpctl_dev->bp_caps |= (TX_CTL_CAP | TX_STATUS_CAP);
+	}
+	if ((pbpctl_dev->subdevice & 0xa00) == 0xa00)
+		pbpctl_dev->bp_caps |= (TX_CTL_CAP | TX_STATUS_CAP);
+	if (PEG5_IF_SERIES(pbpctl_dev->subdevice))
+		pbpctl_dev->bp_caps |= (TX_CTL_CAP | TX_STATUS_CAP);
+
+	if (BP10GB_IF_SERIES(pbpctl_dev->subdevice)) {
+		pbpctl_dev->bp_caps &= ~(TX_CTL_CAP | TX_STATUS_CAP);
+	}
+	pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+	if (pbpctl_dev_m != NULL) {
+		int cap_reg = 0;
+		if (pbpctl_dev_m->bp_ext_ver >= 0x9) {
+			cap_reg = get_bp_prod_caps(pbpctl_dev_m);
+			if ((cap_reg & DISC_PORT_SUPPORT_MASK) ==
+			    DISC_PORT_SUPPORT_MASK)
+				pbpctl_dev->bp_caps |=
+				    (TX_CTL_CAP | TX_STATUS_CAP);
+			pbpctl_dev->bp_caps_ex |= DISC_PORT_CAP_EX;
+		}
+	}
+}
+
+int bypass_off_init(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+		return dis_bypass_cap(pbpctl_dev);
+	wdt_off(pbpctl_dev);
+	if (pbpctl_dev->bp_caps & BP_CAP)
+		bypass_off(pbpctl_dev);
+	if (pbpctl_dev->bp_caps & TAP_CAP)
+		tap_off(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+	return 0;
+}
+
+void remove_bypass_wd_auto(bpctl_dev_t *pbpctl_dev)
+{
+#ifdef BP_SELF_TEST
+	bpctl_dev_t *pbpctl_dev_sl = NULL;
+#endif
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+
+		del_timer_sync(&pbpctl_dev->bp_timer);
+#ifdef BP_SELF_TEST
+		pbpctl_dev_sl = get_status_port_fn(pbpctl_dev);
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,31))
+		if (pbpctl_dev_sl && (pbpctl_dev_sl->ndev)
+		    && (pbpctl_dev_sl->ndev->hard_start_xmit)
+		    && (pbpctl_dev_sl->hard_start_xmit_save)) {
+			rtnl_lock();
+			pbpctl_dev_sl->ndev->hard_start_xmit =
+			    pbpctl_dev_sl->hard_start_xmit_save;
+			rtnl_unlock();
+		}
+#else
+		if (pbpctl_dev_sl && (pbpctl_dev_sl->ndev)) {
+			if ((pbpctl_dev_sl->ndev->netdev_ops)
+			    && (pbpctl_dev_sl->old_ops)) {
+				rtnl_lock();
+				pbpctl_dev_sl->ndev->netdev_ops =
+				    pbpctl_dev_sl->old_ops;
+				pbpctl_dev_sl->old_ops = NULL;
+
+				rtnl_unlock();
+
+			}
+
+		}
+
+#endif
+#endif
+	}
+
+}
+
+int init_bypass_wd_auto(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		init_timer(&pbpctl_dev->bp_timer);
+		pbpctl_dev->bp_timer.function = &wd_reset_timer;
+		pbpctl_dev->bp_timer.data = (unsigned long)pbpctl_dev;
+		return 1;
+	}
+	return BP_NOT_CAP;
+}
+
+#ifdef BP_SELF_TEST
+int bp_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	bpctl_dev_t *pbpctl_dev = NULL, *pbpctl_dev_m = NULL;
+	int idx_dev = 0;
+	struct ethhdr *eth = (struct ethhdr *)skb->data;
+
+	for (idx_dev = 0;
+	     ((bpctl_dev_arr[idx_dev].ndev != NULL) && (idx_dev < device_num));
+	     idx_dev++) {
+		if (bpctl_dev_arr[idx_dev].ndev == dev) {
+			pbpctl_dev = &bpctl_dev_arr[idx_dev];
+			break;
+		}
+	}
+	if (!pbpctl_dev)
+		return 1;
+	if ((htons(ETH_P_BPTEST) == eth->h_proto)) {
+
+		pbpctl_dev_m = get_master_port_fn(pbpctl_dev);
+		if (pbpctl_dev_m) {
+
+			if (bypass_status(pbpctl_dev_m)) {
+				cmnd_on(pbpctl_dev_m);
+				bypass_off(pbpctl_dev_m);
+				cmnd_off(pbpctl_dev_m);
+			}
+			wdt_timer_reload(pbpctl_dev_m);
+		}
+		dev_kfree_skb_irq(skb);
+		return 0;
+	}
+	return pbpctl_dev->hard_start_xmit_save(skb, dev);
+}
+#endif
+
+int set_bypass_wd_auto(bpctl_dev_t *pbpctl_dev, unsigned int param)
+{
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (pbpctl_dev->reset_time != param) {
+			if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+				pbpctl_dev->reset_time =
+				    (param <
+				     WDT_AUTO_MIN_INT) ? WDT_AUTO_MIN_INT :
+				    param;
+			else
+				pbpctl_dev->reset_time = param;
+			if (param)
+				mod_timer(&pbpctl_dev->bp_timer, jiffies);
+		}
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_bypass_wd_auto(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		return pbpctl_dev->reset_time;
+	}
+	return BP_NOT_CAP;
+}
+
+#ifdef  BP_SELF_TEST
+
+int set_bp_self_test(bpctl_dev_t *pbpctl_dev, unsigned int param)
+{
+	bpctl_dev_t *pbpctl_dev_sl = NULL;
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		pbpctl_dev->bp_self_test_flag = param == 0 ? 0 : 1;
+		pbpctl_dev_sl = get_status_port_fn(pbpctl_dev);
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,31))
+		if ((pbpctl_dev_sl->ndev) &&
+		    (pbpctl_dev_sl->ndev->hard_start_xmit)) {
+			rtnl_lock();
+			if (pbpctl_dev->bp_self_test_flag == 1) {
+
+				pbpctl_dev_sl->hard_start_xmit_save =
+				    pbpctl_dev_sl->ndev->hard_start_xmit;
+				pbpctl_dev_sl->ndev->hard_start_xmit =
+				    bp_hard_start_xmit;
+			} else if (pbpctl_dev_sl->hard_start_xmit_save) {
+				pbpctl_dev_sl->ndev->hard_start_xmit =
+				    pbpctl_dev_sl->hard_start_xmit_save;
+			}
+			rtnl_unlock();
+		}
+#else
+		if ((pbpctl_dev_sl->ndev) && (pbpctl_dev_sl->ndev->netdev_ops)) {
+			rtnl_lock();
+			if (pbpctl_dev->bp_self_test_flag == 1) {
+
+				pbpctl_dev_sl->old_ops =
+				    pbpctl_dev_sl->ndev->netdev_ops;
+				pbpctl_dev_sl->new_ops =
+				    *pbpctl_dev_sl->old_ops;
+				pbpctl_dev_sl->new_ops.ndo_start_xmit =
+				    bp_hard_start_xmit;
+				pbpctl_dev_sl->ndev->netdev_ops =
+				    &pbpctl_dev_sl->new_ops;
+
+			} else if (pbpctl_dev_sl->old_ops) {
+				pbpctl_dev_sl->ndev->netdev_ops =
+				    pbpctl_dev_sl->old_ops;
+				pbpctl_dev_sl->old_ops = NULL;
+			}
+			rtnl_unlock();
+		}
+#endif
+
+		set_bypass_wd_auto(pbpctl_dev, param);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_bp_self_test(bpctl_dev_t *pbpctl_dev)
+{
+
+	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
+		if (pbpctl_dev->bp_self_test_flag == 1)
+			return pbpctl_dev->reset_time;
+		else
+			return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+#endif
+
+/**************************************************************/
+/************************* API ********************************/
+/**************************************************************/
+
+int is_bypass_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return (((pbpctl_dev->func == 0) || (pbpctl_dev->func == 2)) ? 1 : 0);
+}
+
+int set_bypass_fn(bpctl_dev_t *pbpctl_dev, int bypass_mode)
+{
+	int ret = 0;
+
+	if (!(pbpctl_dev->bp_caps & BP_CAP))
+		return BP_NOT_CAP;
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (!bypass_mode)
+		ret = bypass_off(pbpctl_dev);
+	else
+		ret = bypass_on(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+
+	return ret;
+}
+
+int get_bypass_fn(bpctl_dev_t *pbpctl_dev)
+{
+	return bypass_status(pbpctl_dev);
+}
+
+int get_bypass_change_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return bypass_change_status(pbpctl_dev);
+}
+
+int set_dis_bypass_fn(bpctl_dev_t *pbpctl_dev, int dis_param)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!(pbpctl_dev->bp_caps & BP_DIS_CAP))
+		return BP_NOT_CAP;
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (dis_param)
+		ret = dis_bypass_cap(pbpctl_dev);
+	else
+		ret = en_bypass_cap(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+	return ret;
+}
+
+int get_dis_bypass_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return dis_bypass_cap_status(pbpctl_dev);
+}
+
+int set_bypass_pwoff_fn(bpctl_dev_t *pbpctl_dev, int bypass_mode)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!(pbpctl_dev->bp_caps & BP_PWOFF_CTL_CAP))
+		return BP_NOT_CAP;
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (bypass_mode)
+		ret = bypass_state_pwroff(pbpctl_dev);
+	else
+		ret = normal_state_pwroff(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+	return ret;
+}
+
+int get_bypass_pwoff_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return default_pwroff_status(pbpctl_dev);
+}
+
+int set_bypass_pwup_fn(bpctl_dev_t *pbpctl_dev, int bypass_mode)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!(pbpctl_dev->bp_caps & BP_PWUP_CTL_CAP))
+		return BP_NOT_CAP;
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (bypass_mode)
+		ret = bypass_state_pwron(pbpctl_dev);
+	else
+		ret = normal_state_pwron(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+	return ret;
+}
+
+int get_bypass_pwup_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return default_pwron_status(pbpctl_dev);
+}
+
+int set_bypass_wd_fn(bpctl_dev_t *pbpctl_dev, int timeout)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!(pbpctl_dev->bp_caps & WD_CTL_CAP))
+		return BP_NOT_CAP;
+
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (!timeout)
+		ret = wdt_off(pbpctl_dev);
+	else {
+		wdt_on(pbpctl_dev, timeout);
+		ret = pbpctl_dev->bypass_timer_interval;
+	}
+	cmnd_off(pbpctl_dev);
+	return ret;
+}
+
+int get_bypass_wd_fn(bpctl_dev_t *pbpctl_dev, int *timeout)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return wdt_programmed(pbpctl_dev, timeout);
+}
+
+int get_wd_expire_time_fn(bpctl_dev_t *pbpctl_dev, int *time_left)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return wdt_timer(pbpctl_dev, time_left);
+}
+
+int reset_bypass_wd_timer_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return wdt_timer_reload(pbpctl_dev);
+}
+
+int get_wd_set_caps_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int bp_status = 0;
+
+	unsigned int step_value = TIMEOUT_MAX_STEP + 1, bit_cnt = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (INTEL_IF_SERIES(pbpctl_dev->subdevice))
+		return BP_NOT_CAP;
+
+	while ((step_value >>= 1))
+		bit_cnt++;
+
+	if (is_bypass_fn(pbpctl_dev)) {
+		bp_status =
+		    WD_STEP_COUNT_MASK(bit_cnt) | WDT_STEP_TIME |
+		    WD_MIN_TIME_MASK(TIMEOUT_UNIT / 100);
+	} else
+		return -1;
+
+	return bp_status;
+}
+
+int set_std_nic_fn(bpctl_dev_t *pbpctl_dev, int nic_mode)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!(pbpctl_dev->bp_caps & STD_NIC_CAP))
+		return BP_NOT_CAP;
+
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	if (nic_mode)
+		ret = std_nic_on(pbpctl_dev);
+	else
+		ret = std_nic_off(pbpctl_dev);
+	cmnd_off(pbpctl_dev);
+	return ret;
+}
+
+int get_std_nic_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return std_nic_status(pbpctl_dev);
+}
+
+int set_tap_fn(bpctl_dev_t *pbpctl_dev, int tap_mode)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & TAP_CAP) && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (!tap_mode)
+			tap_off(pbpctl_dev);
+		else
+			tap_on(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_tap_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return tap_status(pbpctl_dev);
+}
+
+int set_tap_pwup_fn(bpctl_dev_t *pbpctl_dev, int tap_mode)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & TAP_PWUP_CTL_CAP)
+	    && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (tap_mode)
+			ret = tap_state_pwron(pbpctl_dev);
+		else
+			ret = normal_state_pwron(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+int get_tap_pwup_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((ret = default_pwron_tap_status(pbpctl_dev)) < 0)
+		return ret;
+	return ((ret == 0) ? 1 : 0);
+}
+
+int get_tap_change_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return tap_change_status(pbpctl_dev);
+}
+
+int set_dis_tap_fn(bpctl_dev_t *pbpctl_dev, int dis_param)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & TAP_DIS_CAP) && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (dis_param)
+			ret = dis_tap_cap(pbpctl_dev);
+		else
+			ret = en_tap_cap(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+		return ret;
+	} else
+		return BP_NOT_CAP;
+}
+
+int get_dis_tap_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return dis_tap_cap_status(pbpctl_dev);
+}
+
+int set_disc_fn(bpctl_dev_t *pbpctl_dev, int disc_mode)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & DISC_CAP) && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (!disc_mode)
+			disc_off(pbpctl_dev);
+		else
+			disc_on(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+
+		return BP_OK;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_disc_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	ret = disc_status(pbpctl_dev);
+
+	return ret;
+}
+
+int set_disc_pwup_fn(bpctl_dev_t *pbpctl_dev, int disc_mode)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & DISC_PWUP_CTL_CAP)
+	    && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (disc_mode)
+			ret = disc_state_pwron(pbpctl_dev);
+		else
+			ret = normal_state_pwron(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+	} else
+		ret = BP_NOT_CAP;
+	return ret;
+}
+
+int get_disc_pwup_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	ret = default_pwron_disc_status(pbpctl_dev);
+	return (ret == 0 ? 1 : (ret < 0 ? BP_NOT_CAP : 0));
+}
+
+int get_disc_change_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	ret = disc_change_status(pbpctl_dev);
+	return ret;
+}
+
+int set_dis_disc_fn(bpctl_dev_t *pbpctl_dev, int dis_param)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & DISC_DIS_CAP)
+	    && ((cmnd_on(pbpctl_dev)) >= 0)) {
+		if (dis_param)
+			ret = dis_disc_cap(pbpctl_dev);
+		else
+			ret = en_disc_cap(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+		return ret;
+	} else
+		return BP_NOT_CAP;
+}
+
+int get_dis_disc_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	ret = dis_disc_cap_status(pbpctl_dev);
+
+	return ret;
+}
+
+int set_disc_port_fn(bpctl_dev_t *pbpctl_dev, int disc_mode)
+{
+	int ret = BP_NOT_CAP;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!disc_mode)
+		ret = disc_port_off(pbpctl_dev);
+	else
+		ret = disc_port_on(pbpctl_dev);
+
+	return ret;
+}
+
+int get_disc_port_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return disc_port_status(pbpctl_dev);
+}
+
+int set_disc_port_pwup_fn(bpctl_dev_t *pbpctl_dev, int disc_mode)
+{
+	int ret = BP_NOT_CAP;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (!disc_mode)
+		ret = normal_port_state_pwron(pbpctl_dev);
+	else
+		ret = disc_port_state_pwron(pbpctl_dev);
+
+	return ret;
+}
+
+int get_disc_port_pwup_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((ret = default_pwron_disc_port_status(pbpctl_dev)) < 0)
+		return ret;
+	return ((ret == 0) ? 1 : 0);
+}
+
+int get_wd_exp_mode_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return wdt_exp_mode_status(pbpctl_dev);
+}
+
+int set_wd_exp_mode_fn(bpctl_dev_t *pbpctl_dev, int param)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return wdt_exp_mode(pbpctl_dev, param);
+}
+
+int reset_cont_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((ret = cmnd_on(pbpctl_dev)) < 0)
+		return ret;
+	return reset_cont(pbpctl_dev);
+}
+
+int set_tx_fn(bpctl_dev_t *pbpctl_dev, int tx_state)
+{
+
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & TPL_CAP) &&
+	    (pbpctl_dev->bp_caps & SW_CTL_CAP)) {
+		if ((pbpctl_dev->bp_tpl_flag))
+			return BP_NOT_CAP;
+	} else if ((pbpctl_dev_b = get_master_port_fn(pbpctl_dev))) {
+		if ((pbpctl_dev_b->bp_caps & TPL_CAP) &&
+		    (pbpctl_dev_b->bp_tpl_flag))
+			return BP_NOT_CAP;
+	}
+	return set_tx(pbpctl_dev, tx_state);
+}
+
+int set_bp_force_link_fn(int dev_num, int tx_state)
+{
+	static bpctl_dev_t *bpctl_dev_curr;
+
+	if ((dev_num < 0) || (dev_num > device_num)
+	    || (bpctl_dev_arr[dev_num].pdev == NULL))
+		return -1;
+	bpctl_dev_curr = &bpctl_dev_arr[dev_num];
+
+	return set_bp_force_link(bpctl_dev_curr, tx_state);
+}
+
+int set_wd_autoreset_fn(bpctl_dev_t *pbpctl_dev, int param)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return set_bypass_wd_auto(pbpctl_dev, param);
+}
+
+int get_wd_autoreset_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return get_bypass_wd_auto(pbpctl_dev);
+}
+
+#ifdef BP_SELF_TEST
+int set_bp_self_test_fn(bpctl_dev_t *pbpctl_dev, int param)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return set_bp_self_test(pbpctl_dev, param);
+}
+
+int get_bp_self_test_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return get_bp_self_test(pbpctl_dev);
+}
+
+#endif
+
+int get_bypass_caps_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	return pbpctl_dev->bp_caps;
+
+}
+
+int get_bypass_slave_fn(bpctl_dev_t *pbpctl_dev, bpctl_dev_t **pbpctl_dev_out)
+{
+	int idx_dev = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->func == 0) || (pbpctl_dev->func == 2)) {
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus)
+			    && (bpctl_dev_arr[idx_dev].slot ==
+				pbpctl_dev->slot)) {
+				if ((pbpctl_dev->func == 0)
+				    && (bpctl_dev_arr[idx_dev].func == 1)) {
+					*pbpctl_dev_out =
+					    &bpctl_dev_arr[idx_dev];
+					return 1;
+				}
+				if ((pbpctl_dev->func == 2) &&
+				    (bpctl_dev_arr[idx_dev].func == 3)) {
+					*pbpctl_dev_out =
+					    &bpctl_dev_arr[idx_dev];
+					return 1;
+				}
+			}
+		}
+		return -1;
+	} else
+		return 0;
+}
+
+int is_bypass(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->func == 0) || (pbpctl_dev->func == 2))
+		return 1;
+	else
+		return 0;
+}
+
+int get_tx_fn(bpctl_dev_t *pbpctl_dev)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+	if (!pbpctl_dev)
+		return -1;
+
+	if ((pbpctl_dev->bp_caps & TPL_CAP) &&
+	    (pbpctl_dev->bp_caps & SW_CTL_CAP)) {
+		if ((pbpctl_dev->bp_tpl_flag))
+			return BP_NOT_CAP;
+	} else if ((pbpctl_dev_b = get_master_port_fn(pbpctl_dev))) {
+		if ((pbpctl_dev_b->bp_caps & TPL_CAP) &&
+		    (pbpctl_dev_b->bp_tpl_flag))
+			return BP_NOT_CAP;
+	}
+	return tx_status(pbpctl_dev);
+}
+
+int get_bp_force_link_fn(int dev_num)
+{
+	static bpctl_dev_t *bpctl_dev_curr;
+
+	if ((dev_num < 0) || (dev_num > device_num)
+	    || (bpctl_dev_arr[dev_num].pdev == NULL))
+		return -1;
+	bpctl_dev_curr = &bpctl_dev_arr[dev_num];
+
+	return bp_force_link_status(bpctl_dev_curr);
+}
+
+static int get_bypass_link_status(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->media_type == bp_fiber)
+		return ((BPCTL_READ_REG(pbpctl_dev, CTRL) &
+			 BPCTLI_CTRL_SWDPIN1));
+	else
+		return ((BPCTL_READ_REG(pbpctl_dev, STATUS) &
+			 BPCTLI_STATUS_LU));
+
+}
+
+static void bp_tpl_timer_fn(unsigned long param)
+{
+	bpctl_dev_t *pbpctl_dev = (bpctl_dev_t *) param;
+	uint32_t link1, link2;
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+
+	if (!(pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+		return;
+
+	if (!pbpctl_dev->bp_tpl_flag) {
+		set_tx(pbpctl_dev_b, 1);
+		set_tx(pbpctl_dev, 1);
+		return;
+	}
+	link1 = get_bypass_link_status(pbpctl_dev);
+
+	link2 = get_bypass_link_status(pbpctl_dev_b);
+	if ((link1) && (tx_status(pbpctl_dev))) {
+		if ((!link2) && (tx_status(pbpctl_dev_b))) {
+			set_tx(pbpctl_dev, 0);
+		} else if (!tx_status(pbpctl_dev_b)) {
+			set_tx(pbpctl_dev_b, 1);
+		}
+	} else if ((!link1) && (tx_status(pbpctl_dev))) {
+		if ((link2) && (tx_status(pbpctl_dev_b))) {
+			set_tx(pbpctl_dev_b, 0);
+		}
+	} else if ((link1) && (!tx_status(pbpctl_dev))) {
+		if ((link2) && (tx_status(pbpctl_dev_b))) {
+			set_tx(pbpctl_dev, 1);
+		}
+	} else if ((!link1) && (!tx_status(pbpctl_dev))) {
+		if ((link2) && (tx_status(pbpctl_dev_b))) {
+			set_tx(pbpctl_dev, 1);
+		}
+	}
+
+	mod_timer(&pbpctl_dev->bp_tpl_timer, jiffies + BP_LINK_MON_DELAY * HZ);
+}
+
+void remove_bypass_tpl_auto(bpctl_dev_t *pbpctl_dev)
+{
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+	if (!pbpctl_dev)
+		return;
+	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
+
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		del_timer_sync(&pbpctl_dev->bp_tpl_timer);
+		pbpctl_dev->bp_tpl_flag = 0;
+		pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
+		if (pbpctl_dev_b)
+			set_tx(pbpctl_dev_b, 1);
+		set_tx(pbpctl_dev, 1);
+	}
+	return;
+}
+
+int init_bypass_tpl_auto(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		init_timer(&pbpctl_dev->bp_tpl_timer);
+		pbpctl_dev->bp_tpl_timer.function = &bp_tpl_timer_fn;
+		pbpctl_dev->bp_tpl_timer.data = (unsigned long)pbpctl_dev;
+		return BP_OK;
+	}
+	return BP_NOT_CAP;
+}
+
+int set_bypass_tpl_auto(bpctl_dev_t *pbpctl_dev, unsigned int param)
+{
+	if (!pbpctl_dev)
+		return -1;
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		if ((param) && (!pbpctl_dev->bp_tpl_flag)) {
+			pbpctl_dev->bp_tpl_flag = param;
+			mod_timer(&pbpctl_dev->bp_tpl_timer, jiffies + 1);
+			return BP_OK;
+		};
+		if ((!param) && (pbpctl_dev->bp_tpl_flag))
+			remove_bypass_tpl_auto(pbpctl_dev);
+
+		return BP_OK;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_bypass_tpl_auto(bpctl_dev_t *pbpctl_dev)
+{
+	if (!pbpctl_dev)
+		return -1;
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		return pbpctl_dev->bp_tpl_flag;
+	}
+	return BP_NOT_CAP;
+}
+
+int set_tpl_fn(bpctl_dev_t *pbpctl_dev, int tpl_mode)
+{
+
+	bpctl_dev_t *pbpctl_dev_b = NULL;
+	if (!pbpctl_dev)
+		return -1;
+
+	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
+
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		if (tpl_mode) {
+			if ((pbpctl_dev_b = get_status_port_fn(pbpctl_dev)))
+				set_tx(pbpctl_dev_b, 1);
+			set_tx(pbpctl_dev, 1);
+		}
+		if ((TPL_IF_SERIES(pbpctl_dev->subdevice)) ||
+		    (pbpctl_dev->bp_caps_ex & TPL2_CAP_EX)) {
+			pbpctl_dev->bp_tpl_flag = tpl_mode;
+			if (!tpl_mode)
+				tpl_hw_off(pbpctl_dev);
+			else
+				tpl_hw_on(pbpctl_dev);
+		} else
+			set_bypass_tpl_auto(pbpctl_dev, tpl_mode);
+		return 0;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_tpl_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = BP_NOT_CAP;
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_caps & TPL_CAP) {
+		if (pbpctl_dev->bp_caps_ex & TPL2_CAP_EX)
+			return tpl2_flag_status(pbpctl_dev);
+		ret = pbpctl_dev->bp_tpl_flag;
+	}
+	return ret;
+}
+
+int set_bp_wait_at_pwup_fn(bpctl_dev_t *pbpctl_dev, int tap_mode)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		/* bp_lock(pbp_device_block); */
+		cmnd_on(pbpctl_dev);
+		if (!tap_mode)
+			bp_wait_at_pwup_dis(pbpctl_dev);
+		else
+			bp_wait_at_pwup_en(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+
+		/* bp_unlock(pbp_device_block); */
+		return BP_OK;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_bp_wait_at_pwup_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	/* bp_lock(pbp_device_block); */
+	ret = bp_wait_at_pwup_status(pbpctl_dev);
+	/* bp_unlock(pbp_device_block); */
+
+	return ret;
+}
+
+int set_bp_hw_reset_fn(bpctl_dev_t *pbpctl_dev, int tap_mode)
+{
+	if (!pbpctl_dev)
+		return -1;
+
+	if (pbpctl_dev->bp_caps & SW_CTL_CAP) {
+		/*   bp_lock(pbp_device_block); */
+		cmnd_on(pbpctl_dev);
+
+		if (!tap_mode)
+			bp_hw_reset_dis(pbpctl_dev);
+		else
+			bp_hw_reset_en(pbpctl_dev);
+		cmnd_off(pbpctl_dev);
+		/*    bp_unlock(pbp_device_block); */
+		return BP_OK;
+	}
+	return BP_NOT_CAP;
+}
+
+int get_bp_hw_reset_fn(bpctl_dev_t *pbpctl_dev)
+{
+	int ret = 0;
+	if (!pbpctl_dev)
+		return -1;
+
+	/* bp_lock(pbp_device_block); */
+	ret = bp_hw_reset_status(pbpctl_dev);
+
+	/* bp_unlock(pbp_device_block); */
+
+	return ret;
+}
+
+
+int get_bypass_info_fn(bpctl_dev_t *pbpctl_dev, char *dev_name,
+		       char *add_param)
+{
+	if (!pbpctl_dev)
+		return -1;
+	if (!is_bypass_fn(pbpctl_dev))
+		return -1;
+	strcpy(dev_name, pbpctl_dev->name);
+	*add_param = pbpctl_dev->bp_fw_ver;
+	return 0;
+}
+
+int get_dev_idx_bsf(int bus, int slot, int func)
+{
+	int idx_dev = 0;
+	for (idx_dev = 0;
+	     ((bpctl_dev_arr[idx_dev].pdev != NULL) && (idx_dev < device_num));
+	     idx_dev++) {
+		if ((bus == bpctl_dev_arr[idx_dev].bus)
+		    && (slot == bpctl_dev_arr[idx_dev].slot)
+		    && (func == bpctl_dev_arr[idx_dev].func))
+
+			return idx_dev;
+	}
+	return -1;
+}
+
+static void str_low(char *str)
+{
+	int i;
+
+	for (i = 0; i < strlen(str); i++)
+		if ((str[i] >= 65) && (str[i] <= 90))
+			str[i] += 32;
+}
+
+static unsigned long str_to_hex(char *p)
+{
+	unsigned long hex = 0;
+	unsigned long length = strlen(p), shift = 0;
+	unsigned char dig = 0;
+
+	str_low(p);
+	length = strlen(p);
+
+	if (length == 0)
+		return 0;
+
+	do {
+		dig = p[--length];
+		dig = dig < 'a' ? (dig - '0') : (dig - 'a' + 0xa);
+		hex |= (dig << shift);
+		shift += 4;
+	} while (length);
+	return hex;
+}
+
+static int get_dev_idx(int ifindex)
+{
+	int idx_dev = 0;
+
+	for (idx_dev = 0;
+	     ((bpctl_dev_arr[idx_dev].pdev != NULL) && (idx_dev < device_num));
+	     idx_dev++) {
+		if (ifindex == bpctl_dev_arr[idx_dev].ifindex)
+			return idx_dev;
+	}
+
+	return -1;
+}
+
+static bpctl_dev_t *get_dev_idx_p(int ifindex)
+{
+	int idx_dev = 0;
+
+	for (idx_dev = 0;
+	     ((bpctl_dev_arr[idx_dev].pdev != NULL) && (idx_dev < device_num));
+	     idx_dev++) {
+		if (ifindex == bpctl_dev_arr[idx_dev].ifindex)
+			return &bpctl_dev_arr[idx_dev];
+	}
+
+	return NULL;
+}
+
+static void if_scan_init(void)
+{
+	int idx_dev = 0;
+	struct net_device *dev;
+	int ifindex;
+	/* rcu_read_lock(); */
+	/* rtnl_lock();     */
+	/* rcu_read_lock(); */
+#if 1
+#if (LINUX_VERSION_CODE >= 0x020618)
+	for_each_netdev(&init_net, dev)
+#elif (LINUX_VERSION_CODE >= 0x20616)
+	for_each_netdev(dev)
+#else
+	for (dev = dev_base; dev; dev = dev->next)
+#endif
+	{
+
+		struct ethtool_drvinfo drvinfo;
+		char cbuf[32];
+		char *buf = NULL;
+		char res[10];
+		int i = 0;
+		int bus = 0, slot = 0, func = 0;
+		ifindex = dev->ifindex;
+
+		memset(res, 0, 10);
+		memset(&drvinfo, 0, sizeof(struct ethtool_drvinfo));
+
+		if (dev->ethtool_ops && dev->ethtool_ops->get_drvinfo) {
+			memset(&drvinfo, 0, sizeof(drvinfo));
+			dev->ethtool_ops->get_drvinfo(dev, &drvinfo);
+		} else
+			continue;
+		if (!drvinfo.bus_info)
+			continue;
+		if (!strcmp(drvinfo.bus_info, "N/A"))
+			continue;
+		memcpy(&cbuf, drvinfo.bus_info, 32);
+		buf = &cbuf[0];
+
+		while (*buf++ != ':') ;
+		for (i = 0; i < 10; i++, buf++) {
+			if (*buf == ':')
+				break;
+			res[i] = *buf;
+
+		}
+		buf++;
+		bus = str_to_hex(res);
+		memset(res, 0, 10);
+
+		for (i = 0; i < 10; i++, buf++) {
+			if (*buf == '.')
+				break;
+			res[i] = *buf;
+
+		}
+		buf++;
+		slot = str_to_hex(res);
+		func = str_to_hex(buf);
+		idx_dev = get_dev_idx_bsf(bus, slot, func);
+
+		if (idx_dev != -1) {
+
+			bpctl_dev_arr[idx_dev].ifindex = ifindex;
+			bpctl_dev_arr[idx_dev].ndev = dev;
+
+		}
+
+	}
+#endif
+	/* rtnl_unlock();     */
+	/* rcu_read_unlock(); */
+
+}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30))
+static int device_ioctl(struct inode *inode,	/* see include/linux/fs.h */
+			struct file *file,	/* ditto */
+			unsigned int ioctl_num,	/* number and param for ioctl */
+			unsigned long ioctl_param)
+#else
+static long device_ioctl(struct file *file,	/* ditto */
+			 unsigned int ioctl_num,	/* number and param for ioctl */
+			 unsigned long ioctl_param)
+#endif
+{
+	struct bpctl_cmd bpctl_cmd;
+	int dev_idx = 0;
+	bpctl_dev_t *pbpctl_dev_out;
+	void __user *argp = (void __user *)ioctl_param;
+	int ret = 0;
+	unsigned long flags;
+
+	static bpctl_dev_t *pbpctl_dev;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,30))
+	/* lock_kernel(); */
+#endif
+	lock_bpctl();
+	/* local_irq_save(flags); */
+	/* if(!spin_trylock_irqsave(&bpvm_lock)){
+	   local_irq_restore(flags);
+	   unlock_bpctl();
+	   unlock_kernel();
+	   return -1;
+	   } */
+	/* spin_lock_irqsave(&bpvm_lock, flags); */
+
+/*
+* Switch according to the ioctl called
+*/
+	if (ioctl_num == IOCTL_TX_MSG(IF_SCAN)) {
+		if_scan_init();
+		ret = SUCCESS;
+		goto bp_exit;
+	}
+	if (copy_from_user(&bpctl_cmd, argp, sizeof(struct bpctl_cmd))) {
+
+		ret = -EFAULT;
+		goto bp_exit;
+	}
+
+	if (ioctl_num == IOCTL_TX_MSG(GET_DEV_NUM)) {
+		bpctl_cmd.out_param[0] = device_num;
+		if (copy_to_user
+		    (argp, (void *)&bpctl_cmd, sizeof(struct bpctl_cmd))) {
+			ret = -EFAULT;
+			goto bp_exit;
+		}
+		ret = SUCCESS;
+		goto bp_exit;
+
+	}
+	/* lock_bpctl();      */
+	/* preempt_disable(); */
+	local_irq_save(flags);
+	if (!spin_trylock(&bpvm_lock)) {
+		local_irq_restore(flags);
+		unlock_bpctl();
+		return -1;
+	}
+
+/*    	preempt_disable();
+	rcu_read_lock();
+      	spin_lock_irqsave(&bpvm_lock, flags);
+*/
+	if ((bpctl_cmd.in_param[5]) ||
+	    (bpctl_cmd.in_param[6]) || (bpctl_cmd.in_param[7]))
+		dev_idx = get_dev_idx_bsf(bpctl_cmd.in_param[5],
+					  bpctl_cmd.in_param[6],
+					  bpctl_cmd.in_param[7]);
+	else if (bpctl_cmd.in_param[1] == 0)
+		dev_idx = bpctl_cmd.in_param[0];
+	else
+		dev_idx = get_dev_idx(bpctl_cmd.in_param[1]);
+
+	if (dev_idx < 0 || dev_idx > device_num) {
+		/* unlock_bpctl();
+		   preempt_enable(); */
+		ret = -EOPNOTSUPP;
+		/* preempt_enable();
+		   rcu_read_unlock();  */
+		spin_unlock_irqrestore(&bpvm_lock, flags);
+		goto bp_exit;
+	}
+
+	bpctl_cmd.out_param[0] = bpctl_dev_arr[dev_idx].bus;
+	bpctl_cmd.out_param[1] = bpctl_dev_arr[dev_idx].slot;
+	bpctl_cmd.out_param[2] = bpctl_dev_arr[dev_idx].func;
+	bpctl_cmd.out_param[3] = bpctl_dev_arr[dev_idx].ifindex;
+
+	if ((bpctl_dev_arr[dev_idx].bp_10gb)
+	    && (!(bpctl_dev_arr[dev_idx].ifindex))) {
+		printk("Please load network driver for %s adapter!\n",
+		       bpctl_dev_arr[dev_idx].name);
+		bpctl_cmd.status = -1;
+		ret = SUCCESS;
+		/* preempt_enable(); */
+		/* rcu_read_unlock(); */
+		spin_unlock_irqrestore(&bpvm_lock, flags);
+		goto bp_exit;
+
+	}
+	if ((bpctl_dev_arr[dev_idx].bp_10gb) && (bpctl_dev_arr[dev_idx].ndev)) {
+		if (!(bpctl_dev_arr[dev_idx].ndev->flags & IFF_UP)) {
+			if (!(bpctl_dev_arr[dev_idx].ndev->flags & IFF_UP)) {
+				printk
+				    ("Please bring up network interfaces for %s adapter!\n",
+				     bpctl_dev_arr[dev_idx].name);
+				bpctl_cmd.status = -1;
+				ret = SUCCESS;
+				/* preempt_enable(); */
+				/* rcu_read_unlock(); */
+				spin_unlock_irqrestore(&bpvm_lock, flags);
+				goto bp_exit;
+			}
+
+		}
+	}
+
+	if ((dev_idx < 0) || (dev_idx > device_num)
+	    || (bpctl_dev_arr[dev_idx].pdev == NULL)) {
+		bpctl_cmd.status = -1;
+		goto bpcmd_exit;
+	}
+
+	pbpctl_dev = &bpctl_dev_arr[dev_idx];
+
+	switch (ioctl_num) {
+	case IOCTL_TX_MSG(SET_BYPASS_PWOFF):
+		bpctl_cmd.status =
+		    set_bypass_pwoff_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_PWOFF):
+		bpctl_cmd.status = get_bypass_pwoff_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_BYPASS_PWUP):
+		bpctl_cmd.status =
+		    set_bypass_pwup_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_PWUP):
+		bpctl_cmd.status = get_bypass_pwup_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_BYPASS_WD):
+		bpctl_cmd.status =
+		    set_bypass_wd_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_WD):
+		bpctl_cmd.status =
+		    get_bypass_wd_fn(pbpctl_dev, (int *)&(bpctl_cmd.data[0]));
+		break;
+
+	case IOCTL_TX_MSG(GET_WD_EXPIRE_TIME):
+		bpctl_cmd.status =
+		    get_wd_expire_time_fn(pbpctl_dev,
+					  (int *)&(bpctl_cmd.data[0]));
+		break;
+
+	case IOCTL_TX_MSG(RESET_BYPASS_WD_TIMER):
+		bpctl_cmd.status = reset_bypass_wd_timer_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(GET_WD_SET_CAPS):
+		bpctl_cmd.status = get_wd_set_caps_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_STD_NIC):
+		bpctl_cmd.status =
+		    set_std_nic_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_STD_NIC):
+		bpctl_cmd.status = get_std_nic_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_TAP):
+		bpctl_cmd.status =
+		    set_tap_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_TAP):
+		bpctl_cmd.status = get_tap_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(GET_TAP_CHANGE):
+		bpctl_cmd.status = get_tap_change_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_DIS_TAP):
+		bpctl_cmd.status =
+		    set_dis_tap_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_DIS_TAP):
+		bpctl_cmd.status = get_dis_tap_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_TAP_PWUP):
+		bpctl_cmd.status =
+		    set_tap_pwup_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_TAP_PWUP):
+		bpctl_cmd.status = get_tap_pwup_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_WD_EXP_MODE):
+		bpctl_cmd.status =
+		    set_wd_exp_mode_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_WD_EXP_MODE):
+		bpctl_cmd.status = get_wd_exp_mode_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(GET_DIS_BYPASS):
+		bpctl_cmd.status = get_dis_bypass_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_DIS_BYPASS):
+		bpctl_cmd.status =
+		    set_dis_bypass_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_CHANGE):
+		bpctl_cmd.status = get_bypass_change_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS):
+		bpctl_cmd.status = get_bypass_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_BYPASS):
+		bpctl_cmd.status =
+		    set_bypass_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_CAPS):
+		bpctl_cmd.status = get_bypass_caps_fn(pbpctl_dev);
+		/*preempt_enable(); */
+		/*rcu_read_unlock();*/
+		spin_unlock_irqrestore(&bpvm_lock, flags);
+		if (copy_to_user
+		    (argp, (void *)&bpctl_cmd, sizeof(struct bpctl_cmd))) {
+			/*unlock_bpctl();   */
+			/*preempt_enable(); */
+			ret = -EFAULT;
+			goto bp_exit;
+		}
+		goto bp_exit;
+
+	case IOCTL_TX_MSG(GET_BYPASS_SLAVE):
+		bpctl_cmd.status =
+		    get_bypass_slave_fn(pbpctl_dev, &pbpctl_dev_out);
+		if (bpctl_cmd.status == 1) {
+			bpctl_cmd.out_param[4] = pbpctl_dev_out->bus;
+			bpctl_cmd.out_param[5] = pbpctl_dev_out->slot;
+			bpctl_cmd.out_param[6] = pbpctl_dev_out->func;
+			bpctl_cmd.out_param[7] = pbpctl_dev_out->ifindex;
+		}
+		break;
+
+	case IOCTL_TX_MSG(IS_BYPASS):
+		bpctl_cmd.status = is_bypass(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_TX):
+		bpctl_cmd.status = set_tx_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+	case IOCTL_TX_MSG(GET_TX):
+		bpctl_cmd.status = get_tx_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_WD_AUTORESET):
+		bpctl_cmd.status =
+		    set_wd_autoreset_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+
+		break;
+	case IOCTL_TX_MSG(GET_WD_AUTORESET):
+
+		bpctl_cmd.status = get_wd_autoreset_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_DISC):
+		bpctl_cmd.status =
+		    set_disc_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+	case IOCTL_TX_MSG(GET_DISC):
+		bpctl_cmd.status = get_disc_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(GET_DISC_CHANGE):
+		bpctl_cmd.status = get_disc_change_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_DIS_DISC):
+		bpctl_cmd.status =
+		    set_dis_disc_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+	case IOCTL_TX_MSG(GET_DIS_DISC):
+		bpctl_cmd.status = get_dis_disc_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_DISC_PWUP):
+		bpctl_cmd.status =
+		    set_disc_pwup_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+	case IOCTL_TX_MSG(GET_DISC_PWUP):
+		bpctl_cmd.status = get_disc_pwup_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(GET_BYPASS_INFO):
+
+		bpctl_cmd.status =
+		    get_bypass_info_fn(pbpctl_dev, (char *)&bpctl_cmd.data,
+				       (char *)&bpctl_cmd.out_param[4]);
+		break;
+
+	case IOCTL_TX_MSG(SET_TPL):
+		bpctl_cmd.status =
+		    set_tpl_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_TPL):
+		bpctl_cmd.status = get_tpl_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_BP_WAIT_AT_PWUP):
+		bpctl_cmd.status =
+		    set_bp_wait_at_pwup_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BP_WAIT_AT_PWUP):
+		bpctl_cmd.status = get_bp_wait_at_pwup_fn(pbpctl_dev);
+		break;
+	case IOCTL_TX_MSG(SET_BP_HW_RESET):
+		bpctl_cmd.status =
+		    set_bp_hw_reset_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BP_HW_RESET):
+		bpctl_cmd.status = get_bp_hw_reset_fn(pbpctl_dev);
+		break;
+#ifdef BP_SELF_TEST
+	case IOCTL_TX_MSG(SET_BP_SELF_TEST):
+		bpctl_cmd.status =
+		    set_bp_self_test_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+
+		break;
+	case IOCTL_TX_MSG(GET_BP_SELF_TEST):
+		bpctl_cmd.status = get_bp_self_test_fn(pbpctl_dev);
+		break;
+
+#endif
+#if 0
+	case IOCTL_TX_MSG(SET_DISC_PORT):
+		bpctl_cmd.status =
+		    set_disc_port_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_DISC_PORT):
+		bpctl_cmd.status = get_disc_port_fn(pbpctl_dev);
+		break;
+
+	case IOCTL_TX_MSG(SET_DISC_PORT_PWUP):
+		bpctl_cmd.status =
+		    set_disc_port_pwup_fn(pbpctl_dev, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_DISC_PORT_PWUP):
+		bpctl_cmd.status = get_disc_port_pwup_fn(pbpctl_dev);
+		break;
+#endif
+	case IOCTL_TX_MSG(SET_BP_FORCE_LINK):
+		bpctl_cmd.status =
+		    set_bp_force_link_fn(dev_idx, bpctl_cmd.in_param[2]);
+		break;
+
+	case IOCTL_TX_MSG(GET_BP_FORCE_LINK):
+		bpctl_cmd.status = get_bp_force_link_fn(dev_idx);
+		break;
+
+	default:
+		/*    unlock_bpctl(); */
+
+		ret = -EOPNOTSUPP;
+		/* preempt_enable(); */
+		/* rcu_read_unlock();*/
+		spin_unlock_irqrestore(&bpvm_lock, flags);
+		goto bp_exit;
+	}
+	/* unlock_bpctl();   */
+	/* preempt_enable(); */
+ bpcmd_exit:
+	/* rcu_read_unlock(); */
+	spin_unlock_irqrestore(&bpvm_lock, flags);
+	if (copy_to_user(argp, (void *)&bpctl_cmd, sizeof(struct bpctl_cmd)))
+		ret = -EFAULT;
+	ret = SUCCESS;
+ bp_exit:
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,30))
+	/* unlock_kernel(); */
+#endif
+	/* spin_unlock_irqrestore(&bpvm_lock, flags); */
+	unlock_bpctl();
+	/* unlock_kernel(); */
+	return ret;
+}
+
+struct file_operations Fops = {
+	.owner = THIS_MODULE,
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30))
+	.ioctl = device_ioctl,
+#else
+	.unlocked_ioctl = device_ioctl,
+#endif
+
+	.open = device_open,
+	.release = device_release,	/* a.k.a. close */
+};
+
+#ifndef PCI_DEVICE
+#define PCI_DEVICE(vend,dev) \
+	.vendor = (vend), .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID
+#endif
+
+#define SILICOM_E1000BP_ETHERNET_DEVICE(device_id) {\
+	PCI_DEVICE(SILICOM_VID, device_id)}
+
+typedef enum {
+	PXG2BPFI,
+	PXG2BPFIL,
+	PXG2BPFILX,
+	PXG2BPFILLX,
+	PXGBPI,
+	PXGBPIG,
+	PXG2TBFI,
+	PXG4BPI,
+	PXG4BPFI,
+	PEG4BPI,
+	PEG2BPI,
+	PEG4BPIN,
+	PEG2BPFI,
+	PEG2BPFILX,
+	PMCXG2BPFI,
+	PMCXG2BPFIN,
+	PEG4BPII,
+	PEG4BPFII,
+	PXG4BPFILX,
+	PMCXG2BPIN,
+	PMCXG4BPIN,
+	PXG2BISC1,
+	PEG2TBFI,
+	PXG2TBI,
+	PXG4BPFID,
+	PEG4BPFI,
+	PEG4BPIPT,
+	PXG6BPI,
+	PEG4BPIL,
+	PMCXG2BPIN2,
+	PMCXG4BPIN2,
+	PMCX2BPI,
+	PEG2BPFID,
+	PEG2BPFIDLX,
+	PMCX4BPI,
+	MEG2BPFILN,
+	MEG2BPFINX,
+	PEG4BPFILX,
+	PE10G2BPISR,
+	PE10G2BPILR,
+	MHIO8AD,
+	PE10G2BPICX4,
+	PEG2BPI5,
+	PEG6BPI,
+	PEG4BPFI5,
+	PEG4BPFI5LX,
+	MEG2BPFILXLN,
+	PEG2BPIX1,
+	MEG2BPFILXNX,
+	XE10G2BPIT,
+	XE10G2BPICX4,
+	XE10G2BPISR,
+	XE10G2BPILR,
+	PEG4BPIIO,
+	XE10G2BPIXR,
+	PE10GDBISR,
+	PE10GDBILR,
+	PEG2BISC6,
+	PEG6BPIFC,
+	PE10G2BPTCX4,
+	PE10G2BPTSR,
+	PE10G2BPTLR,
+	PE10G2BPTT,
+	PEG4BPI6,
+	PEG4BPFI6,
+	PEG4BPFI6LX,
+	PEG4BPFI6ZX,
+	PEG2BPI6,
+	PEG2BPFI6,
+	PEG2BPFI6LX,
+	PEG2BPFI6ZX,
+	PEG2BPFI6FLXM,
+	PEG4BPI6FC,
+	PEG4BPFI6FC,
+	PEG4BPFI6FCLX,
+	PEG4BPFI6FCZX,
+	PEG6BPI6,
+	PEG2BPI6SC6,
+	MEG2BPI6,
+	XEG2BPI6,
+	MEG4BPI6,
+	PEG2BPFI5,
+	PEG2BPFI5LX,
+	PXEG4BPFI,
+	M1EG2BPI6,
+	M1EG2BPFI6,
+	M1EG2BPFI6LX,
+	M1EG2BPFI6ZX,
+	M1EG4BPI6,
+	M1EG4BPFI6,
+	M1EG4BPFI6LX,
+	M1EG4BPFI6ZX,
+	M1EG6BPI6,
+	M1E2G4BPi80,
+	M1E2G4BPFi80,
+	M1E2G4BPFi80LX,
+	M1E2G4BPFi80ZX,
+	PE210G2SPI9,
+	M1E10G2BPI9CX4,
+	M1E10G2BPI9SR,
+	M1E10G2BPI9LR,
+	M1E10G2BPI9T,
+	PE210G2BPI9CX4,
+	PE210G2BPI9SR,
+	PE210G2BPI9LR,
+	PE210G2BPI9T,
+	M2EG2BPFI6,
+	M2EG2BPFI6LX,
+	M2EG2BPFI6ZX,
+	M2EG4BPI6,
+	M2EG4BPFI6,
+	M2EG4BPFI6LX,
+	M2EG4BPFI6ZX,
+	M2EG6BPI6,
+	PEG2DBI6,
+	PEG2DBFI6,
+	PEG2DBFI6LX,
+	PEG2DBFI6ZX,
+	PE2G4BPi80,
+	PE2G4BPFi80,
+	PE2G4BPFi80LX,
+	PE2G4BPFi80ZX,
+	PE2G4BPi80L,
+	M6E2G8BPi80A,
+
+	PE2G2BPi35,
+	PAC1200BPi35,
+	PE2G2BPFi35,
+	PE2G2BPFi35LX,
+	PE2G2BPFi35ZX,
+	PE2G4BPi35,
+	PE2G4BPi35L,
+	PE2G4BPFi35,
+	PE2G4BPFi35LX,
+	PE2G4BPFi35ZX,
+
+	PE2G6BPi35,
+	PE2G6BPi35CX,
+
+	PE2G2BPi80,
+	PE2G2BPFi80,
+	PE2G2BPFi80LX,
+	PE2G2BPFi80ZX,
+	M2E10G2BPI9CX4,
+	M2E10G2BPI9SR,
+	M2E10G2BPI9LR,
+	M2E10G2BPI9T,
+	M6E2G8BPi80,
+	PE210G2DBi9SR,
+	PE210G2DBi9SRRB,
+	PE210G2DBi9LR,
+	PE210G2DBi9LRRB,
+	PE310G4DBi940SR,
+	PE310G4BPi9T,
+	PE310G4BPi9SR,
+	PE310G4BPi9LR,
+	PE210G2BPi40,
+} board_t;
+
+typedef struct _bpmod_info_t {
+	unsigned int vendor;
+	unsigned int device;
+	unsigned int subvendor;
+	unsigned int subdevice;
+	unsigned int index;
+	char *bp_name;
+
+} bpmod_info_t;
+
+typedef struct _dev_desc {
+	char *name;
+} dev_desc_t;
+
+dev_desc_t dev_desc[] = {
+	{"Silicom Bypass PXG2BPFI-SD series adapter"},
+	{"Silicom Bypass PXG2BPFIL-SD series adapter"},
+	{"Silicom Bypass PXG2BPFILX-SD series adapter"},
+	{"Silicom Bypass PXG2BPFILLX-SD series adapter"},
+	{"Silicom Bypass PXG2BPI-SD series adapter"},
+	{"Silicom Bypass PXG2BPIG-SD series adapter"},
+	{"Silicom Bypass PXG2TBFI-SD series adapter"},
+	{"Silicom Bypass PXG4BPI-SD series adapter"},
+	{"Silicom Bypass PXG4BPFI-SD series adapter"},
+	{"Silicom Bypass PEG4BPI-SD series adapter"},
+	{"Silicom Bypass PEG2BPI-SD series adapter"},
+	{"Silicom Bypass PEG4BPIN-SD series adapter"},
+	{"Silicom Bypass PEG2BPFI-SD series adapter"},
+	{"Silicom Bypass PEG2BPFI-LX-SD series adapter"},
+	{"Silicom Bypass PMCX2BPFI-SD series adapter"},
+	{"Silicom Bypass PMCX2BPFI-N series adapter"},
+	{"Intel Bypass PEG2BPII series adapter"},
+	{"Intel Bypass PEG2BPFII series adapter"},
+	{"Silicom Bypass PXG4BPFILX-SD series adapter"},
+	{"Silicom Bypass PMCX2BPI-N series adapter"},
+	{"Silicom Bypass PMCX4BPI-N series adapter"},
+	{"Silicom Bypass PXG2BISC1-SD series adapter"},
+	{"Silicom Bypass PEG2TBFI-SD series adapter"},
+	{"Silicom Bypass PXG2TBI-SD series adapter"},
+	{"Silicom Bypass PXG4BPFID-SD series adapter"},
+	{"Silicom Bypass PEG4BPFI-SD series adapter"},
+	{"Silicom Bypass PEG4BPIPT-SD series adapter"},
+	{"Silicom Bypass PXG6BPI-SD series adapter"},
+	{"Silicom Bypass PEG4BPIL-SD series adapter"},
+	{"Silicom Bypass PMCX2BPI-N2 series adapter"},
+	{"Silicom Bypass PMCX4BPI-N2 series adapter"},
+	{"Silicom Bypass PMCX2BPI-SD series adapter"},
+	{"Silicom Bypass PEG2BPFID-SD series adapter"},
+	{"Silicom Bypass PEG2BPFIDLX-SD series adapter"},
+	{"Silicom Bypass PMCX4BPI-SD series adapter"},
+	{"Silicom Bypass MEG2BPFILN-SD series adapter"},
+	{"Silicom Bypass MEG2BPFINX-SD series adapter"},
+	{"Silicom Bypass PEG4BPFILX-SD series adapter"},
+	{"Silicom Bypass PE10G2BPISR-SD series adapter"},
+	{"Silicom Bypass PE10G2BPILR-SD series adapter"},
+	{"Silicom Bypass MHIO8AD-SD series adapter"},
+	{"Silicom Bypass PE10G2BPICX4-SD series adapter"},
+	{"Silicom Bypass PEG2BPI5-SD series adapter"},
+	{"Silicom Bypass PEG6BPI5-SD series adapter"},
+	{"Silicom Bypass PEG4BPFI5-SD series adapter"},
+	{"Silicom Bypass PEG4BPFI5LX-SD series adapter"},
+	{"Silicom Bypass MEG2BPFILXLN-SD series adapter"},
+	{"Silicom Bypass PEG2BPIX1-SD series adapter"},
+	{"Silicom Bypass MEG2BPFILXNX-SD series adapter"},
+	{"Silicom Bypass XE10G2BPIT-SD series adapter"},
+	{"Silicom Bypass XE10G2BPICX4-SD series adapter"},
+	{"Silicom Bypass XE10G2BPISR-SD series adapter"},
+	{"Silicom Bypass XE10G2BPILR-SD series adapter"},
+	{"Intel Bypass PEG2BPFII0 series adapter"},
+	{"Silicom Bypass XE10G2BPIXR series adapter"},
+	{"Silicom Bypass PE10G2DBISR series adapter"},
+	{"Silicom Bypass PEG2BI5SC6 series adapter"},
+	{"Silicom Bypass PEG6BPI5FC series adapter"},
+
+	{"Silicom Bypass PE10G2BPTCX4 series adapter"},
+	{"Silicom Bypass PE10G2BPTSR series adapter"},
+	{"Silicom Bypass PE10G2BPTLR series adapter"},
+	{"Silicom Bypass PE10G2BPTT series adapter"},
+	{"Silicom Bypass PEG4BPI6 series adapter"},
+	{"Silicom Bypass PEG4BPFI6 series adapter"},
+	{"Silicom Bypass PEG4BPFI6LX series adapter"},
+	{"Silicom Bypass PEG4BPFI6ZX series adapter"},
+	{"Silicom Bypass PEG2BPI6 series adapter"},
+	{"Silicom Bypass PEG2BPFI6 series adapter"},
+	{"Silicom Bypass PEG2BPFI6LX series adapter"},
+	{"Silicom Bypass PEG2BPFI6ZX series adapter"},
+	{"Silicom Bypass PEG2BPFI6FLXM series adapter"},
+	{"Silicom Bypass PEG4BPI6FC series adapter"},
+	{"Silicom Bypass PEG4BPFI6FC series adapter"},
+	{"Silicom Bypass PEG4BPFI6FCLX series adapter"},
+	{"Silicom Bypass PEG4BPFI6FCZX series adapter"},
+	{"Silicom Bypass PEG6BPI6 series adapter"},
+	{"Silicom Bypass PEG2BPI6SC6 series adapter"},
+	{"Silicom Bypass MEG2BPI6 series adapter"},
+	{"Silicom Bypass XEG2BPI6 series adapter"},
+	{"Silicom Bypass MEG4BPI6 series adapter"},
+	{"Silicom Bypass PEG2BPFI5-SD series adapter"},
+	{"Silicom Bypass PEG2BPFI5LX-SD series adapter"},
+	{"Silicom Bypass PXEG4BPFI-SD series adapter"},
+	{"Silicom Bypass MxEG2BPI6 series adapter"},
+	{"Silicom Bypass MxEG2BPFI6 series adapter"},
+	{"Silicom Bypass MxEG2BPFI6LX series adapter"},
+	{"Silicom Bypass MxEG2BPFI6ZX series adapter"},
+	{"Silicom Bypass MxEG4BPI6 series adapter"},
+	{"Silicom Bypass MxEG4BPFI6 series adapter"},
+	{"Silicom Bypass MxEG4BPFI6LX series adapter"},
+	{"Silicom Bypass MxEG4BPFI6ZX series adapter"},
+	{"Silicom Bypass MxEG6BPI6 series adapter"},
+	{"Silicom Bypass MxE2G4BPi80 series adapter"},
+	{"Silicom Bypass MxE2G4BPFi80 series adapter"},
+	{"Silicom Bypass MxE2G4BPFi80LX series adapter"},
+	{"Silicom Bypass MxE2G4BPFi80ZX series adapter"},
+
+	{"Silicom Bypass PE210G2SPI9 series adapter"},
+
+	{"Silicom Bypass MxE210G2BPI9CX4 series adapter"},
+	{"Silicom Bypass MxE210G2BPI9SR series adapter"},
+	{"Silicom Bypass MxE210G2BPI9LR series adapter"},
+	{"Silicom Bypass MxE210G2BPI9T series adapter"},
+
+	{"Silicom Bypass PE210G2BPI9CX4 series adapter"},
+	{"Silicom Bypass PE210G2BPI9SR series adapter"},
+	{"Silicom Bypass PE210G2BPI9LR series adapter"},
+	{"Silicom Bypass PE210G2BPI9T series adapter"},
+
+	{"Silicom Bypass M2EG2BPFI6 series adapter"},
+	{"Silicom Bypass M2EG2BPFI6LX series adapter"},
+	{"Silicom Bypass M2EG2BPFI6ZX series adapter"},
+	{"Silicom Bypass M2EG4BPI6 series adapter"},
+	{"Silicom Bypass M2EG4BPFI6 series adapter"},
+	{"Silicom Bypass M2EG4BPFI6LX series adapter"},
+	{"Silicom Bypass M2EG4BPFI6ZX series adapter"},
+	{"Silicom Bypass M2EG6BPI6 series adapter"},
+
+	{"Silicom Bypass PEG2DBI6    series adapter"},
+	{"Silicom Bypass PEG2DBFI6   series adapter"},
+	{"Silicom Bypass PEG2DBFI6LX series adapter"},
+	{"Silicom Bypass PEG2DBFI6ZX series adapter"},
+
+	{"Silicom Bypass PE2G4BPi80 series adapter"},
+	{"Silicom Bypass PE2G4BPFi80 series adapter"},
+	{"Silicom Bypass PE2G4BPFi80LX series adapter"},
+	{"Silicom Bypass PE2G4BPFi80ZX series adapter"},
+
+	{"Silicom Bypass PE2G4BPi80L series adapter"},
+	{"Silicom Bypass MxE2G8BPi80A series adapter"},
+
+	{"Silicom Bypass PE2G2BPi35 series adapter"},
+	{"Silicom Bypass PAC1200BPi35 series adapter"},
+	{"Silicom Bypass PE2G2BPFi35 series adapter"},
+	{"Silicom Bypass PE2G2BPFi35LX series adapter"},
+	{"Silicom Bypass PE2G2BPFi35ZX series adapter"},
+
+	{"Silicom Bypass PE2G4BPi35 series adapter"},
+	{"Silicom Bypass PE2G4BPi35L series adapter"},
+	{"Silicom Bypass PE2G4BPFi35 series adapter"},
+	{"Silicom Bypass PE2G4BPFi35LX series adapter"},
+	{"Silicom Bypass PE2G4BPFi35ZX series adapter"},
+
+	{"Silicom Bypass PE2G6BPi35 series adapter"},
+	{"Silicom Bypass PE2G6BPi35CX series adapter"},
+
+	{"Silicom Bypass PE2G2BPi80 series adapter"},
+	{"Silicom Bypass PE2G2BPFi80 series adapter"},
+	{"Silicom Bypass PE2G2BPFi80LX series adapter"},
+	{"Silicom Bypass PE2G2BPFi80ZX series adapter"},
+
+	{"Silicom Bypass M2E10G2BPI9CX4 series adapter"},
+	{"Silicom Bypass M2E10G2BPI9SR series adapter"},
+	{"Silicom Bypass M2E10G2BPI9LR series adapter"},
+	{"Silicom Bypass M2E10G2BPI9T series adapter"},
+	{"Silicom Bypass MxE2G8BPi80 series adapter"},
+	{"Silicom Bypass PE210G2DBi9SR series adapter"},
+	{"Silicom Bypass PE210G2DBi9SRRB series adapter"},
+	{"Silicom Bypass PE210G2DBi9LR series adapter"},
+	{"Silicom Bypass PE210G2DBi9LRRB series adapter"},
+	{"Silicom Bypass PE310G4DBi9-SR series adapter"},
+	{"Silicom Bypass PE310G4BPi9T series adapter"},
+	{"Silicom Bypass PE310G4BPi9SR series adapter"},
+	{"Silicom Bypass PE310G4BPi9LR series adapter"},
+	{"Silicom Bypass PE210G2BPi40T series adapter"},
+	{0},
+};
+
+static bpmod_info_t tx_ctl_pci_tbl[] = {
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG2BPFI_SSID, PXG2BPFI,
+	 "PXG2BPFI-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG2BPFIL_SSID, PXG2BPFIL,
+	 "PXG2BPFIL-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG2BPFILX_SSID, PXG2BPFILX,
+	 "PXG2BPFILX-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG2BPFILLX_SSID, PXG2BPFILLX,
+	 "PXG2BPFILLXSD"},
+	{0x8086, 0x1010, SILICOM_SVID, SILICOM_PXGBPI_SSID, PXGBPI,
+	 "PXG2BPI-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PXGBPIG_SSID, PXGBPIG,
+	 "PXG2BPIG-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG2TBFI_SSID, PXG2TBFI,
+	 "PXG2TBFI-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PXG4BPI_SSID, PXG4BPI,
+	 "PXG4BPI-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG4BPFI_SSID, PXG4BPFI,
+	 "PXG4BPFI-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG4BPFILX_SSID, PXG4BPFILX,
+	 "PXG4BPFILX-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PEG4BPI_SSID, PEG4BPI,
+	 "PEXG4BPI-SD"},
+	{0x8086, 0x105e, SILICOM_SVID, SILICOM_PEG2BPI_SSID, PEG2BPI,
+	 "PEG2BPI-SD"},
+	{0x8086, 0x105e, SILICOM_SVID, SILICOM_PEG4BPIN_SSID, PEG4BPIN,
+	 "PEG4BPI-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG2BPFI_SSID, PEG2BPFI,
+	 "PEG2BPFI-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG2BPFILX_SSID, PEG2BPFILX,
+	 "PEG2BPFILX-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PMCXG2BPFI_SSID, PMCXG2BPFI,
+	 "PMCX2BPFI-SD"},
+	{0x8086, 0x107a, NOKIA_PMCXG2BPFIN_SVID, NOKIA_PMCXG2BPFIN_SSID,
+	 PMCXG2BPFIN, "PMCX2BPFI-N"},
+	{0x8086, INTEL_PEG4BPII_PID, 0x8086, INTEL_PEG4BPII_SSID, PEG4BPII,
+	 "PEG4BPII"},
+	{0x8086, INTEL_PEG4BPIIO_PID, 0x8086, INTEL_PEG4BPIIO_SSID, PEG4BPIIO,
+	 "PEG4BPII0"},
+	{0x8086, INTEL_PEG4BPFII_PID, 0x8086, INTEL_PEG4BPFII_SSID, PEG4BPFII,
+	 "PEG4BPFII"},
+	{0x8086, 0x1079, NOKIA_PMCXG2BPFIN_SVID, NOKIA_PMCXG2BPIN_SSID,
+	 PMCXG2BPIN, "PMCX2BPI-N"},
+	{0x8086, 0x1079, NOKIA_PMCXG2BPFIN_SVID, NOKIA_PMCXG4BPIN_SSID,
+	 PMCXG4BPIN, "PMCX4BPI-N"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PXG2BISC1_SSID, PXG2BISC1,
+	 "PXG2BISC1-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG2TBFI_SSID, PEG2TBFI,
+	 "PEG2TBFI-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PXG2TBI_SSID, PXG2TBI,
+	 "PXG2TBI-SD"},
+	{0x8086, 0x107a, SILICOM_SVID, SILICOM_PXG4BPFID_SSID, PXG4BPFID,
+	 "PXG4BPFID-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG4BPFI_SSID, PEG4BPFI,
+	 "PEG4BPFI-SD"},
+	{0x8086, 0x105e, SILICOM_SVID, SILICOM_PEG4BPIPT_SSID, PEG4BPIPT,
+	 "PEG4BPIPT-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PXG6BPI_SSID, PXG6BPI,
+	 "PXG6BPI-SD"},
+	{0x8086, 0x10a7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPIL_SSID /*PCI_ANY_ID */ , PEG4BPIL, "PEG4BPIL-SD"},
+	{0x8086, 0x1079, NOKIA_PMCXG2BPFIN_SVID, NOKIA_PMCXG2BPIN2_SSID,
+	 PMCXG2BPIN2, "PMCX2BPI-N2"},
+	{0x8086, 0x1079, NOKIA_PMCXG2BPFIN_SVID, NOKIA_PMCXG4BPIN2_SSID,
+	 PMCXG4BPIN2, "PMCX4BPI-N2"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PMCX2BPI_SSID, PMCX2BPI,
+	 "PMCX2BPI-SD"},
+	{0x8086, 0x1079, SILICOM_SVID, SILICOM_PMCX4BPI_SSID, PMCX4BPI,
+	 "PMCX4BPI-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG2BPFID_SSID, PEG2BPFID,
+	 "PEG2BPFID-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG2BPFIDLX_SSID, PEG2BPFIDLX,
+	 "PEG2BPFIDLXSD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_MEG2BPFILN_SSID, MEG2BPFILN,
+	 "MEG2BPFILN-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_MEG2BPFINX_SSID, MEG2BPFINX,
+	 "MEG2BPFINX-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PEG4BPFILX_SSID, PEG4BPFILX,
+	 "PEG4BPFILX-SD"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID, SILICOM_PE10G2BPISR_SSID,
+	 PE10G2BPISR, "PE10G2BPISR"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID, SILICOM_PE10G2BPILR_SSID,
+	 PE10G2BPILR, "PE10G2BPILR"},
+	{0x8086, 0x10a9, SILICOM_SVID, SILICOM_MHIO8AD_SSID, MHIO8AD,
+	 "MHIO8AD-SD"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID, SILICOM_PE10G2BPICX4_SSID,
+	 PE10G2BPISR, "PE10G2BPICX4"},
+	{0x8086, 0x10a7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPI5_SSID /*PCI_ANY_ID */ , PEG2BPI5, "PEG2BPI5-SD"},
+	{0x8086, 0x10a7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG6BPI_SSID /*PCI_ANY_ID */ , PEG6BPI, "PEG6BPI5"},
+	{0x8086, 0x10a9, SILICOM_SVID /*PCI_ANY_ID */ , SILICOM_PEG4BPFI5_SSID,
+	 PEG4BPFI5, "PEG4BPFI5"},
+	{0x8086, 0x10a9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI5LX_SSID, PEG4BPFI5LX, "PEG4BPFI5LX"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_MEG2BPFILXLN_SSID, MEG2BPFILXLN,
+	 "MEG2BPFILXLN"},
+	{0x8086, 0x105e, SILICOM_SVID, SILICOM_PEG2BPIX1_SSID, PEG2BPIX1,
+	 "PEG2BPIX1-SD"},
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_MEG2BPFILXNX_SSID, MEG2BPFILXNX,
+	 "MEG2BPFILXNX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID, SILICOM_XE10G2BPIT_SSID, XE10G2BPIT,
+	 "XE10G2BPIT"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID, SILICOM_XE10G2BPICX4_SSID,
+	 XE10G2BPICX4, "XE10G2BPICX4"},
+	{0x8086, 0x10C6, SILICOM_SVID, SILICOM_XE10G2BPISR_SSID, XE10G2BPISR,
+	 "XE10G2BPISR"},
+	{0x8086, 0x10C6, SILICOM_SVID, SILICOM_XE10G2BPILR_SSID, XE10G2BPILR,
+	 "XE10G2BPILR"},
+	{0x8086, 0x10C6, NOKIA_XE10G2BPIXR_SVID, NOKIA_XE10G2BPIXR_SSID,
+	 XE10G2BPIXR, "XE10G2BPIXR"},
+	{0x8086, 0x10C6, SILICOM_SVID, SILICOM_PE10GDBISR_SSID, PE10GDBISR,
+	 "PE10G2DBISR"},
+	{0x8086, 0x10C6, SILICOM_SVID, SILICOM_PE10GDBILR_SSID, PE10GDBILR,
+	 "PE10G2DBILR"},
+	{0x8086, 0x10a7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BISC6_SSID /*PCI_ANY_ID */ , PEG2BISC6, "PEG2BI5SC6"},
+	{0x8086, 0x10a7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG6BPIFC_SSID /*PCI_ANY_ID */ , PEG6BPIFC, "PEG6BPI5FC"},
+
+	{BROADCOM_VID, BROADCOM_PE10G2_PID, SILICOM_SVID,
+	 SILICOM_PE10G2BPTCX4_SSID, PE10G2BPTCX4, "PE10G2BPTCX4"},
+	{BROADCOM_VID, BROADCOM_PE10G2_PID, SILICOM_SVID,
+	 SILICOM_PE10G2BPTSR_SSID, PE10G2BPTSR, "PE10G2BPTSR"},
+	{BROADCOM_VID, BROADCOM_PE10G2_PID, SILICOM_SVID,
+	 SILICOM_PE10G2BPTLR_SSID, PE10G2BPTLR, "PE10G2BPTLR"},
+	{BROADCOM_VID, BROADCOM_PE10G2_PID, SILICOM_SVID,
+	 SILICOM_PE10G2BPTT_SSID, PE10G2BPTT, "PE10G2BPTT"},
+
+	/* {BROADCOM_VID, BROADCOM_PE10G2_PID, PCI_ANY_ID, PCI_ANY_ID, PE10G2BPTCX4, "PE10G2BPTCX4"}, */
+
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPI6_SSID /*PCI_ANY_ID */ , PEG4BPI6, "PEG4BPI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6_SSID /*PCI_ANY_ID */ , PEG4BPFI6, "PEG4BPFI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6LX_SSID /*PCI_ANY_ID */ , PEG4BPFI6LX, "PEG4BPFI6LX"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6ZX_SSID /*PCI_ANY_ID */ , PEG4BPFI6ZX, "PEG4BPFI6ZX"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPI6_SSID /*PCI_ANY_ID */ , PEG2BPI6, "PEG2BPI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPFI6_SSID /*PCI_ANY_ID */ , PEG2BPFI6, "PEG2BPFI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPFI6LX_SSID /*PCI_ANY_ID */ , PEG2BPFI6LX, "PEG2BPFI6LX"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPFI6ZX_SSID /*PCI_ANY_ID */ , PEG2BPFI6ZX, "PEG2BPFI6ZX"},
+	{0x8086, 0x10e7, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPFI6FLXM_SSID /*PCI_ANY_ID */ , PEG2BPFI6FLXM,
+	 "PEG2BPFI6FLXM"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPI6FC_SSID /*PCI_ANY_ID */ , PEG4BPI6FC, "PEG4BPI6FC"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6FC_SSID /*PCI_ANY_ID */ , PEG4BPFI6FC, "PEG4BPFI6FC"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6FCLX_SSID /*PCI_ANY_ID */ , PEG4BPFI6FCLX,
+	 "PEG4BPFI6FCLX"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG4BPFI6FCZX_SSID /*PCI_ANY_ID */ , PEG4BPFI6FCZX,
+	 "PEG4BPFI6FCZX"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG6BPI6_SSID /*PCI_ANY_ID */ , PEG6BPI6, "PEG6BPI6"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPI6SC6_SSID /*PCI_ANY_ID */ , PEG2BPI6SC6,
+	 "PEG6BPI62SC6"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_MEG2BPI6_SSID /*PCI_ANY_ID */ , MEG2BPI6, "MEG2BPI6"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_XEG2BPI6_SSID /*PCI_ANY_ID */ , XEG2BPI6, "XEG2BPI6"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_MEG4BPI6_SSID /*PCI_ANY_ID */ , MEG4BPI6, "MEG4BPI6"},
+
+	{0x8086, 0x10a9, SILICOM_SVID /*PCI_ANY_ID */ , SILICOM_PEG2BPFI5_SSID,
+	 PEG2BPFI5, "PEG2BPFI5"},
+	{0x8086, 0x10a9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2BPFI5LX_SSID, PEG2BPFI5LX, "PEG2BPFI5LX"},
+
+	{0x8086, 0x105f, SILICOM_SVID, SILICOM_PXEG4BPFI_SSID, PXEG4BPFI,
+	 "PXEG4BPFI-SD"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG2BPI6_SSID /*PCI_ANY_ID */ , M1EG2BPI6, "MxEG2BPI6"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG2BPFI6_SSID /*PCI_ANY_ID */ , M1EG2BPFI6, "MxEG2BPFI6"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG2BPFI6LX_SSID /*PCI_ANY_ID */ , M1EG2BPFI6LX,
+	 "MxEG2BPFI6LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG2BPFI6ZX_SSID /*PCI_ANY_ID */ , M1EG2BPFI6ZX,
+	 "MxEG2BPFI6ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG4BPI6_SSID /*PCI_ANY_ID */ , M1EG4BPI6, "MxEG4BPI6"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG4BPFI6_SSID /*PCI_ANY_ID */ , M1EG4BPFI6, "MxEG4BPFI6"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG4BPFI6LX_SSID /*PCI_ANY_ID */ , M1EG4BPFI6LX,
+	 "MxEG4BPFI6LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG4BPFI6ZX_SSID /*PCI_ANY_ID */ , M1EG4BPFI6ZX,
+	 "MxEG4BPFI6ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1EG6BPI6_SSID /*PCI_ANY_ID */ , M1EG6BPI6, "MxEG6BPI6"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E2G4BPi80_SSID /*PCI_ANY_ID */ , M1E2G4BPi80, "MxE2G4BPi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E2G4BPFi80_SSID /*PCI_ANY_ID */ , M1E2G4BPFi80,
+	 "MxE2G4BPFi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E2G4BPFi80LX_SSID /*PCI_ANY_ID */ , M1E2G4BPFi80LX,
+	 "MxE2G4BPFi80LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E2G4BPFi80ZX_SSID /*PCI_ANY_ID */ , M1E2G4BPFi80ZX,
+	 "MxE2G4BPFi80ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG2BPFI6_SSID /*PCI_ANY_ID */ , M2EG2BPFI6, "M2EG2BPFI6"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG2BPFI6LX_SSID /*PCI_ANY_ID */ , M2EG2BPFI6LX,
+	 "M2EG2BPFI6LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG2BPFI6ZX_SSID /*PCI_ANY_ID */ , M2EG2BPFI6ZX,
+	 "M2EG2BPFI6ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG4BPI6_SSID /*PCI_ANY_ID */ , M2EG4BPI6, "M2EG4BPI6"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG4BPFI6_SSID /*PCI_ANY_ID */ , M2EG4BPFI6, "M2EG4BPFI6"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG4BPFI6LX_SSID /*PCI_ANY_ID */ , M2EG4BPFI6LX,
+	 "M2EG4BPFI6LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG4BPFI6ZX_SSID /*PCI_ANY_ID */ , M2EG4BPFI6ZX,
+	 "M2EG4BPFI6ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2EG6BPI6_SSID /*PCI_ANY_ID */ , M2EG6BPI6, "M2EG6BPI6"},
+
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2DBI6_SSID /*PCI_ANY_ID */ , PEG2DBI6, "PEG2DBI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2DBFI6_SSID /*PCI_ANY_ID */ , PEG2DBFI6, "PEG2DBFI6"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2DBFI6LX_SSID /*PCI_ANY_ID */ , PEG2DBFI6LX, "PEG2DBFI6LX"},
+	{0x8086, 0x10e6, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PEG2DBFI6ZX_SSID /*PCI_ANY_ID */ , PEG2DBFI6ZX, "PEG2DBFI6ZX"},
+
+	{0x8086, 0x10F9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE210G2DBi9SR_SSID, PE210G2DBi9SR, "PE210G2DBi9SR"},
+	{0x8086, 0x10F9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE210G2DBi9LR_SSID, PE210G2DBi9LR, "PE210G2DBi9LR"},
+	{0x8086, 0x10F9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE310G4DBi940SR_SSID, PE310G4DBi940SR, "PE310G4DBi9SR"},
+
+	{0x8086, 0x10Fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE310G4BPi9T_SSID, PE310G4BPi9T, "PE310G4BPi9T"},
+	{0x8086, 0x10Fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE310G4BPi9SR_SSID, PE310G4BPi9SR, "PE310G4BPi9SR"},
+	{0x8086, 0x10Fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE310G4BPi9LR_SSID, PE310G4BPi9LR, "PE310G4BPi9LR"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPi80_SSID /*PCI_ANY_ID */ , PE2G4BPi80, "PE2G4BPi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi80_SSID /*PCI_ANY_ID */ , PE2G4BPFi80, "PE2G4BPFi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi80LX_SSID /*PCI_ANY_ID */ , PE2G4BPFi80LX,
+	 "PE2G4BPFi80LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi80ZX_SSID /*PCI_ANY_ID */ , PE2G4BPFi80ZX,
+	 "PE2G4BPFi80ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPi80L_SSID /*PCI_ANY_ID */ , PE2G4BPi80L, "PE2G4BPi80L"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M6E2G8BPi80A_SSID /*PCI_ANY_ID */ , M6E2G8BPi80A,
+	 "MxE2G8BPi80A"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPi35_SSID /*PCI_ANY_ID */ , PE2G2BPi35, "PE2G2BPi35"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PAC1200BPi35_SSID /*PCI_ANY_ID */ , PAC1200BPi35,
+	 "PAC1200BPi35"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi35_SSID /*PCI_ANY_ID */ , PE2G2BPFi35, "PE2G2BPFi35"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi35LX_SSID /*PCI_ANY_ID */ , PE2G2BPFi35LX,
+	 "PE2G2BPFi35LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi35ZX_SSID /*PCI_ANY_ID */ , PE2G2BPFi35ZX,
+	 "PE2G2BPFi35ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPi35_SSID /*PCI_ANY_ID */ , PE2G4BPi35, "PE2G4BPi35"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPi35L_SSID /*PCI_ANY_ID */ , PE2G4BPi35L, "PE2G4BPi35L"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi35_SSID /*PCI_ANY_ID */ , PE2G4BPFi35, "PE2G4BPFi35"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi35LX_SSID /*PCI_ANY_ID */ , PE2G4BPFi35LX,
+	 "PE2G4BPFi35LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G4BPFi35ZX_SSID /*PCI_ANY_ID */ , PE2G4BPFi35ZX,
+	 "PE2G4BPFi35ZX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G6BPi35_SSID /*PCI_ANY_ID */ , PE2G6BPi35, "PE2G6BPi35"},
+
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa0, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa1, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa2, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa3, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa4, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa5, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa6, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa7, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa8, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaa9, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaaa, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaab, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaac, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaad, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaae, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaaf, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab0, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab1, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab2, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab3, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab4, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab5, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab6, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab7, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab8, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xab9, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xaba, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xabb, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xabc, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xabd, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xabe, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ , 0xabf, PE2G6BPi35CX,
+	 "PE2G6BPi35CX"},
+
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPi80_SSID /*PCI_ANY_ID */ , PE2G2BPi80, "PE2G2BPi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi80_SSID /*PCI_ANY_ID */ , PE2G2BPFi80, "PE2G2BPFi80"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi80LX_SSID /*PCI_ANY_ID */ , PE2G2BPFi80LX,
+	 "PE2G2BPFi80LX"},
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE2G2BPFi80ZX_SSID /*PCI_ANY_ID */ , PE2G2BPFi80ZX,
+	 "PE2G2BPFi80ZX"},
+
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_MEG2BPI6_SSID /*PCI_ANY_ID */ , MEG2BPI6, "MEG2BPI6"},
+	{0x8086, 0x10c9, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_XEG2BPI6_SSID /*PCI_ANY_ID */ , XEG2BPI6, "XEG2BPI6"},
+
+#if 0
+	{0x8086, 0x10fb, 0x8086, INTEL_PE210G2SPI9_SSID, PE210G2SPI9,
+	 "PE210G2SPI9"},
+#endif
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E10G2BPI9CX4_SSID /*PCI_ANY_ID */ , M1E10G2BPI9CX4,
+	 "MxE210G2BPI9CX4"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E10G2BPI9SR_SSID /*PCI_ANY_ID */ , M1E10G2BPI9SR,
+	 "MxE210G2BPI9SR"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E10G2BPI9LR_SSID /*PCI_ANY_ID */ , M1E10G2BPI9LR,
+	 "MxE210G2BPI9LR"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M1E10G2BPI9T_SSID /*PCI_ANY_ID */ , M1E10G2BPI9T,
+	 "MxE210G2BPI9T"},
+
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2E10G2BPI9CX4_SSID /*PCI_ANY_ID */ , M2E10G2BPI9CX4,
+	 "M2E10G2BPI9CX4"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2E10G2BPI9SR_SSID /*PCI_ANY_ID */ , M2E10G2BPI9SR,
+	 "M2E10G2BPI9SR"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2E10G2BPI9LR_SSID /*PCI_ANY_ID */ , M2E10G2BPI9LR,
+	 "M2E10G2BPI9LR"},
+	{0x8086, 0x10fb, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M2E10G2BPI9T_SSID /*PCI_ANY_ID */ , M2E10G2BPI9T,
+	 "M2E10G2BPI9T"},
+
+	{0x8086, 0x10fb, SILICOM_SVID, SILICOM_PE210G2BPI9CX4_SSID,
+	 PE210G2BPI9CX4, "PE210G2BPI9CX4"},
+	{0x8086, 0x10fb, SILICOM_SVID, SILICOM_PE210G2BPI9SR_SSID,
+	 PE210G2BPI9SR, "PE210G2BPI9SR"},
+	{0x8086, 0x10fb, SILICOM_SVID, SILICOM_PE210G2BPI9LR_SSID,
+	 PE210G2BPI9LR, "PE210G2BPI9LR"},
+	{0x8086, 0x10fb, SILICOM_SVID, SILICOM_PE210G2BPI9T_SSID, PE210G2BPI9T,
+	 "PE210G2BPI9T"},
+
+#if 0
+	{0x1374, 0x2c, SILICOM_SVID, SILICOM_PXG4BPI_SSID, PXG4BPI,
+	 "PXG4BPI-SD"},
+
+	{0x1374, 0x2d, SILICOM_SVID, SILICOM_PXG4BPFI_SSID, PXG4BPFI,
+	 "PXG4BPFI-SD"},
+
+	{0x1374, 0x3f, SILICOM_SVID, SILICOM_PXG2TBI_SSID, PXG2TBI,
+	 "PXG2TBI-SD"},
+
+	{0x1374, 0x3d, SILICOM_SVID, SILICOM_PXG2BISC1_SSID, PXG2BISC1,
+	 "PXG2BISC1-SD"},
+
+	{0x1374, 0x40, SILICOM_SVID, SILICOM_PEG4BPFI_SSID, PEG4BPFI,
+	 "PEG4BPFI-SD"},
+
+#ifdef BP_SELF_TEST
+	{0x1374, 0x28, SILICOM_SVID, 0x28, PXGBPI, "PXG2BPI-SD"},
+#endif
+#endif
+	{0x8086, PCI_ANY_ID, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_M6E2G8BPi80_SSID /*PCI_ANY_ID */ , M6E2G8BPi80, "MxE2G8BPi80"},
+	{0x8086, 0x1528, SILICOM_SVID /*PCI_ANY_ID */ ,
+	 SILICOM_PE210G2BPi40_SSID /*PCI_ANY_ID */ , PE210G2BPi40,
+	 "PE210G2BPi40T"},
+
+	/* required last entry */
+	{0,}
+};
+
+/*
+* Initialize the module - Register the character device
+*/
+
+static int __init bypass_init_module(void)
+{
+	int ret_val, idx, idx_dev = 0;
+	struct pci_dev *pdev1 = NULL;
+	unsigned long mmio_start, mmio_len;
+
+	printk(BP_MOD_DESCR " v" BP_MOD_VER "\n");
+	ret_val = register_chrdev(major_num, DEVICE_NAME, &Fops);
+	if (ret_val < 0) {
+		printk("%s failed with %d\n", DEVICE_NAME, ret_val);
+		return ret_val;
+	}
+	major_num = ret_val;	/* dynamic */
+	for (idx = 0; tx_ctl_pci_tbl[idx].vendor; idx++) {
+		while ((pdev1 = pci_get_subsys(tx_ctl_pci_tbl[idx].vendor,
+					       tx_ctl_pci_tbl[idx].device,
+					       tx_ctl_pci_tbl[idx].subvendor,
+					       tx_ctl_pci_tbl[idx].subdevice,
+					       pdev1))) {
+
+			device_num++;
+		}
+	}
+	if (!device_num) {
+		printk("No such device\n");
+		unregister_chrdev(major_num, DEVICE_NAME);
+		return -1;
+	}
+
+	bpctl_dev_arr = kmalloc((device_num) * sizeof(bpctl_dev_t), GFP_KERNEL);
+
+	if (!bpctl_dev_arr) {
+		printk("Allocation error\n");
+		unregister_chrdev(major_num, DEVICE_NAME);
+		return -1;
+	}
+	memset(bpctl_dev_arr, 0, ((device_num) * sizeof(bpctl_dev_t)));
+
+	pdev1 = NULL;
+	for (idx = 0; tx_ctl_pci_tbl[idx].vendor; idx++) {
+		while ((pdev1 = pci_get_subsys(tx_ctl_pci_tbl[idx].vendor,
+					       tx_ctl_pci_tbl[idx].device,
+					       tx_ctl_pci_tbl[idx].subvendor,
+					       tx_ctl_pci_tbl[idx].subdevice,
+					       pdev1))) {
+			bpctl_dev_arr[idx_dev].pdev = pdev1;
+
+			mmio_start = pci_resource_start(pdev1, 0);
+			mmio_len = pci_resource_len(pdev1, 0);
+
+			bpctl_dev_arr[idx_dev].desc =
+			    dev_desc[tx_ctl_pci_tbl[idx].index].name;
+			bpctl_dev_arr[idx_dev].name =
+			    tx_ctl_pci_tbl[idx].bp_name;
+			bpctl_dev_arr[idx_dev].device =
+			    tx_ctl_pci_tbl[idx].device;
+			bpctl_dev_arr[idx_dev].vendor =
+			    tx_ctl_pci_tbl[idx].vendor;
+			bpctl_dev_arr[idx_dev].subdevice =
+			    tx_ctl_pci_tbl[idx].subdevice;
+			bpctl_dev_arr[idx_dev].subvendor =
+			    tx_ctl_pci_tbl[idx].subvendor;
+			/* bpctl_dev_arr[idx_dev].pdev=pdev1; */
+			bpctl_dev_arr[idx_dev].func = PCI_FUNC(pdev1->devfn);
+			bpctl_dev_arr[idx_dev].slot = PCI_SLOT(pdev1->devfn);
+			bpctl_dev_arr[idx_dev].bus = pdev1->bus->number;
+			bpctl_dev_arr[idx_dev].mem_map =
+			    (unsigned long)ioremap(mmio_start, mmio_len);
+#ifdef BP_SYNC_FLAG
+			spin_lock_init(&bpctl_dev_arr[idx_dev].bypass_wr_lock);
+#endif
+			if (BP10G9_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
+				bpctl_dev_arr[idx_dev].bp_10g9 = 1;
+			if (BP10G_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
+				bpctl_dev_arr[idx_dev].bp_10g = 1;
+			if (PEG540_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice)) {
+
+				bpctl_dev_arr[idx_dev].bp_540 = 1;
+			}
+			if (PEGF5_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
+				bpctl_dev_arr[idx_dev].bp_fiber5 = 1;
+			if (PEG80_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
+				bpctl_dev_arr[idx_dev].bp_i80 = 1;
+			if (PEGF80_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
+				bpctl_dev_arr[idx_dev].bp_i80 = 1;
+			if ((bpctl_dev_arr[idx_dev].subdevice & 0xa00) == 0xa00)
+				bpctl_dev_arr[idx_dev].bp_i80 = 1;
+			if (BP10GB_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice)) {
+				if (bpctl_dev_arr[idx_dev].ifindex == 0) {
+					unregister_chrdev(major_num,
+							  DEVICE_NAME);
+					printk
+					    ("Please load network driver for %s adapter!\n",
+					     bpctl_dev_arr[idx_dev].name);
+					return -1;
+				}
+
+				if (bpctl_dev_arr[idx_dev].ndev) {
+					if (!
+					    (bpctl_dev_arr[idx_dev].ndev->
+					     flags & IFF_UP)) {
+						if (!
+						    (bpctl_dev_arr[idx_dev].
+						     ndev->flags & IFF_UP)) {
+							unregister_chrdev
+							    (major_num,
+							     DEVICE_NAME);
+							printk
+							    ("Please bring up network interfaces for %s adapter!\n",
+							     bpctl_dev_arr
+							     [idx_dev].name);
+							return -1;
+						}
+
+					}
+				}
+				bpctl_dev_arr[idx_dev].bp_10gb = 1;
+			}
+
+			if (!bpctl_dev_arr[idx_dev].bp_10g9) {
+
+				if (is_bypass_fn(&bpctl_dev_arr[idx_dev])) {
+					printk(KERN_INFO "%s found, ",
+					       bpctl_dev_arr[idx_dev].name);
+					if ((OLD_IF_SERIES
+					     (bpctl_dev_arr[idx_dev].subdevice))
+					    ||
+					    (INTEL_IF_SERIES
+					     (bpctl_dev_arr[idx_dev].
+					      subdevice)))
+						bpctl_dev_arr[idx_dev].
+						    bp_fw_ver = 0xff;
+					else
+						bpctl_dev_arr[idx_dev].
+						    bp_fw_ver =
+						    bypass_fw_ver(&bpctl_dev_arr
+								  [idx_dev]);
+					if ((bpctl_dev_arr[idx_dev].bp_10gb ==
+					     1)
+					    && (bpctl_dev_arr[idx_dev].
+						bp_fw_ver == 0xff)) {
+						int cnt = 100;
+						while (cnt--) {
+							iounmap((void
+								 *)
+								(bpctl_dev_arr
+								 [idx_dev].
+								 mem_map));
+							mmio_start =
+							    pci_resource_start
+							    (pdev1, 0);
+							mmio_len =
+							    pci_resource_len
+							    (pdev1, 0);
+
+							bpctl_dev_arr[idx_dev].
+							    mem_map =
+							    (unsigned long)
+							    ioremap(mmio_start,
+								    mmio_len);
+
+							bpctl_dev_arr[idx_dev].
+							    bp_fw_ver =
+							    bypass_fw_ver
+							    (&bpctl_dev_arr
+							     [idx_dev]);
+							if (bpctl_dev_arr
+							    [idx_dev].
+							    bp_fw_ver == 0xa8)
+								break;
+
+						}
+					}
+					/* bpctl_dev_arr[idx_dev].bp_fw_ver=0xa8; */
+					printk("firmware version: 0x%x\n",
+					       bpctl_dev_arr[idx_dev].
+					       bp_fw_ver);
+				}
+				bpctl_dev_arr[idx_dev].wdt_status =
+				    WDT_STATUS_UNKNOWN;
+				bpctl_dev_arr[idx_dev].reset_time = 0;
+				atomic_set(&bpctl_dev_arr[idx_dev].wdt_busy, 0);
+				bpctl_dev_arr[idx_dev].bp_status_un = 1;
+
+				bypass_caps_init(&bpctl_dev_arr[idx_dev]);
+
+				init_bypass_wd_auto(&bpctl_dev_arr[idx_dev]);
+				init_bypass_tpl_auto(&bpctl_dev_arr[idx_dev]);
+				if (NOKIA_SERIES
+				    (bpctl_dev_arr[idx_dev].subdevice))
+					reset_cont(&bpctl_dev_arr[idx_dev]);
+			}
+#ifdef BP_SELF_TEST
+			if ((bpctl_dev_arr[idx_dev].bp_tx_data =
+			     kmalloc(BPTEST_DATA_LEN, GFP_KERNEL))) {
+
+				memset(bpctl_dev_arr[idx_dev].bp_tx_data, 0x0,
+				       BPTEST_DATA_LEN);
+
+				memset(bpctl_dev_arr[idx_dev].bp_tx_data, 0xff,
+				       6);
+				memset(bpctl_dev_arr[idx_dev].bp_tx_data + 6,
+				       0x0, 1);
+				memset(bpctl_dev_arr[idx_dev].bp_tx_data + 7,
+				       0xaa, 5);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+				bpctl_dev_arr[idx_dev].bp_tx_data[12] =
+				    (ETH_P_BPTEST >> 8) & 0xff;
+				bpctl_dev_arr[idx_dev].bp_tx_data[13] =
+				    ETH_P_BPTEST & 0xff;
+#else
+				*(__be16 *) (bpctl_dev_arr[idx_dev].bp_tx_data +
+					     12) = htons(ETH_P_BPTEST);
+#endif
+
+			} else
+				printk("bp_ctl: Memory allocation error!\n");
+#endif
+			idx_dev++;
+
+		}
+	}
+	if_scan_init();
+
+	sema_init(&bpctl_sema, 1);
+	spin_lock_init(&bpvm_lock);
+	{
+
+		bpctl_dev_t *pbpctl_dev_c = NULL;
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if (bpctl_dev_arr[idx_dev].bp_10g9) {
+				pbpctl_dev_c =
+				    get_status_port_fn(&bpctl_dev_arr[idx_dev]);
+				if (is_bypass_fn(&bpctl_dev_arr[idx_dev])) {
+					printk(KERN_INFO "%s found, ",
+					       bpctl_dev_arr[idx_dev].name);
+					bpctl_dev_arr[idx_dev].bp_fw_ver =
+					    bypass_fw_ver(&bpctl_dev_arr
+							  [idx_dev]);
+					printk("firmware version: 0x%x\n",
+					       bpctl_dev_arr[idx_dev].
+					       bp_fw_ver);
+
+				}
+				bpctl_dev_arr[idx_dev].wdt_status =
+				    WDT_STATUS_UNKNOWN;
+				bpctl_dev_arr[idx_dev].reset_time = 0;
+				atomic_set(&bpctl_dev_arr[idx_dev].wdt_busy, 0);
+				bpctl_dev_arr[idx_dev].bp_status_un = 1;
+
+				bypass_caps_init(&bpctl_dev_arr[idx_dev]);
+
+				init_bypass_wd_auto(&bpctl_dev_arr[idx_dev]);
+				init_bypass_tpl_auto(&bpctl_dev_arr[idx_dev]);
+
+			}
+
+		}
+	}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+	inter_module_register("is_bypass_sd", THIS_MODULE, &is_bypass_sd);
+	inter_module_register("get_bypass_slave_sd", THIS_MODULE,
+			      &get_bypass_slave_sd);
+	inter_module_register("get_bypass_caps_sd", THIS_MODULE,
+			      &get_bypass_caps_sd);
+	inter_module_register("get_wd_set_caps_sd", THIS_MODULE,
+			      &get_wd_set_caps_sd);
+	inter_module_register("set_bypass_sd", THIS_MODULE, &set_bypass_sd);
+	inter_module_register("get_bypass_sd", THIS_MODULE, &get_bypass_sd);
+	inter_module_register("get_bypass_change_sd", THIS_MODULE,
+			      &get_bypass_change_sd);
+	inter_module_register("set_dis_bypass_sd", THIS_MODULE,
+			      &set_dis_bypass_sd);
+	inter_module_register("get_dis_bypass_sd", THIS_MODULE,
+			      &get_dis_bypass_sd);
+	inter_module_register("set_bypass_pwoff_sd", THIS_MODULE,
+			      &set_bypass_pwoff_sd);
+	inter_module_register("get_bypass_pwoff_sd", THIS_MODULE,
+			      &get_bypass_pwoff_sd);
+	inter_module_register("set_bypass_pwup_sd", THIS_MODULE,
+			      &set_bypass_pwup_sd);
+	inter_module_register("get_bypass_pwup_sd", THIS_MODULE,
+			      &get_bypass_pwup_sd);
+	inter_module_register("get_bypass_wd_sd", THIS_MODULE,
+			      &get_bypass_wd_sd);
+	inter_module_register("set_bypass_wd_sd", THIS_MODULE,
+			      &set_bypass_wd_sd);
+	inter_module_register("get_wd_expire_time_sd", THIS_MODULE,
+			      &get_wd_expire_time_sd);
+	inter_module_register("reset_bypass_wd_timer_sd", THIS_MODULE,
+			      &reset_bypass_wd_timer_sd);
+	inter_module_register("set_std_nic_sd", THIS_MODULE, &set_std_nic_sd);
+	inter_module_register("get_std_nic_sd", THIS_MODULE, &get_std_nic_sd);
+	inter_module_register("set_tx_sd", THIS_MODULE, &set_tx_sd);
+	inter_module_register("get_tx_sd", THIS_MODULE, &get_tx_sd);
+	inter_module_register("set_tpl_sd", THIS_MODULE, &set_tpl_sd);
+	inter_module_register("get_tpl_sd", THIS_MODULE, &get_tpl_sd);
+
+	inter_module_register("set_bp_hw_reset_sd", THIS_MODULE,
+			      &set_bp_hw_reset_sd);
+	inter_module_register("get_bp_hw_reset_sd", THIS_MODULE,
+			      &get_bp_hw_reset_sd);
+
+	inter_module_register("set_tap_sd", THIS_MODULE, &set_tap_sd);
+	inter_module_register("get_tap_sd", THIS_MODULE, &get_tap_sd);
+	inter_module_register("get_tap_change_sd", THIS_MODULE,
+			      &get_tap_change_sd);
+	inter_module_register("set_dis_tap_sd", THIS_MODULE, &set_dis_tap_sd);
+	inter_module_register("get_dis_tap_sd", THIS_MODULE, &get_dis_tap_sd);
+	inter_module_register("set_tap_pwup_sd", THIS_MODULE, &set_tap_pwup_sd);
+	inter_module_register("get_tap_pwup_sd", THIS_MODULE, &get_tap_pwup_sd);
+	inter_module_register("set_bp_disc_sd", THIS_MODULE, &set_bp_disc_sd);
+	inter_module_register("get_bp_disc_sd", THIS_MODULE, &get_bp_disc_sd);
+	inter_module_register("get_bp_disc_change_sd", THIS_MODULE,
+			      &get_bp_disc_change_sd);
+	inter_module_register("set_bp_dis_disc_sd", THIS_MODULE,
+			      &set_bp_dis_disc_sd);
+	inter_module_register("get_bp_dis_disc_sd", THIS_MODULE,
+			      &get_bp_dis_disc_sd);
+	inter_module_register("set_bp_disc_pwup_sd", THIS_MODULE,
+			      &set_bp_disc_pwup_sd);
+	inter_module_register("get_bp_disc_pwup_sd", THIS_MODULE,
+			      &get_bp_disc_pwup_sd);
+	inter_module_register("set_wd_exp_mode_sd", THIS_MODULE,
+			      &set_wd_exp_mode_sd);
+	inter_module_register("get_wd_exp_mode_sd", THIS_MODULE,
+			      &get_wd_exp_mode_sd);
+	inter_module_register("set_wd_autoreset_sd", THIS_MODULE,
+			      &set_wd_autoreset_sd);
+	inter_module_register("get_wd_autoreset_sd", THIS_MODULE,
+			      &get_wd_autoreset_sd);
+	inter_module_register("get_bypass_info_sd", THIS_MODULE,
+			      &get_bypass_info_sd);
+	inter_module_register("bp_if_scan_sd", THIS_MODULE, &bp_if_scan_sd);
+
+#endif
+	register_netdevice_notifier(&bp_notifier_block);
+#ifdef BP_PROC_SUPPORT
+	{
+		int i = 0;
+		/* unsigned long flags; */
+		/* rcu_read_lock(); */
+		bp_proc_create();
+		for (i = 0; i < device_num; i++) {
+			if (bpctl_dev_arr[i].ifindex) {
+				/* spin_lock_irqsave(&bpvm_lock, flags); */
+				bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
+				bypass_proc_create_dev_sd(&bpctl_dev_arr[i]);
+				/* spin_unlock_irqrestore(&bpvm_lock, flags); */
+			}
+
+		}
+		/* rcu_read_unlock(); */
+	}
+#endif
+
+	return 0;
+}
+
+/*
+* Cleanup - unregister the appropriate file from /proc
+*/
+static void __exit bypass_cleanup_module(void)
+{
+	int i;
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,23))
+	int ret;
+#endif
+	unregister_netdevice_notifier(&bp_notifier_block);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+	inter_module_unregister("is_bypass_sd");
+	inter_module_unregister("get_bypass_slave_sd");
+	inter_module_unregister("get_bypass_caps_sd");
+	inter_module_unregister("get_wd_set_caps_sd");
+	inter_module_unregister("set_bypass_sd");
+	inter_module_unregister("get_bypass_sd");
+	inter_module_unregister("get_bypass_change_sd");
+	inter_module_unregister("set_dis_bypass_sd");
+	inter_module_unregister("get_dis_bypass_sd");
+	inter_module_unregister("set_bypass_pwoff_sd");
+	inter_module_unregister("get_bypass_pwoff_sd");
+	inter_module_unregister("set_bypass_pwup_sd");
+	inter_module_unregister("get_bypass_pwup_sd");
+	inter_module_unregister("set_bypass_wd_sd");
+	inter_module_unregister("get_bypass_wd_sd");
+	inter_module_unregister("get_wd_expire_time_sd");
+	inter_module_unregister("reset_bypass_wd_timer_sd");
+	inter_module_unregister("set_std_nic_sd");
+	inter_module_unregister("get_std_nic_sd");
+	inter_module_unregister("set_tx_sd");
+	inter_module_unregister("get_tx_sd");
+	inter_module_unregister("set_tpl_sd");
+	inter_module_unregister("get_tpl_sd");
+	inter_module_unregister("set_tap_sd");
+	inter_module_unregister("get_tap_sd");
+	inter_module_unregister("get_tap_change_sd");
+	inter_module_unregister("set_dis_tap_sd");
+	inter_module_unregister("get_dis_tap_sd");
+	inter_module_unregister("set_tap_pwup_sd");
+	inter_module_unregister("get_tap_pwup_sd");
+	inter_module_unregister("set_bp_disc_sd");
+	inter_module_unregister("get_bp_disc_sd");
+	inter_module_unregister("get_bp_disc_change_sd");
+	inter_module_unregister("set_bp_dis_disc_sd");
+	inter_module_unregister("get_bp_dis_disc_sd");
+	inter_module_unregister("set_bp_disc_pwup_sd");
+	inter_module_unregister("get_bp_disc_pwup_sd");
+	inter_module_unregister("set_wd_exp_mode_sd");
+	inter_module_unregister("get_wd_exp_mode_sd");
+	inter_module_unregister("set_wd_autoreset_sd");
+	inter_module_unregister("get_wd_autoreset_sd");
+	inter_module_unregister("get_bypass_info_sd");
+	inter_module_unregister("bp_if_scan_sd");
+
+#endif
+
+	for (i = 0; i < device_num; i++) {
+		/* unsigned long flags; */
+#ifdef BP_PROC_SUPPORT
+/*	spin_lock_irqsave(&bpvm_lock, flags);
+	rcu_read_lock(); */
+		bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
+/*	spin_unlock_irqrestore(&bpvm_lock, flags);        
+	rcu_read_unlock(); */
+#endif
+		remove_bypass_wd_auto(&bpctl_dev_arr[i]);
+		bpctl_dev_arr[i].reset_time = 0;
+
+		remove_bypass_tpl_auto(&bpctl_dev_arr[i]);
+	}
+
+	/* unmap all devices */
+	for (i = 0; i < device_num; i++) {
+#ifdef BP_SELF_TEST
+		if (bpctl_dev_arr[i].bp_tx_data)
+			kfree(bpctl_dev_arr[i].bp_tx_data);
+#endif
+		iounmap((void *)(bpctl_dev_arr[i].mem_map));
+	}
+
+	/* free all devices space */
+	if (bpctl_dev_arr)
+		kfree(bpctl_dev_arr);
+
+/*
+* Unregister the device                             
+*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,23))
+	ret = unregister_chrdev(major_num, DEVICE_NAME);
+/*
+* If there's an error, report it
+*/
+	if (ret < 0)
+		printk("Error in module_unregister_chrdev: %d\n", ret);
+#else
+	unregister_chrdev(major_num, DEVICE_NAME);
+
+#endif
+}
+
+module_init(bypass_init_module);
+module_exit(bypass_cleanup_module);
+
+int is_bypass_sd(int ifindex)
+{
+	return is_bypass(get_dev_idx_p(ifindex));
+}
+
+int set_bypass_sd(int ifindex, int bypass_mode)
+{
+
+	return set_bypass_fn(get_dev_idx_p(ifindex), bypass_mode);
+}
+
+int get_bypass_sd(int ifindex)
+{
+
+	return get_bypass_fn(get_dev_idx_p(ifindex));
+}
+
+int get_bypass_change_sd(int ifindex)
+{
+
+	return get_bypass_change_fn(get_dev_idx_p(ifindex));
+}
+
+int set_dis_bypass_sd(int ifindex, int dis_param)
+{
+	return set_dis_bypass_fn(get_dev_idx_p(ifindex), dis_param);
+}
+
+int get_dis_bypass_sd(int ifindex)
+{
+
+	return get_dis_bypass_fn(get_dev_idx_p(ifindex));
+}
+
+int set_bypass_pwoff_sd(int ifindex, int bypass_mode)
+{
+	return set_bypass_pwoff_fn(get_dev_idx_p(ifindex), bypass_mode);
+
+}
+
+int get_bypass_pwoff_sd(int ifindex)
+{
+	return get_bypass_pwoff_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_bypass_pwup_sd(int ifindex, int bypass_mode)
+{
+	return set_bypass_pwup_fn(get_dev_idx_p(ifindex), bypass_mode);
+
+}
+
+int get_bypass_pwup_sd(int ifindex)
+{
+	return get_bypass_pwup_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_bypass_wd_sd(int if_index, int ms_timeout, int *ms_timeout_set)
+{
+	if ((is_bypass(get_dev_idx_p(if_index))) <= 0)
+		return BP_NOT_CAP;
+	*ms_timeout_set = set_bypass_wd_fn(get_dev_idx_p(if_index), ms_timeout);
+	return 0;
+}
+
+int get_bypass_wd_sd(int ifindex, int *timeout)
+{
+	return get_bypass_wd_fn(get_dev_idx_p(ifindex), timeout);
+
+}
+
+int get_wd_expire_time_sd(int ifindex, int *time_left)
+{
+	return get_wd_expire_time_fn(get_dev_idx_p(ifindex), time_left);
+}
+
+int reset_bypass_wd_timer_sd(int ifindex)
+{
+	return reset_bypass_wd_timer_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_wd_set_caps_sd(int ifindex)
+{
+	return get_wd_set_caps_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_std_nic_sd(int ifindex, int nic_mode)
+{
+	return set_std_nic_fn(get_dev_idx_p(ifindex), nic_mode);
+
+}
+
+int get_std_nic_sd(int ifindex)
+{
+	return get_std_nic_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_tap_sd(int ifindex, int tap_mode)
+{
+	return set_tap_fn(get_dev_idx_p(ifindex), tap_mode);
+
+}
+
+int get_tap_sd(int ifindex)
+{
+	return get_tap_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_tap_pwup_sd(int ifindex, int tap_mode)
+{
+	return set_tap_pwup_fn(get_dev_idx_p(ifindex), tap_mode);
+
+}
+
+int get_tap_pwup_sd(int ifindex)
+{
+	return get_tap_pwup_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_tap_change_sd(int ifindex)
+{
+	return get_tap_change_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_dis_tap_sd(int ifindex, int dis_param)
+{
+	return set_dis_tap_fn(get_dev_idx_p(ifindex), dis_param);
+
+}
+
+int get_dis_tap_sd(int ifindex)
+{
+	return get_dis_tap_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_bp_disc_sd(int ifindex, int disc_mode)
+{
+	return set_disc_fn(get_dev_idx_p(ifindex), disc_mode);
+
+}
+
+int get_bp_disc_sd(int ifindex)
+{
+	return get_disc_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_bp_disc_pwup_sd(int ifindex, int disc_mode)
+{
+	return set_disc_pwup_fn(get_dev_idx_p(ifindex), disc_mode);
+
+}
+
+int get_bp_disc_pwup_sd(int ifindex)
+{
+	return get_disc_pwup_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_bp_disc_change_sd(int ifindex)
+{
+	return get_disc_change_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_bp_dis_disc_sd(int ifindex, int dis_param)
+{
+	return set_dis_disc_fn(get_dev_idx_p(ifindex), dis_param);
+
+}
+
+int get_bp_dis_disc_sd(int ifindex)
+{
+	return get_dis_disc_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_wd_exp_mode_sd(int ifindex)
+{
+	return get_wd_exp_mode_fn(get_dev_idx_p(ifindex));
+}
+
+int set_wd_exp_mode_sd(int ifindex, int param)
+{
+	return set_wd_exp_mode_fn(get_dev_idx_p(ifindex), param);
+
+}
+
+int reset_cont_sd(int ifindex)
+{
+	return reset_cont_fn(get_dev_idx_p(ifindex));
+
+}
+
+int set_tx_sd(int ifindex, int tx_state)
+{
+	return set_tx_fn(get_dev_idx_p(ifindex), tx_state);
+
+}
+
+int set_tpl_sd(int ifindex, int tpl_state)
+{
+	return set_tpl_fn(get_dev_idx_p(ifindex), tpl_state);
+
+}
+
+int set_bp_hw_reset_sd(int ifindex, int status)
+{
+	return set_bp_hw_reset_fn(get_dev_idx_p(ifindex), status);
+
+}
+
+int set_wd_autoreset_sd(int ifindex, int param)
+{
+	return set_wd_autoreset_fn(get_dev_idx_p(ifindex), param);
+
+}
+
+int get_wd_autoreset_sd(int ifindex)
+{
+	return get_wd_autoreset_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_bypass_caps_sd(int ifindex)
+{
+	return get_bypass_caps_fn(get_dev_idx_p(ifindex));
+}
+
+int get_bypass_slave_sd(int ifindex)
+{
+	bpctl_dev_t *pbpctl_dev_out;
+	int ret = get_bypass_slave_fn(get_dev_idx_p(ifindex), &pbpctl_dev_out);
+	if (ret == 1)
+		return pbpctl_dev_out->ifindex;
+	return -1;
+
+}
+
+int get_tx_sd(int ifindex)
+{
+	return get_tx_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_tpl_sd(int ifindex)
+{
+	return get_tpl_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_bp_hw_reset_sd(int ifindex)
+{
+	return get_bp_hw_reset_fn(get_dev_idx_p(ifindex));
+
+}
+
+int get_bypass_info_sd(int ifindex, struct bp_info *bp_info)
+{
+	return get_bypass_info_fn(get_dev_idx_p(ifindex), bp_info->prod_name, &bp_info->fw_ver);
+}
+
+int bp_if_scan_sd(void)
+{
+	if_scan_init();
+	return 0;
+}
+
+EXPORT_SYMBOL_NOVERS(is_bypass_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_slave_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_caps_sd);
+EXPORT_SYMBOL_NOVERS(get_wd_set_caps_sd);
+EXPORT_SYMBOL_NOVERS(set_bypass_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_change_sd);
+EXPORT_SYMBOL_NOVERS(set_dis_bypass_sd);
+EXPORT_SYMBOL_NOVERS(get_dis_bypass_sd);
+EXPORT_SYMBOL_NOVERS(set_bypass_pwoff_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_pwoff_sd);
+EXPORT_SYMBOL_NOVERS(set_bypass_pwup_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_pwup_sd);
+EXPORT_SYMBOL_NOVERS(set_bypass_wd_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_wd_sd);
+EXPORT_SYMBOL_NOVERS(get_wd_expire_time_sd);
+EXPORT_SYMBOL_NOVERS(reset_bypass_wd_timer_sd);
+EXPORT_SYMBOL_NOVERS(set_std_nic_sd);
+EXPORT_SYMBOL_NOVERS(get_std_nic_sd);
+EXPORT_SYMBOL_NOVERS(set_tx_sd);
+EXPORT_SYMBOL_NOVERS(get_tx_sd);
+EXPORT_SYMBOL_NOVERS(set_tpl_sd);
+EXPORT_SYMBOL_NOVERS(get_tpl_sd);
+EXPORT_SYMBOL_NOVERS(set_bp_hw_reset_sd);
+EXPORT_SYMBOL_NOVERS(get_bp_hw_reset_sd);
+EXPORT_SYMBOL_NOVERS(set_tap_sd);
+EXPORT_SYMBOL_NOVERS(get_tap_sd);
+EXPORT_SYMBOL_NOVERS(get_tap_change_sd);
+EXPORT_SYMBOL_NOVERS(set_dis_tap_sd);
+EXPORT_SYMBOL_NOVERS(get_dis_tap_sd);
+EXPORT_SYMBOL_NOVERS(set_tap_pwup_sd);
+EXPORT_SYMBOL_NOVERS(get_tap_pwup_sd);
+EXPORT_SYMBOL_NOVERS(set_wd_exp_mode_sd);
+EXPORT_SYMBOL_NOVERS(get_wd_exp_mode_sd);
+EXPORT_SYMBOL_NOVERS(set_wd_autoreset_sd);
+EXPORT_SYMBOL_NOVERS(get_wd_autoreset_sd);
+EXPORT_SYMBOL_NOVERS(set_bp_disc_sd);
+EXPORT_SYMBOL_NOVERS(get_bp_disc_sd);
+EXPORT_SYMBOL_NOVERS(get_bp_disc_change_sd);
+EXPORT_SYMBOL_NOVERS(set_bp_dis_disc_sd);
+EXPORT_SYMBOL_NOVERS(get_bp_dis_disc_sd);
+EXPORT_SYMBOL_NOVERS(set_bp_disc_pwup_sd);
+EXPORT_SYMBOL_NOVERS(get_bp_disc_pwup_sd);
+EXPORT_SYMBOL_NOVERS(get_bypass_info_sd);
+EXPORT_SYMBOL_NOVERS(bp_if_scan_sd);
+
+#define BP_PROC_DIR "bypass"
+
+#define GPIO6_SET_ENTRY_SD           "gpio6_set"
+#define GPIO6_CLEAR_ENTRY_SD         "gpio6_clear"
+
+#define GPIO7_SET_ENTRY_SD           "gpio7_set"
+#define GPIO7_CLEAR_ENTRY_SD         "gpio7_clear"
+
+#define PULSE_SET_ENTRY_SD            "pulse_set"
+#define ZERO_SET_ENTRY_SD            "zero_set"
+#define PULSE_GET1_ENTRY_SD            "pulse_get1"
+#define PULSE_GET2_ENTRY_SD            "pulse_get2"
+
+#define CMND_ON_ENTRY_SD              "cmnd_on"
+#define CMND_OFF_ENTRY_SD             "cmnd_off"
+#define RESET_CONT_ENTRY_SD           "reset_cont"
+
+ /*COMMANDS*/
+#define BYPASS_INFO_ENTRY_SD     "bypass_info"
+#define BYPASS_SLAVE_ENTRY_SD    "bypass_slave"
+#define BYPASS_CAPS_ENTRY_SD     "bypass_caps"
+#define WD_SET_CAPS_ENTRY_SD     "wd_set_caps"
+#define BYPASS_ENTRY_SD          "bypass"
+#define BYPASS_CHANGE_ENTRY_SD   "bypass_change"
+#define BYPASS_WD_ENTRY_SD       "bypass_wd"
+#define WD_EXPIRE_TIME_ENTRY_SD  "wd_expire_time"
+#define RESET_BYPASS_WD_ENTRY_SD "reset_bypass_wd"
+#define DIS_BYPASS_ENTRY_SD      "dis_bypass"
+#define BYPASS_PWUP_ENTRY_SD     "bypass_pwup"
+#define BYPASS_PWOFF_ENTRY_SD     "bypass_pwoff"
+#define STD_NIC_ENTRY_SD         "std_nic"
+#define STD_NIC_ENTRY_SD         "std_nic"
+#define TAP_ENTRY_SD             "tap"
+#define TAP_CHANGE_ENTRY_SD      "tap_change"
+#define DIS_TAP_ENTRY_SD         "dis_tap"
+#define TAP_PWUP_ENTRY_SD        "tap_pwup"
+#define TWO_PORT_LINK_ENTRY_SD   "two_port_link"
+#define WD_EXP_MODE_ENTRY_SD     "wd_exp_mode"
+#define WD_AUTORESET_ENTRY_SD    "wd_autoreset"
+#define TPL_ENTRY_SD             "tpl"
+#define WAIT_AT_PWUP_ENTRY_SD    "wait_at_pwup"
+#define HW_RESET_ENTRY_SD        "hw_reset"
+#define DISC_ENTRY_SD             "disc"
+#define DISC_CHANGE_ENTRY_SD      "disc_change"
+#define DIS_DISC_ENTRY_SD         "dis_disc"
+#define DISC_PWUP_ENTRY_SD        "disc_pwup"
+static struct proc_dir_entry *bp_procfs_dir;
+
+static struct proc_dir_entry *proc_getdir(char *name,
+					  struct proc_dir_entry *proc_dir)
+{
+	struct proc_dir_entry *pde = proc_dir;
+
+	for (pde = pde->subdir; pde; pde = pde->next) {
+		if (pde->namelen && (strcmp(name, pde->name) == 0)) {
+			/* directory exists */
+			break;
+		}
+	}
+	if (pde == (struct proc_dir_entry *)0) {
+		/* create the directory */
+#if (LINUX_VERSION_CODE > 0x20300)
+		pde = proc_mkdir(name, proc_dir);
+#else
+		pde = create_proc_entry(name, S_IFDIR, proc_dir);
+#endif
+		if (pde == (struct proc_dir_entry *)0) {
+
+			return pde;
+		}
+	}
+
+	return pde;
+}
+
+int bp_proc_create(void)
+{
+	bp_procfs_dir = proc_getdir(BP_PROC_DIR, init_net.proc_net);
+	if (bp_procfs_dir == (struct proc_dir_entry *)0) {
+		printk(KERN_DEBUG
+		       "Could not create procfs nicinfo directory %s\n",
+		       BP_PROC_DIR);
+		return -1;
+	}
+	return 0;
+}
+
+int
+bypass_proc_create_entry_sd(struct pfs_unit_sd *pfs_unit_curr,
+			    char *proc_name,
+			    write_proc_t *write_proc,
+			    read_proc_t *read_proc,
+			    struct proc_dir_entry *parent_pfs, void *data)
+{
+	strcpy(pfs_unit_curr->proc_name, proc_name);
+	pfs_unit_curr->proc_entry = create_proc_entry(pfs_unit_curr->proc_name,
+						      S_IFREG | S_IRUSR |
+						      S_IWUSR | S_IRGRP |
+						      S_IROTH, parent_pfs);
+	if (pfs_unit_curr->proc_entry == NULL)
+		return -1;
+
+	pfs_unit_curr->proc_entry->read_proc = read_proc;
+	pfs_unit_curr->proc_entry->write_proc = write_proc;
+	pfs_unit_curr->proc_entry->data = data;
+
+	return 0;
+
+}
+
+int
+get_bypass_info_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	int len = 0;
+
+	len += sprintf(page, "Name\t\t\t%s\n", pbp_device_block->name);
+	len +=
+	    sprintf(page + len, "Firmware version\t0x%x\n",
+		    pbp_device_block->bp_fw_ver);
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_slave_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0;
+	bpctl_dev_t *pbp_device_block_slave = NULL;
+	int idx_dev = 0;
+	struct net_device *net_slave_dev = NULL;
+
+	if ((pbp_device_block->func == 0) || (pbp_device_block->func == 2)) {
+		for (idx_dev = 0;
+		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
+		      && (idx_dev < device_num)); idx_dev++) {
+			if ((bpctl_dev_arr[idx_dev].bus ==
+			     pbp_device_block->bus)
+			    && (bpctl_dev_arr[idx_dev].slot ==
+				pbp_device_block->slot)) {
+				if ((pbp_device_block->func == 0)
+				    && (bpctl_dev_arr[idx_dev].func == 1)) {
+					pbp_device_block_slave =
+					    &bpctl_dev_arr[idx_dev];
+					break;
+				}
+				if ((pbp_device_block->func == 2) &&
+				    (bpctl_dev_arr[idx_dev].func == 3)) {
+					pbp_device_block_slave =
+					    &bpctl_dev_arr[idx_dev];
+					break;
+				}
+			}
+		}
+	} else
+		pbp_device_block_slave = pbp_device_block;
+	if (!pbp_device_block_slave) {
+		len = sprintf(page, "fail\n");
+		*eof = 1;
+		return len;
+	}
+	net_slave_dev = pbp_device_block_slave->ndev;
+	if (net_slave_dev) {
+		if (net_slave_dev)
+			len = sprintf(page, "%s\n", net_slave_dev->name);
+		else
+			len = sprintf(page, "fail\n");
+
+	}
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_caps_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_caps_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "-1\n");
+	else
+		len = sprintf(page, "0x%x\n", ret);
+	*eof = 1;
+	return len;
+
+}
+
+int
+get_wd_set_caps_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_set_caps_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "-1\n");
+	else
+		len = sprintf(page, "0x%x\n", ret);
+	*eof = 1;
+	return len;
+}
+
+int
+set_bypass_pfs(struct file *file, const char *buffer,
+	       unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_tap_pfs(struct file *file, const char *buffer,
+	    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_tap_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_disc_pfs(struct file *file, const char *buffer,
+	     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_disc_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_bypass_pfs(char *page, char **start, off_t off, int count,
+	       int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_pfs(char *page, char **start, off_t off, int count,
+	    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_pfs(char *page, char **start, off_t off, int count,
+	     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_change_pfs(char *page, char **start, off_t off, int count,
+		      int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_change_pfs(char *page, char **start, off_t off, int count,
+		   int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_change_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+#define isdigit(c) (c >= '0' && c <= '9')
+__inline static int atoi(char **s)
+{
+	int i = 0;
+	while (isdigit(**s))
+		i = i * 10 + *((*s)++) - '0';
+	return i;
+}
+
+int
+set_bypass_wd_pfs(struct file *file, const char *buffer,
+		  unsigned long count, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	int timeout;
+	int ret;
+
+	ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+	if (ret)
+		return ret;
+	set_bypass_wd_fn(pbp_device_block, timeout);
+
+	return count;
+}
+
+int
+get_bypass_wd_pfs(char *page, char **start, off_t off, int count,
+		  int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0, timeout = 0;
+
+	ret = get_bypass_wd_fn(pbp_device_block, &timeout);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (timeout == -1)
+		len = sprintf(page, "unknown\n");
+	else if (timeout == 0)
+		len = sprintf(page, "disable\n");
+	else
+		len = sprintf(page, "%d\n", timeout);
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_wd_expire_time_pfs(char *page, char **start, off_t off, int count,
+		       int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0, timeout = 0;
+
+	ret = get_wd_expire_time_fn(pbp_device_block, &timeout);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (timeout == -1)
+		len = sprintf(page, "expire\n");
+	else if (timeout == 0)
+		len = sprintf(page, "disable\n");
+
+	else
+		len = sprintf(page, "%d\n", timeout);
+	*eof = 1;
+	return len;
+}
+
+int
+get_tpl_pfs(char *page, char **start, off_t off, int count,
+	    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tpl_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+#ifdef PMC_FIX_FLAG
+int
+get_wait_at_pwup_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bp_wait_at_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_hw_reset_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bp_hw_reset_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+#endif				/*PMC_WAIT_FLAG */
+
+int
+reset_bypass_wd_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = reset_bypass_wd_timer_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "disable\n");
+	else if (ret == 1)
+		len = sprintf(page, "success\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_dis_bypass_pfs(struct file *file, const char *buffer,
+		   unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_dis_bypass_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_dis_tap_pfs(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_dis_tap_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_dis_disc_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_dis_disc_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_dis_bypass_pfs(char *page, char **start, off_t off, int count,
+		   int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_bypass_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_dis_tap_pfs(char *page, char **start, off_t off, int count,
+		int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_tap_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_dis_disc_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_disc_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_bypass_pwup_pfs(struct file *file, const char *buffer,
+		    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_pwup_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_bypass_pwoff_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_pwoff_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_tap_pwup_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_tap_pwup_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_disc_pwup_pfs(struct file *file, const char *buffer,
+		  unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_disc_pwup_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_bypass_pwup_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_pwoff_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_pwoff_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_pwup_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_pwup_pfs(char *page, char **start, off_t off, int count,
+		  int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_std_nic_pfs(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_std_nic_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+get_std_nic_pfs(char *page, char **start, off_t off, int count,
+		int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_std_nic_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_wd_exp_mode_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_exp_mode_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "tap\n");
+	else if (ret == 0)
+		len = sprintf(page, "bypass\n");
+	else if (ret == 2)
+		len = sprintf(page, "disc\n");
+
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_wd_exp_mode_pfs(struct file *file, const char *buffer,
+		    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "tap") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "bypass") == 0)
+		bypass_param = 0;
+	else if (strcmp(kbuf, "disc") == 0)
+		bypass_param = 2;
+
+	set_wd_exp_mode_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+get_wd_autoreset_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_autoreset_fn(pbp_device_block);
+	if (ret >= 0)
+		len = sprintf(page, "%d\n", ret);
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_wd_autoreset_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	int timeout;
+	int ret;
+
+	ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+	if (ret)
+		return ret;
+	set_wd_autoreset_fn(pbp_device_block, timeout);
+
+	return count;
+}
+
+int
+set_tpl_pfs(struct file *file, const char *buffer,
+	    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_tpl_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+#ifdef PMC_FIX_FLAG
+int
+set_wait_at_pwup_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_bp_wait_at_pwup_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+int
+set_hw_reset_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_bp_hw_reset_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+#endif				/*PMC_FIX_FLAG */
+
+int bypass_proc_create_dev_sd(bpctl_dev_t *pbp_device_block)
+{
+	struct bypass_pfs_sd *current_pfs = &(pbp_device_block->bypass_pfs_set);
+	static struct proc_dir_entry *procfs_dir = NULL;
+	int ret = 0;
+
+	if (!pbp_device_block->ndev)
+		return -1;
+	sprintf(current_pfs->dir_name, "bypass_%s",
+		pbp_device_block->ndev->name);
+
+	if (!bp_procfs_dir)
+		return -1;
+
+	/* create device proc dir */
+	procfs_dir = proc_getdir(current_pfs->dir_name, bp_procfs_dir);
+	if (procfs_dir == 0) {
+		printk(KERN_DEBUG "Could not create procfs directory %s\n",
+		       current_pfs->dir_name);
+		return -1;
+	}
+	current_pfs->bypass_entry = procfs_dir;
+
+	if (bypass_proc_create_entry_sd(&(current_pfs->bypass_info), BYPASS_INFO_ENTRY_SD, NULL,	/* write */
+					get_bypass_info_pfs,	/* read  */
+					procfs_dir, pbp_device_block))
+		ret = -1;
+
+	if (pbp_device_block->bp_caps & SW_CTL_CAP) {
+
+		/* Create set param proc's */
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_slave), BYPASS_SLAVE_ENTRY_SD, NULL,	/* write */
+						get_bypass_slave_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_caps), BYPASS_CAPS_ENTRY_SD, NULL,	/* write */
+						get_bypass_caps_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_set_caps), WD_SET_CAPS_ENTRY_SD, NULL,	/* write */
+						get_wd_set_caps_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_wd), BYPASS_WD_ENTRY_SD, set_bypass_wd_pfs,	/* write */
+						get_bypass_wd_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_expire_time), WD_EXPIRE_TIME_ENTRY_SD, NULL,	/* write */
+						get_wd_expire_time_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->reset_bypass_wd), RESET_BYPASS_WD_ENTRY_SD, NULL,	/* write */
+						reset_bypass_wd_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->std_nic), STD_NIC_ENTRY_SD, set_std_nic_pfs,	/* write */
+						get_std_nic_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (pbp_device_block->bp_caps & BP_CAP) {
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass), BYPASS_ENTRY_SD, set_bypass_pfs,	/* write */
+							get_bypass_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_bypass), DIS_BYPASS_ENTRY_SD, set_dis_bypass_pfs,	/* write */
+							get_dis_bypass_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwup), BYPASS_PWUP_ENTRY_SD, set_bypass_pwup_pfs,	/* write */
+							get_bypass_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwoff), BYPASS_PWOFF_ENTRY_SD, set_bypass_pwoff_pfs,	/* write */
+							get_bypass_pwoff_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_change), BYPASS_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_bypass_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+
+		if (pbp_device_block->bp_caps & TAP_CAP) {
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap), TAP_ENTRY_SD, set_tap_pfs,	/* write */
+							get_tap_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_TAP_ENTRY_SD, set_dis_tap_pfs,	/* write */
+							get_dis_tap_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), TAP_PWUP_ENTRY_SD, set_tap_pwup_pfs,	/* write */
+							get_tap_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), TAP_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_tap_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+		if (pbp_device_block->bp_caps & DISC_CAP) {
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap), DISC_ENTRY_SD, set_disc_pfs,	/* write */
+							get_disc_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+#if 1
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_DISC_ENTRY_SD, set_dis_disc_pfs,	/* write */
+							get_dis_disc_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+#endif
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), DISC_PWUP_ENTRY_SD, set_disc_pwup_pfs,	/* write */
+							get_disc_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), DISC_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_disc_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_exp_mode), WD_EXP_MODE_ENTRY_SD, set_wd_exp_mode_pfs,	/* write */
+						get_wd_exp_mode_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_autoreset), WD_AUTORESET_ENTRY_SD, set_wd_autoreset_pfs,	/* write */
+						get_wd_autoreset_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), TPL_ENTRY_SD, set_tpl_pfs,	/* write */
+						get_tpl_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+#ifdef PMC_FIX_FLAG
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), WAIT_AT_PWUP_ENTRY_SD, set_wait_at_pwup_pfs,	/* write */
+						get_wait_at_pwup_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), HW_RESET_ENTRY_SD, set_hw_reset_pfs,	/* write */
+						get_hw_reset_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+#endif
+
+	}
+	if (ret < 0)
+		printk(KERN_DEBUG "Create proc entry failed\n");
+
+	return ret;
+}
+
+int bypass_proc_remove_dev_sd(bpctl_dev_t *pbp_device_block)
+{
+
+	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
+	struct proc_dir_entry *pde = current_pfs->bypass_entry, *pde_curr =
+	    NULL;
+	char name[256];
+
+	if (!pde)
+		return 0;
+	for (pde = pde->subdir; pde;) {
+		strcpy(name, pde->name);
+		pde_curr = pde;
+		pde = pde->next;
+		remove_proc_entry(name, current_pfs->bypass_entry);
+	}
+	if (!pde)
+		remove_proc_entry(current_pfs->dir_name, bp_procfs_dir);
+	current_pfs->bypass_entry = NULL;
+
+	return 0;
+}
diff --git a/drivers/staging/silicom/bp_mod.h b/drivers/staging/silicom/bp_mod.h
new file mode 100644
index 000000000000..b8275f5611fa
--- /dev/null
+++ b/drivers/staging/silicom/bp_mod.h
@@ -0,0 +1,704 @@
+/******************************************************************************/
+/*                                                                            */
+/* Bypass Control utility, Copyright (c) 2005 Silicom                         */
+/*                                                                            */
+/* This program is free software; you can redistribute it and/or modify       */
+/* it under the terms of the GNU General Public License as published by       */
+/* the Free Software Foundation, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/* bp_mod.h                                                                   */
+/*                                                                            */
+/******************************************************************************/
+
+#ifndef BP_MOD_H
+#define BP_MOD_H
+#include "bits.h"
+
+#define EXPORT_SYMBOL_NOVERS EXPORT_SYMBOL
+
+#define usec_delay(x) udelay(x)
+#ifndef msec_delay_bp
+#define msec_delay_bp(x)			\
+do {						\
+	int  i;					\
+	if (1) {				\
+		for (i = 0; i < 1000; i++) {	\
+			udelay(x) ;		\
+		}				\
+	} else {				\
+		msleep(x);			\
+	}					\
+} while (0)
+
+#endif
+
+#include <linux/param.h>
+
+#ifndef jiffies_to_msecs
+#define jiffies_to_msecs(x) _kc_jiffies_to_msecs(x)
+static inline unsigned int jiffies_to_msecs(const unsigned long j)
+{
+#if HZ <= 1000 && !(1000 % HZ)
+	return (1000 / HZ) * j;
+#elif HZ > 1000 && !(HZ % 1000)
+	return (j + (HZ / 1000) - 1) / (HZ / 1000);
+#else
+	return (j * 1000) / HZ;
+#endif
+}
+#endif
+
+#define SILICOM_VID              0x1374
+#define SILICOM_SVID             0x1374
+
+#define SILICOM_PXG2BPFI_SSID    0x0026
+#define SILICOM_PXG2BPFILX_SSID  0x0027
+#define SILICOM_PXGBPI_SSID      0x0028
+#define SILICOM_PXGBPIG_SSID     0x0029
+#define SILICOM_PXG2TBFI_SSID    0x002a
+#define SILICOM_PXG4BPI_SSID     0x002c
+#define SILICOM_PXG4BPFI_SSID    0x002d
+#define SILICOM_PXG4BPFILX_SSID  0x002e
+#define SILICOM_PXG2BPFIL_SSID   0x002F
+#define SILICOM_PXG2BPFILLX_SSID 0x0030
+#define SILICOM_PEG4BPI_SSID     0x0031
+#define SILICOM_PEG2BPI_SSID     0x0037
+#define SILICOM_PEG4BPIN_SSID    0x0038
+#define SILICOM_PEG2BPFI_SSID    0x0039
+#define SILICOM_PEG2BPFILX_SSID  0x003A
+#define SILICOM_PMCXG2BPFI_SSID  0x003B
+#define NOKIA_PMCXG2BPFIN_SSID   0x0510
+#define NOKIA_PMCXG2BPIN_SSID    0x0513
+#define NOKIA_PMCXG4BPIN_SSID    0x0514
+#define NOKIA_PMCXG2BPFIN_SVID   0x13B8
+#define NOKIA_PMCXG2BPIN2_SSID    0x0515
+#define NOKIA_PMCXG4BPIN2_SSID    0x0516
+#define SILICOM_PMCX2BPI_SSID       0x041
+#define SILICOM_PMCX4BPI_SSID       0x042
+#define SILICOM_PXG2BISC1_SSID   0x003d
+#define SILICOM_PEG2TBFI_SSID    0x003E
+#define SILICOM_PXG2TBI_SSID     0x003f
+#define SILICOM_PXG4BPFID_SSID   0x0043
+#define SILICOM_PEG4BPFI_SSID    0x0040
+#define SILICOM_PEG4BPIPT_SSID   0x0044
+#define SILICOM_PXG6BPI_SSID     0x0045
+#define SILICOM_PEG4BPIL_SSID    0x0046
+#define SILICOM_PEG2BPI5_SSID    0x0052
+#define SILICOM_PEG6BPI_SSID    0x0053
+#define SILICOM_PEG4BPFI5_SSID   0x0050
+#define SILICOM_PEG4BPFI5LX_SSID   0x0051
+#define SILICOM_PEG2BISC6_SSID 0x54
+
+#define SILICOM_PEG6BPIFC_SSID 0x55
+
+#define SILICOM_PEG2BPFI5_SSID   0x0056
+#define SILICOM_PEG2BPFI5LX_SSID   0x0057
+
+#define SILICOM_PXEG4BPFI_SSID    0x0058
+
+#define SILICOM_PEG2BPFID_SSID   0x0047
+#define SILICOM_PEG2BPFIDLX_SSID  0x004C
+#define SILICOM_MEG2BPFILN_SSID  0x0048
+#define SILICOM_MEG2BPFINX_SSID  0x0049
+#define SILICOM_PEG4BPFILX_SSID  0x004A
+#define SILICOM_MHIO8AD_SSID    0x004F
+
+#define SILICOM_MEG2BPFILXLN_SSID 0x004b
+#define SILICOM_PEG2BPIX1_SSID    0x004d
+#define SILICOM_MEG2BPFILXNX_SSID 0x004e
+
+#define SILICOM_PE10G2BPISR_SSID  0x0102
+#define SILICOM_PE10G2BPILR_SSID  0x0103
+#define SILICOM_PE10G2BPICX4_SSID  0x0101
+
+#define SILICOM_XE10G2BPILR_SSID 0x0163
+#define SILICOM_XE10G2BPISR_SSID 0x0162
+#define SILICOM_XE10G2BPICX4_SSID 0x0161
+#define SILICOM_XE10G2BPIT_SSID   0x0160
+
+#define SILICOM_PE10GDBISR_SSID   0x0181
+#define SILICOM_PE10GDBILR_SSID   0x0182
+
+#define SILICOM_PE210G2DBi9SR_SSID	    0x0188
+#define SILICOM_PE210G2DBi9SRRB_SSID	0x0188
+#define SILICOM_PE210G2DBi9LR_SSID	    0x0189
+#define SILICOM_PE210G2DBi9LRRB_SSID	0x0189
+#define SILICOM_PE310G4DBi940SR_SSID	0x018C
+
+#define SILICOM_PE310G4BPi9T_SSID       0x130
+#define SILICOM_PE310G4BPi9SR_SSID        0x132
+#define SILICOM_PE310G4BPi9LR_SSID        0x133
+
+#define NOKIA_XE10G2BPIXR_SVID   0x13B8
+#define NOKIA_XE10G2BPIXR_SSID   0x051C
+
+#define INTEL_PEG4BPII_PID   0x10A0
+#define INTEL_PEG4BPFII_PID   0x10A1
+#define INTEL_PEG4BPII_SSID   0x11A0
+#define INTEL_PEG4BPFII_SSID   0x11A1
+
+#define INTEL_PEG4BPIIO_SSID   0x10A0
+#define INTEL_PEG4BPIIO_PID   0x105e
+
+#define BROADCOM_VID         0x14e4
+#define BROADCOM_PE10G2_PID  0x164e
+
+#define SILICOM_PE10G2BPTCX4_SSID 0x0141
+#define SILICOM_PE10G2BPTSR_SSID  0x0142
+#define SILICOM_PE10G2BPTLR_SSID  0x0143
+#define SILICOM_PE10G2BPTT_SSID   0x0140
+
+#define SILICOM_PEG4BPI6_SSID     0x0320
+#define SILICOM_PEG4BPFI6_SSID    0x0321
+#define SILICOM_PEG4BPFI6LX_SSID    0x0322
+#define SILICOM_PEG4BPFI6ZX_SSID    0x0323
+
+#define SILICOM_PEG2BPI6_SSID    0x0300
+#define SILICOM_PEG2BPFI6_SSID    0x0301
+#define SILICOM_PEG2BPFI6LX_SSID    0x0302
+#define SILICOM_PEG2BPFI6ZX_SSID    0x0303
+#define SILICOM_PEG2BPFI6FLXM_SSID  0x0304
+
+#define SILICOM_PEG2DBI6_SSID    0x0308
+#define SILICOM_PEG2DBFI6_SSID    0x0309
+#define SILICOM_PEG2DBFI6LX_SSID    0x030A
+#define SILICOM_PEG2DBFI6ZX_SSID    0x030B
+
+#define SILICOM_MEG2BPI6_SSID     0x0310
+#define SILICOM_XEG2BPI6_SSID    0x0318
+#define SILICOM_PEG4BPI6FC_SSID     0x0328
+#define SILICOM_PEG4BPFI6FC_SSID    0x0329
+#define SILICOM_PEG4BPFI6FCLX_SSID    0x032A
+#define SILICOM_PEG4BPFI6FCZX_SSID    0x032B
+
+#define SILICOM_PEG6BPI6_SSID     0x0340
+
+#define SILICOM_PEG2BPI6SC6_SSID     0x0360
+
+#define SILICOM_MEG2BPI6_SSID     0x0310
+#define SILICOM_XEG2BPI6_SSID     0x0318
+#define SILICOM_MEG4BPI6_SSID     0x0330
+
+#define SILICOM_PE2G4BPi80L_SSID    0x0380
+
+#define SILICOM_M6E2G8BPi80A_SSID    0x0474
+
+#define SILICOM_PE2G4BPi35_SSID    0x03d8
+
+#define SILICOM_PE2G4BPFi80_SSID    0x0381
+#define SILICOM_PE2G4BPFi80LX_SSID    0x0382
+#define SILICOM_PE2G4BPFi80ZX_SSID    0x0383
+
+#define SILICOM_PE2G4BPi80_SSID    0x0388
+
+#define SILICOM_PE2G2BPi80_SSID    0x0390
+#define SILICOM_PE2G2BPFi80_SSID    0x0391
+#define SILICOM_PE2G2BPFi80LX_SSID    0x0392
+#define SILICOM_PE2G2BPFi80ZX_SSID    0x0393
+
+#define SILICOM_PE2G4BPi35L_SSID    0x03D0
+#define SILICOM_PE2G4BPFi35_SSID    0x03D1
+#define SILICOM_PE2G4BPFi35LX_SSID    0x03D2
+#define SILICOM_PE2G4BPFi35ZX_SSID    0x03D3
+
+#define SILICOM_PE2G2BPi35_SSID    0x03c0
+#define SILICOM_PAC1200BPi35_SSID    0x03cc
+#define SILICOM_PE2G2BPFi35_SSID    0x03C1
+#define SILICOM_PE2G2BPFi35LX_SSID    0x03C2
+#define SILICOM_PE2G2BPFi35ZX_SSID    0x03C3
+
+#define SILICOM_PE2G6BPi35_SSID    0x03E0
+#define SILICOM_PE2G6BPi35CX_SSID  0x0AA0
+
+#define INTEL_PE210G2SPI9_SSID     0x00C
+
+#define SILICOM_M1EG2BPI6_SSID     0x400
+
+#define SILICOM_M1EG2BPFI6_SSID     0x0401
+#define SILICOM_M1EG2BPFI6LX_SSID     0x0402
+#define SILICOM_M1EG2BPFI6ZX_SSID     0x0403
+
+#define SILICOM_M1EG4BPI6_SSID     0x0420
+
+#define SILICOM_M1EG4BPFI6_SSID       0x0421
+#define SILICOM_M1EG4BPFI6LX_SSID     0x0422
+#define SILICOM_M1EG4BPFI6ZX_SSID     0x0423
+
+#define SILICOM_M1EG6BPI6_SSID     0x0440
+
+#define SILICOM_M1E2G4BPi80_SSID     0x0460
+#define SILICOM_M1E2G4BPFi80_SSID       0x0461
+#define SILICOM_M1E2G4BPFi80LX_SSID     0x0462
+#define SILICOM_M1E2G4BPFi80ZX_SSID     0x0463
+
+#define SILICOM_M6E2G8BPi80_SSID        0x0470
+#define SILICOM_PE210G2BPi40_SSID       0x01a0
+
+#define PEG540_IF_SERIES(pid) \
+	((pid == SILICOM_PE210G2BPi40_SSID))
+
+#define OLD_IF_SERIES(pid)\
+	((pid == SILICOM_PXG2BPFI_SSID) || \
+	 (pid == SILICOM_PXG2BPFILX_SSID))
+
+#define P2BPFI_IF_SERIES(pid) \
+	((pid == SILICOM_PXG2BPFI_SSID) || \
+	 (pid == SILICOM_PXG2BPFILX_SSID) || \
+	 (pid == SILICOM_PEG2BPFI_SSID) || \
+	 (pid == SILICOM_PEG2BPFID_SSID) || \
+	 (pid == SILICOM_PEG2BPFIDLX_SSID) || \
+	 (pid == SILICOM_MEG2BPFILN_SSID) || \
+	 (pid == SILICOM_MEG2BPFINX_SSID) || \
+	 (pid == SILICOM_PEG4BPFILX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI_SSID) || \
+	 (pid == SILICOM_PXEG4BPFI_SSID) || \
+	 (pid == SILICOM_PXG4BPFID_SSID) || \
+	 (pid == SILICOM_PEG2TBFI_SSID) || \
+	 (pid == SILICOM_PE10G2BPISR_SSID) || \
+	 (pid == SILICOM_PE10G2BPILR_SSID) || \
+	 (pid == SILICOM_PEG2BPFILX_SSID) || \
+	 (pid == SILICOM_PMCXG2BPFI_SSID) || \
+	 (pid == SILICOM_MHIO8AD_SSID) || \
+	 (pid == SILICOM_PEG4BPFI5LX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI5_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6FC_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6FCLX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6FCZX_SSID) || \
+	 (pid == NOKIA_PMCXG2BPFIN_SSID) || \
+	 (pid == SILICOM_MEG2BPFILXLN_SSID) || \
+	 (pid == SILICOM_MEG2BPFILXNX_SSID) || \
+	 (pid == SILICOM_XE10G2BPIT_SSID) || \
+	 (pid == SILICOM_XE10G2BPICX4_SSID) || \
+	 (pid == SILICOM_XE10G2BPISR_SSID) || \
+	 (pid == NOKIA_XE10G2BPIXR_SSID) || \
+	 (pid == SILICOM_PE10GDBISR_SSID) || \
+	 (pid == SILICOM_PE10GDBILR_SSID) || \
+	 (pid == SILICOM_XE10G2BPILR_SSID))
+
+#define INTEL_IF_SERIES(pid) \
+	((pid == INTEL_PEG4BPII_SSID) || \
+	 (pid == INTEL_PEG4BPIIO_SSID) || \
+	 (pid == INTEL_PEG4BPFII_SSID))
+
+#define NOKIA_SERIES(pid) \
+	((pid == NOKIA_PMCXG2BPIN_SSID) || \
+	 (pid == NOKIA_PMCXG4BPIN_SSID) || \
+	 (pid == SILICOM_PMCX4BPI_SSID) || \
+	 (pid == NOKIA_PMCXG2BPFIN_SSID) || \
+	 (pid == SILICOM_PMCXG2BPFI_SSID) || \
+	 (pid == NOKIA_PMCXG2BPIN2_SSID) || \
+	 (pid == NOKIA_PMCXG4BPIN2_SSID) || \
+	 (pid == SILICOM_PMCX2BPI_SSID))
+
+#define DISCF_IF_SERIES(pid) \
+	(pid == SILICOM_PEG2TBFI_SSID)
+
+#define PEGF_IF_SERIES(pid) \
+	((pid == SILICOM_PEG2BPFI_SSID) || \
+	 (pid == SILICOM_PEG2BPFID_SSID) || \
+	 (pid == SILICOM_PEG2BPFIDLX_SSID) || \
+	 (pid == SILICOM_PEG2BPFILX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI_SSID) || \
+	 (pid == SILICOM_PXEG4BPFI_SSID) || \
+	 (pid == SILICOM_MEG2BPFILN_SSID) || \
+	 (pid == SILICOM_MEG2BPFINX_SSID) || \
+	 (pid == SILICOM_PEG4BPFILX_SSID) || \
+	 (pid == SILICOM_PEG2TBFI_SSID) || \
+	 (pid == SILICOM_MEG2BPFILXLN_SSID) || \
+	 (pid == SILICOM_MEG2BPFILXNX_SSID))
+
+#define TPL_IF_SERIES(pid) \
+	((pid == SILICOM_PXG2BPFIL_SSID) || \
+	 (pid == SILICOM_PXG2BPFILLX_SSID) || \
+	 (pid == SILICOM_PXG2TBFI_SSID) || \
+	 (pid == SILICOM_PXG4BPFID_SSID) || \
+	 (pid == SILICOM_PXG4BPFI_SSID))
+
+#define BP10G_IF_SERIES(pid) \
+	((pid == SILICOM_PE10G2BPISR_SSID) || \
+	 (pid == SILICOM_PE10G2BPICX4_SSID) || \
+	 (pid == SILICOM_PE10G2BPILR_SSID) || \
+	 (pid == SILICOM_XE10G2BPIT_SSID) || \
+	 (pid == SILICOM_XE10G2BPICX4_SSID) || \
+	 (pid == SILICOM_XE10G2BPISR_SSID) || \
+	 (pid == NOKIA_XE10G2BPIXR_SSID) || \
+	 (pid == SILICOM_PE10GDBISR_SSID) || \
+	 (pid == SILICOM_PE10GDBILR_SSID) || \
+	 (pid == SILICOM_XE10G2BPILR_SSID))
+
+#define BP10GB_IF_SERIES(pid) \
+	((pid == SILICOM_PE10G2BPTCX4_SSID) || \
+	 (pid == SILICOM_PE10G2BPTSR_SSID) || \
+	 (pid == SILICOM_PE10G2BPTLR_SSID) || \
+	 (pid == SILICOM_PE10G2BPTT_SSID))
+
+#define BP10G_CX4_SERIES(pid) \
+	(pid == SILICOM_PE10G2BPICX4_SSID)
+
+#define BP10GB_CX4_SERIES(pid) \
+	(pid == SILICOM_PE10G2BPTCX4_SSID)
+
+#define SILICOM_M2EG2BPFI6_SSID       0x0501
+#define SILICOM_M2EG2BPFI6LX_SSID     0x0502
+#define SILICOM_M2EG2BPFI6ZX_SSID     0x0503
+#define SILICOM_M2EG4BPI6_SSID        0x0520
+
+#define SILICOM_M2EG4BPFI6_SSID       0x0521
+#define SILICOM_M2EG4BPFI6LX_SSID     0x0522
+#define SILICOM_M2EG4BPFI6ZX_SSID     0x0523
+
+#define SILICOM_M2EG6BPI6_SSID     0x0540
+
+#define SILICOM_M1E10G2BPI9CX4_SSID  0x481
+#define SILICOM_M1E10G2BPI9SR_SSID   0x482
+#define SILICOM_M1E10G2BPI9LR_SSID   0x483
+#define SILICOM_M1E10G2BPI9T_SSID    0x480
+
+#define SILICOM_M2E10G2BPI9CX4_SSID  0x581
+#define SILICOM_M2E10G2BPI9SR_SSID   0x582
+#define SILICOM_M2E10G2BPI9LR_SSID   0x583
+#define SILICOM_M2E10G2BPI9T_SSID    0x580
+
+#define SILICOM_PE210G2BPI9CX4_SSID  0x121
+#define SILICOM_PE210G2BPI9SR_SSID   0x122
+#define SILICOM_PE210G2BPI9LR_SSID   0x123
+#define SILICOM_PE210G2BPI9T_SSID    0x120
+
+#define DBI_IF_SERIES(pid) \
+	((pid == SILICOM_PE10GDBISR_SSID) || \
+	 (pid == SILICOM_PE10GDBILR_SSID) || \
+	 (pid == SILICOM_XE10G2BPILR_SSID) || \
+	 (pid == SILICOM_PE210G2DBi9LR_SSID))
+
+#define PEGF5_IF_SERIES(pid) \
+	((pid == SILICOM_PEG2BPFI5_SSID) || \
+	 (pid == SILICOM_PEG2BPFI5LX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6LX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6ZX_SSID) || \
+	 (pid == SILICOM_PEG2BPFI6_SSID) || \
+	 (pid == SILICOM_PEG2BPFI6LX_SSID) || \
+	 (pid == SILICOM_PEG2BPFI6ZX_SSID) || \
+	 (pid == SILICOM_PEG2BPFI6FLXM_SSID) || \
+	 (pid == SILICOM_PEG2DBFI6_SSID) || \
+	 (pid == SILICOM_PEG2DBFI6LX_SSID) || \
+	 (pid == SILICOM_PEG2DBFI6ZX_SSID) || \
+	 (pid == SILICOM_PEG4BPI6FC_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6FCLX_SSID) || \
+	 (pid == SILICOM_PEG4BPI6FC_SSID) || \
+	 (pid == SILICOM_M1EG2BPFI6_SSID) || \
+	 (pid == SILICOM_M1EG2BPFI6LX_SSID) || \
+	 (pid == SILICOM_M1EG2BPFI6ZX_SSID) || \
+	 (pid == SILICOM_M1EG4BPFI6_SSID) || \
+	 (pid == SILICOM_M1EG4BPFI6LX_SSID) || \
+	 (pid == SILICOM_M1EG4BPFI6ZX_SSID) || \
+	 (pid == SILICOM_M2EG2BPFI6_SSID) || \
+	 (pid == SILICOM_M2EG2BPFI6LX_SSID) || \
+	 (pid == SILICOM_M2EG2BPFI6ZX_SSID) || \
+	 (pid == SILICOM_M2EG4BPFI6_SSID) || \
+	 (pid == SILICOM_M2EG4BPFI6LX_SSID) || \
+	 (pid == SILICOM_M2EG4BPFI6ZX_SSID) || \
+	 (pid == SILICOM_PEG4BPFI6FCZX_SSID))
+
+#define PEG5_IF_SERIES(pid) \
+	((pid == SILICOM_PEG4BPI6_SSID) || \
+	 (pid == SILICOM_PEG2BPI6_SSID) || \
+	 (pid == SILICOM_PEG4BPI6FC_SSID) || \
+	 (pid == SILICOM_PEG6BPI6_SSID) || \
+	 (pid == SILICOM_PEG2BPI6SC6_SSID) || \
+	 (pid == SILICOM_MEG2BPI6_SSID) || \
+	 (pid == SILICOM_XEG2BPI6_SSID) || \
+	 (pid == SILICOM_MEG4BPI6_SSID) || \
+	 (pid == SILICOM_M1EG2BPI6_SSID) || \
+	 (pid == SILICOM_M1EG4BPI6_SSID) || \
+	 (pid == SILICOM_M1EG6BPI6_SSID) || \
+	 (pid == SILICOM_PEG6BPI_SSID) || \
+	 (pid == SILICOM_PEG4BPIL_SSID) || \
+	 (pid == SILICOM_PEG2BISC6_SSID) || \
+	 (pid == SILICOM_PEG2BPI5_SSID))
+
+#define PEG80_IF_SERIES(pid) \
+	((pid == SILICOM_M1E2G4BPi80_SSID) || \
+	 (pid == SILICOM_M6E2G8BPi80_SSID) || \
+	 (pid == SILICOM_PE2G4BPi80L_SSID) || \
+	 (pid == SILICOM_M6E2G8BPi80A_SSID) || \
+	 (pid == SILICOM_PE2G2BPi35_SSID) || \
+	 (pid == SILICOM_PAC1200BPi35_SSID) || \
+	 (pid == SILICOM_PE2G4BPi35_SSID) || \
+	 (pid == SILICOM_PE2G4BPi35L_SSID) || \
+	 (pid == SILICOM_PE2G6BPi35_SSID) || \
+	 (pid == SILICOM_PE2G2BPi80_SSID) || \
+	 (pid == SILICOM_PE2G4BPi80_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80LX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80LX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35LX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35ZX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35LX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35ZX_SSID))
+
+#define PEGF80_IF_SERIES(pid) \
+	((pid == SILICOM_PE2G4BPFi80_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80LX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi80ZX_SSID) || \
+	 (pid == SILICOM_M1E2G4BPFi80_SSID) || \
+	 (pid == SILICOM_M1E2G4BPFi80LX_SSID) || \
+	 (pid == SILICOM_M1E2G4BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80LX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi80ZX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35LX_SSID) || \
+	 (pid == SILICOM_PE2G2BPFi35ZX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35LX_SSID) || \
+	 (pid == SILICOM_PE2G4BPFi35ZX_SSID))
+
+#define BP10G9_IF_SERIES(pid) \
+	((pid == INTEL_PE210G2SPI9_SSID) || \
+	 (pid == SILICOM_M1E10G2BPI9CX4_SSID) || \
+	 (pid == SILICOM_M1E10G2BPI9SR_SSID) || \
+	 (pid == SILICOM_M1E10G2BPI9LR_SSID) || \
+	 (pid == SILICOM_M1E10G2BPI9T_SSID) || \
+	 (pid == SILICOM_M2E10G2BPI9CX4_SSID) || \
+	 (pid == SILICOM_M2E10G2BPI9SR_SSID) || \
+	 (pid == SILICOM_M2E10G2BPI9LR_SSID) || \
+	 (pid == SILICOM_M2E10G2BPI9T_SSID) || \
+	 (pid == SILICOM_PE210G2BPI9CX4_SSID) || \
+	 (pid == SILICOM_PE210G2BPI9SR_SSID) || \
+	 (pid == SILICOM_PE210G2BPI9LR_SSID) || \
+	 (pid == SILICOM_PE210G2DBi9SR_SSID) || \
+	 (pid == SILICOM_PE210G2DBi9SRRB_SSID) || \
+	 (pid == SILICOM_PE210G2DBi9LR_SSID) || \
+	 (pid == SILICOM_PE210G2DBi9LRRB_SSID) || \
+	 (pid == SILICOM_PE310G4DBi940SR_SSID) || \
+	 (pid == SILICOM_PEG2BISC6_SSID) || \
+	 (pid == SILICOM_PE310G4BPi9T_SSID) || \
+	 (pid == SILICOM_PE310G4BPi9SR_SSID) || \
+	 (pid == SILICOM_PE310G4BPi9LR_SSID) || \
+	 (pid == SILICOM_PE210G2BPI9T_SSID))
+
+/*******************************************************/
+/* 1G INTERFACE ****************************************/
+/*******************************************************/
+
+/* Intel Registers */
+#define BPCTLI_CTRL          0x00000
+#define BPCTLI_CTRL_SWDPIO0  0x00400000
+#define BPCTLI_CTRL_SWDPIN0  0x00040000
+
+#define BPCTLI_CTRL_EXT 0x00018	/* Extended Device Control - RW */
+#define BPCTLI_STATUS   0x00008	/* Device Status - RO */
+
+/* HW related */
+#define BPCTLI_CTRL_EXT_SDP6_DATA 0x00000040	/* Value of SW Defineable Pin 6 */
+#define BPCTLI_CTRL_EXT_SDP7_DATA 0x00000080	/* Value of SW Defineable Pin 7 */
+#define BPCTLI_CTRL_SDP0_DATA     0x00040000	/* SWDPIN 0 value */
+#define BPCTLI_CTRL_EXT_SDP6_DIR  0x00000400	/* Direction of SDP6 0=in 1=out */
+#define BPCTLI_CTRL_EXT_SDP7_DIR  0x00000800	/* Direction of SDP7 0=in 1=out */
+#define BPCTLI_CTRL_SDP0_DIR      0x00400000	/* SDP0 Input or output */
+#define BPCTLI_CTRL_SWDPIN1       0x00080000
+#define BPCTLI_CTRL_SDP1_DIR      0x00800000
+
+#define BPCTLI_STATUS_LU          0x00000002	/* Link up.0=no,1=link */
+
+#define BPCTLI_CTRL_SDP0_SHIFT     18
+#define BPCTLI_CTRL_EXT_SDP6_SHIFT 6
+
+#define BPCTLI_STATUS_TBIMODE     0x00000020
+#define BPCTLI_CTRL_EXT_LINK_MODE_PCIE_SERDES  0x00C00000
+#define BPCTLI_CTRL_EXT_LINK_MODE_MASK         0x00C00000
+
+#define BPCTLI_CTRL_EXT_MCLK_DIR  BPCTLI_CTRL_EXT_SDP7_DIR
+#define BPCTLI_CTRL_EXT_MCLK_DATA BPCTLI_CTRL_EXT_SDP7_DATA
+#define BPCTLI_CTRL_EXT_MDIO_DIR  BPCTLI_CTRL_EXT_SDP6_DIR
+#define BPCTLI_CTRL_EXT_MDIO_DATA BPCTLI_CTRL_EXT_SDP6_DATA
+
+#define BPCTLI_CTRL_EXT_MCLK_DIR5  BPCTLI_CTRL_SDP1_DIR
+#define BPCTLI_CTRL_EXT_MCLK_DATA5 BPCTLI_CTRL_SWDPIN1
+#define BPCTLI_CTRL_EXT_MCLK_DIR80  BPCTLI_CTRL_EXT_SDP6_DIR
+#define BPCTLI_CTRL_EXT_MCLK_DATA80 BPCTLI_CTRL_EXT_SDP6_DATA
+#define BPCTLI_CTRL_EXT_MDIO_DIR5  BPCTLI_CTRL_SWDPIO0
+#define BPCTLI_CTRL_EXT_MDIO_DATA5 BPCTLI_CTRL_SWDPIN0
+#define BPCTLI_CTRL_EXT_MDIO_DIR80  BPCTLI_CTRL_SWDPIO0
+#define BPCTLI_CTRL_EXT_MDIO_DATA80 BPCTLI_CTRL_SWDPIN0
+
+#define BPCTL_WRITE_REG(a, reg, value) \
+	(writel((value), (void *)(((a)->mem_map) + BPCTLI_##reg)))
+
+#define BPCTL_READ_REG(a, reg) ( \
+	readl((void *)((a)->mem_map) + BPCTLI_##reg))
+
+#define BPCTL_WRITE_FLUSH(a) BPCTL_READ_REG(a, STATUS)
+
+#define BPCTL_BP_WRITE_REG(a, reg, value) ({ \
+	BPCTL_WRITE_REG(a, reg, value); \
+	BPCTL_WRITE_FLUSH(a); })
+
+/**************************************************************/
+/************** 82575 Interface********************************/
+/**************************************************************/
+
+#define BPCTLI_MII_CR_POWER_DOWN       0x0800
+#define BPCTLI_PHY_CONTROL      0x00	/* Control Register */
+#define BPCTLI_MDIC     0x00020	/* MDI Control - RW */
+#define BPCTLI_IGP01E1000_PHY_PAGE_SELECT        0x1F	/* Page Select */
+#define BPCTLI_MAX_PHY_REG_ADDRESS    0x1F	/* 5 bit address bus (0-0x1F) */
+
+#define BPCTLI_MDIC_DATA_MASK 0x0000FFFF
+#define BPCTLI_MDIC_REG_MASK  0x001F0000
+#define BPCTLI_MDIC_REG_SHIFT 16
+#define BPCTLI_MDIC_PHY_MASK  0x03E00000
+#define BPCTLI_MDIC_PHY_SHIFT 21
+#define BPCTLI_MDIC_OP_WRITE  0x04000000
+#define BPCTLI_MDIC_OP_READ   0x08000000
+#define BPCTLI_MDIC_READY     0x10000000
+#define BPCTLI_MDIC_INT_EN    0x20000000
+#define BPCTLI_MDIC_ERROR     0x40000000
+
+#define BPCTLI_SWFW_PHY0_SM  0x02
+#define BPCTLI_SWFW_PHY1_SM  0x04
+
+#define BPCTLI_SW_FW_SYNC  0x05B5C	/* Software-Firmware Synchronization - RW */
+
+#define BPCTLI_SWSM      0x05B50	/* SW Semaphore */
+#define BPCTLI_FWSM      0x05B54	/* FW Semaphore */
+
+#define BPCTLI_SWSM_SMBI         0x00000001	/* Driver Semaphore bit */
+#define BPCTLI_SWSM_SWESMBI      0x00000002	/* FW Semaphore bit */
+#define BPCTLI_MAX_PHY_MULTI_PAGE_REG 0xF
+#define BPCTLI_GEN_POLL_TIMEOUT          640
+
+/********************************************************/
+
+/********************************************************/
+/* 10G INTERFACE ****************************************/
+/********************************************************/
+
+#define BP10G_I2CCTL              0x28
+
+/* I2CCTL Bit Masks */
+#define BP10G_I2C_CLK_IN    0x00000001
+#define BP10G_I2C_CLK_OUT   0x00000002
+#define BP10G_I2C_DATA_IN   0x00000004
+#define BP10G_I2C_DATA_OUT  0x00000008
+
+#define BP10G_ESDP                0x20
+
+#define BP10G_SDP0_DIR            0x100
+#define BP10G_SDP1_DIR            0x200
+#define BP10G_SDP3_DIR            0x800
+#define BP10G_SDP4_DIR            BIT_12
+#define BP10G_SDP5_DIR            0x2000
+#define BP10G_SDP0_DATA           0x001
+#define BP10G_SDP1_DATA           0x002
+#define BP10G_SDP3_DATA           0x008
+#define BP10G_SDP4_DATA           0x010
+#define BP10G_SDP5_DATA           0x020
+
+#define BP10G_SDP2_DIR            0x400
+#define BP10G_SDP2_DATA            0x4
+
+#define BP10G_EODSDP              0x28
+
+#define BP10G_SDP6_DATA_IN        0x001
+#define BP10G_SDP6_DATA_OUT       0x002
+
+#define BP10G_SDP7_DATA_IN        0x004
+#define BP10G_SDP7_DATA_OUT       0x008
+
+#define BP10G_MCLK_DATA_OUT       BP10G_SDP7_DATA_OUT
+#define BP10G_MDIO_DATA_OUT       BP10G_SDP6_DATA_OUT
+#define BP10G_MDIO_DATA_IN        BP10G_SDP6_DATA_IN
+
+#define BP10G_MDIO_DATA           /*BP10G_SDP5_DATA*/ BP10G_SDP3_DATA
+#define BP10G_MDIO_DIR            /*BP10G_SDP5_DIR*/  BP10G_SDP3_DATA
+
+/*#define BP10G_MCLK_DATA_OUT9       BP10G_I2C_CLK_OUT
+#define BP10G_MDIO_DATA_OUT9       BP10G_I2C_DATA_OUT*/
+
+				       /*#define BP10G_MCLK_DATA_OUT9*//*BP10G_I2C_DATA_OUT */
+#define BP10G_MDIO_DATA_OUT9           BP10G_I2C_DATA_OUT	/*BP10G_I2C_CLK_OUT */
+
+/* VIA EOSDP ! */
+#define BP10G_MCLK_DATA_OUT9           BP10G_SDP4_DATA
+#define BP10G_MCLK_DIR_OUT9            BP10G_SDP4_DIR
+
+/*#define BP10G_MDIO_DATA_IN9        BP10G_I2C_DATA_IN*/
+
+#define BP10G_MDIO_DATA_IN9           BP10G_I2C_DATA_IN	/*BP10G_I2C_CLK_IN */
+
+#define BP540_MDIO_DATA           /*BP10G_SDP5_DATA*/ BP10G_SDP0_DATA
+#define BP540_MDIO_DIR            /*BP10G_SDP5_DIR*/  BP10G_SDP0_DIR
+#define BP540_MCLK_DATA       BP10G_SDP2_DATA
+#define BP540_MCLK_DIR       BP10G_SDP2_DIR
+
+#define BP10G_WRITE_REG(a, reg, value) \
+	(writel((value), (void *)(((a)->mem_map) + BP10G_##reg)))
+
+#define BP10G_READ_REG(a, reg) ( \
+	readl((void *)((a)->mem_map) + BP10G_##reg))
+
+/*****BROADCOM*******************************************/
+
+#define BP10GB_MISC_REG_GPIO						 0xa490
+#define BP10GB_GPIO3_P0                              BIT_3
+#define BP10GB_GPIO3_P1                              BIT_7
+
+#define BP10GB_GPIO3_SET_P0                          BIT_11
+#define BP10GB_GPIO3_CLR_P0                          BIT_19
+#define BP10GB_GPIO3_OE_P0                           BIT_27
+
+#define BP10GB_GPIO3_SET_P1                          BIT_15
+#define BP10GB_GPIO3_CLR_P1                          BIT_23
+#define BP10GB_GPIO3_OE_P1                           BIT_31
+
+#define BP10GB_GPIO0_P1                              0x10
+#define BP10GB_GPIO0_P0                              0x1
+#define BP10GB_GPIO0_CLR_P0                          0x10000
+#define BP10GB_GPIO0_CLR_P1                          0x100000
+#define BP10GB_GPIO0_SET_P0                          0x100
+#define BP10GB_GPIO0_SET_P1                          0x1000
+
+#define BP10GB_GPIO0_OE_P1                           0x10000000
+#define BP10GB_GPIO0_OE_P0                           0x1000000
+
+#define BP10GB_MISC_REG_SPIO						 0xa4fc
+#define BP10GB_GPIO4_OE                              BIT_28
+#define BP10GB_GPIO5_OE                              BIT_29
+#define BP10GB_GPIO4_CLR                             BIT_20
+#define BP10GB_GPIO5_CLR                             BIT_21
+#define BP10GB_GPIO4_SET                             BIT_12
+#define BP10GB_GPIO5_SET                             BIT_13
+#define BP10GB_GPIO4                                 BIT_4
+#define BP10GB_GPIO5                                 BIT_5
+
+#define BP10GB_MCLK_DIR  BP10GB_GPIO5_OE
+#define BP10GB_MDIO_DIR  BP10GB_GPIO4_OE
+
+#define BP10GB_MCLK_DATA BP10GB_GPIO5
+#define BP10GB_MDIO_DATA BP10GB_GPIO4
+
+#define BP10GB_MCLK_SET BP10GB_GPIO5_SET
+#define BP10GB_MDIO_SET BP10GB_GPIO4_SET
+
+#define BP10GB_MCLK_CLR BP10GB_GPIO5_CLR
+#define BP10GB_MDIO_CLR BP10GB_GPIO4_CLR
+
+#define BP10GB_WRITE_REG(a, reg, value) \
+	(writel((value), (void *)(((a)->mem_map) + BP10GB_##reg)))
+
+#define BP10GB_READ_REG(a, reg) ( \
+	readl((void *)((a)->mem_map) + BP10GB_##reg))
+
+#endif
+
+int bp_proc_create(void);
diff --git a/drivers/staging/silicom/bp_proc.c b/drivers/staging/silicom/bp_proc.c
new file mode 100644
index 000000000000..6ad4b27472e4
--- /dev/null
+++ b/drivers/staging/silicom/bp_proc.c
@@ -0,0 +1,1350 @@
+/******************************************************************************/
+/*                                                                            */
+/* Copyright (c) 2004-2006 Silicom, Ltd                                       */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/******************************************************************************/
+
+#include <linux/version.h>
+#if defined(CONFIG_SMP) && ! defined(__SMP__)
+#define __SMP__
+#endif
+
+#include <linux/proc_fs.h>
+#include <linux/netdevice.h>
+#include <asm/uaccess.h>
+//#include <linux/smp_lock.h>
+#include "bp_mod.h"
+
+#define BP_PROC_DIR "bypass"
+//#define BYPASS_SUPPORT "bypass"
+
+#ifdef  BYPASS_SUPPORT
+
+#define GPIO6_SET_ENTRY_SD           "gpio6_set"
+#define GPIO6_CLEAR_ENTRY_SD         "gpio6_clear"
+
+#define GPIO7_SET_ENTRY_SD           "gpio7_set"
+#define GPIO7_CLEAR_ENTRY_SD         "gpio7_clear"
+
+#define PULSE_SET_ENTRY_SD            "pulse_set"
+#define ZERO_SET_ENTRY_SD            "zero_set"
+#define PULSE_GET1_ENTRY_SD            "pulse_get1"
+#define PULSE_GET2_ENTRY_SD            "pulse_get2"
+
+#define CMND_ON_ENTRY_SD              "cmnd_on"
+#define CMND_OFF_ENTRY_SD             "cmnd_off"
+#define RESET_CONT_ENTRY_SD           "reset_cont"
+
+ /*COMMANDS*/
+#define BYPASS_INFO_ENTRY_SD     "bypass_info"
+#define BYPASS_SLAVE_ENTRY_SD    "bypass_slave"
+#define BYPASS_CAPS_ENTRY_SD     "bypass_caps"
+#define WD_SET_CAPS_ENTRY_SD     "wd_set_caps"
+#define BYPASS_ENTRY_SD          "bypass"
+#define BYPASS_CHANGE_ENTRY_SD   "bypass_change"
+#define BYPASS_WD_ENTRY_SD       "bypass_wd"
+#define WD_EXPIRE_TIME_ENTRY_SD  "wd_expire_time"
+#define RESET_BYPASS_WD_ENTRY_SD "reset_bypass_wd"
+#define DIS_BYPASS_ENTRY_SD      "dis_bypass"
+#define BYPASS_PWUP_ENTRY_SD     "bypass_pwup"
+#define BYPASS_PWOFF_ENTRY_SD     "bypass_pwoff"
+#define STD_NIC_ENTRY_SD         "std_nic"
+#define STD_NIC_ENTRY_SD         "std_nic"
+#define TAP_ENTRY_SD             "tap"
+#define TAP_CHANGE_ENTRY_SD      "tap_change"
+#define DIS_TAP_ENTRY_SD         "dis_tap"
+#define TAP_PWUP_ENTRY_SD        "tap_pwup"
+#define TWO_PORT_LINK_ENTRY_SD   "two_port_link"
+#define WD_EXP_MODE_ENTRY_SD     "wd_exp_mode"
+#define WD_AUTORESET_ENTRY_SD    "wd_autoreset"
+#define TPL_ENTRY_SD             "tpl"
+#define WAIT_AT_PWUP_ENTRY_SD    "wait_at_pwup"
+#define HW_RESET_ENTRY_SD        "hw_reset"
+#define DISC_ENTRY_SD             "disc"
+#define DISC_CHANGE_ENTRY_SD      "disc_change"
+#define DIS_DISC_ENTRY_SD         "dis_disc"
+#define DISC_PWUP_ENTRY_SD        "disc_pwup"
+#endif				//bypass_support
+static struct proc_dir_entry *bp_procfs_dir;
+
+static struct proc_dir_entry *proc_getdir(char *name,
+					  struct proc_dir_entry *proc_dir)
+{
+	struct proc_dir_entry *pde = proc_dir;
+	for (pde = pde->subdir; pde; pde = pde->next) {
+		if (pde->namelen && (strcmp(name, pde->name) == 0)) {
+			/* directory exists */
+			break;
+		}
+	}
+	if (pde == (struct proc_dir_entry *)0) {
+		/* create the directory */
+		pde = create_proc_entry(name, S_IFDIR, proc_dir);
+		if (pde == (struct proc_dir_entry *)0) {
+			return (pde);
+		}
+	}
+	return (pde);
+}
+
+#ifdef BYPASS_SUPPORT
+
+int
+bypass_proc_create_entry_sd(struct pfs_unit *pfs_unit_curr,
+			    char *proc_name,
+			    write_proc_t * write_proc,
+			    read_proc_t * read_proc,
+			    struct proc_dir_entry *parent_pfs, void *data)
+{
+	strcpy(pfs_unit_curr->proc_name, proc_name);
+	pfs_unit_curr->proc_entry = create_proc_entry(pfs_unit_curr->proc_name,
+						      S_IFREG | S_IRUSR |
+						      S_IWUSR | S_IRGRP |
+						      S_IROTH, parent_pfs);
+	if (pfs_unit_curr->proc_entry == 0) {
+
+		return -1;
+	}
+
+	pfs_unit_curr->proc_entry->read_proc = read_proc;
+	pfs_unit_curr->proc_entry->write_proc = write_proc;
+	pfs_unit_curr->proc_entry->data = data;
+
+	return 0;
+
+}
+
+int
+get_bypass_info_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	int len = 0;
+
+	len += sprintf(page, "Name\t\t\t%s\n", pbp_device_block->bp_name);
+	len +=
+	    sprintf(page + len, "Firmware version\t0x%x\n",
+		    pbp_device_block->bp_fw_ver);
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_slave_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	struct pci_dev *pci_slave_dev = pbp_device_block->bp_slave;
+	struct net_device *net_slave_dev;
+	int len = 0;
+
+	if (is_bypass_fn(pbp_device_block)) {
+		net_slave_dev = pci_get_drvdata(pci_slave_dev);
+		if (net_slave_dev)
+			len = sprintf(page, "%s\n", net_slave_dev->name);
+		else
+			len = sprintf(page, "fail\n");
+	} else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_caps_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_caps_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "-1\n");
+	else
+		len = sprintf(page, "0x%x\n", ret);
+	*eof = 1;
+	return len;
+
+}
+
+int
+get_wd_set_caps_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_set_caps_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "-1\n");
+	else
+		len = sprintf(page, "0x%x\n", ret);
+	*eof = 1;
+	return len;
+}
+
+int
+set_bypass_pfs(struct file *file, const char *buffer,
+	       unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_tap_pfs(struct file *file, const char *buffer,
+	    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_tap_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_disc_pfs(struct file *file, const char *buffer,
+	     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_disc_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_bypass_pfs(char *page, char **start, off_t off, int count,
+	       int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_pfs(char *page, char **start, off_t off, int count,
+	    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_pfs(char *page, char **start, off_t off, int count,
+	     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_change_pfs(char *page, char **start, off_t off, int count,
+		      int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_change_pfs(char *page, char **start, off_t off, int count,
+		   int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_change_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_change_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_bypass_wd_pfs(struct file *file, const char *buffer,
+		  unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	unsigned int timeout = 0;
+	char *timeout_ptr = kbuf;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	timeout_ptr = kbuf;
+	timeout = atoi(&timeout_ptr);
+
+	set_bypass_wd_fn(pbp_device_block, timeout);
+
+	return count;
+}
+
+int
+get_bypass_wd_pfs(char *page, char **start, off_t off, int count,
+		  int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0, timeout = 0;
+
+	ret = get_bypass_wd_fn(pbp_device_block, &timeout);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (timeout == -1)
+		len = sprintf(page, "unknown\n");
+	else if (timeout == 0)
+		len = sprintf(page, "disable\n");
+	else
+		len = sprintf(page, "%d\n", timeout);
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_wd_expire_time_pfs(char *page, char **start, off_t off, int count,
+		       int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0, timeout = 0;
+
+	ret = get_wd_expire_time_fn(pbp_device_block, &timeout);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (timeout == -1)
+		len = sprintf(page, "expire\n");
+	else if (timeout == 0)
+		len = sprintf(page, "disable\n");
+
+	else
+		len = sprintf(page, "%d\n", timeout);
+	*eof = 1;
+	return len;
+}
+
+int
+get_tpl_pfs(char *page, char **start, off_t off, int count,
+	    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tpl_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+#ifdef PMC_FIX_FLAG
+int
+get_wait_at_pwup_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bp_wait_at_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_hw_reset_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bp_hw_reset_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 1)
+		len = sprintf(page, "on\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+
+	*eof = 1;
+	return len;
+}
+
+#endif				/*PMC_WAIT_FLAG */
+
+int
+reset_bypass_wd_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = reset_bypass_wd_timer_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "disable\n");
+	else if (ret == 1)
+		len = sprintf(page, "success\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_dis_bypass_pfs(struct file *file, const char *buffer,
+		   unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_dis_bypass_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_dis_tap_pfs(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_dis_tap_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_dis_disc_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_dis_disc_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_dis_bypass_pfs(char *page, char **start, off_t off, int count,
+		   int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_bypass_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_dis_tap_pfs(char *page, char **start, off_t off, int count,
+		int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_tap_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_dis_disc_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_dis_disc_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_bypass_pwup_pfs(struct file *file, const char *buffer,
+		    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_pwup_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_bypass_pwoff_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_bypass_pwoff_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+set_tap_pwup_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_tap_pwup_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+set_disc_pwup_pfs(struct file *file, const char *buffer,
+		  unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tap_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tap_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tap_param = 0;
+
+	set_disc_pwup_fn(pbp_device_block, tap_param);
+
+	return count;
+}
+
+int
+get_bypass_pwup_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_bypass_pwoff_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_bypass_pwoff_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_tap_pwup_pfs(char *page, char **start, off_t off, int count,
+		 int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_tap_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_disc_pwup_pfs(char *page, char **start, off_t off, int count,
+		  int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_disc_pwup_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_std_nic_pfs(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		bypass_param = 0;
+
+	set_std_nic_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+get_std_nic_pfs(char *page, char **start, off_t off, int count,
+		int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_std_nic_fn(pbp_device_block);
+	if (ret == BP_NOT_CAP)
+		len = sprintf(page, "fail\n");
+	else if (ret == 0)
+		len = sprintf(page, "off\n");
+	else
+		len = sprintf(page, "on\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+get_wd_exp_mode_pfs(char *page, char **start, off_t off, int count,
+		    int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_exp_mode_fn(pbp_device_block);
+	if (ret == 1)
+		len = sprintf(page, "tap\n");
+	else if (ret == 0)
+		len = sprintf(page, "bypass\n");
+	else if (ret == 2)
+		len = sprintf(page, "disc\n");
+
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_wd_exp_mode_pfs(struct file *file, const char *buffer,
+		    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int bypass_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "tap") == 0)
+		bypass_param = 1;
+	else if (strcmp(kbuf, "bypass") == 0)
+		bypass_param = 0;
+	else if (strcmp(kbuf, "disc") == 0)
+		bypass_param = 2;
+
+	set_wd_exp_mode_fn(pbp_device_block, bypass_param);
+
+	return count;
+}
+
+int
+get_wd_autoreset_pfs(char *page, char **start, off_t off, int count,
+		     int *eof, void *data)
+{
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int len = 0, ret = 0;
+
+	ret = get_wd_autoreset_fn(pbp_device_block);
+	if (ret >= 0)
+		len = sprintf(page, "%d\n", ret);
+	else
+		len = sprintf(page, "fail\n");
+
+	*eof = 1;
+	return len;
+}
+
+int
+set_wd_autoreset_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	u32 timeout = 0;
+	char *timeout_ptr = kbuf;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	timeout_ptr = kbuf;
+	timeout = atoi(&timeout_ptr);
+
+	set_wd_autoreset_fn(pbp_device_block, timeout);
+
+	return count;
+}
+
+int
+set_tpl_pfs(struct file *file, const char *buffer,
+	    unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_tpl_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+#ifdef PMC_FIX_FLAG
+int
+set_wait_at_pwup_pfs(struct file *file, const char *buffer,
+		     unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_bp_wait_at_pwup_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+int
+set_hw_reset_pfs(struct file *file, const char *buffer,
+		 unsigned long count, void *data)
+{
+
+	char kbuf[256];
+	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+
+	int tpl_param = 0, length = 0;
+
+	if (count > (sizeof(kbuf) - 1))
+		return -1;
+
+	if (copy_from_user(&kbuf, buffer, count)) {
+		return -1;
+	}
+
+	kbuf[count] = '\0';
+	length = strlen(kbuf);
+	if (kbuf[length - 1] == '\n')
+		kbuf[--length] = '\0';
+
+	if (strcmp(kbuf, "on") == 0)
+		tpl_param = 1;
+	else if (strcmp(kbuf, "off") == 0)
+		tpl_param = 0;
+
+	set_bp_hw_reset_fn(pbp_device_block, tpl_param);
+
+	return count;
+}
+
+#endif				/*PMC_FIX_FLAG */
+
+int bypass_proc_create_dev_sd(bpctl_dev_t * pbp_device_block)
+{
+	struct bypass_pfs_sd *current_pfs = &(pbp_device_block->bypass_pfs_set);
+	static struct proc_dir_entry *procfs_dir = NULL;
+	int ret = 0;
+
+	sprintf(current_pfs->dir_name, "bypass_%s", dev->name);
+
+	if (!bp_procfs_dir)
+		return -1;
+
+	/* create device proc dir */
+	procfs_dir = proc_getdir(current_pfs->dir_name, bp_procfs_dir);
+	if (procfs_dir == 0) {
+		printk(KERN_DEBUG "Could not create procfs directory %s\n",
+		       current_pfs->dir_name);
+		return -1;
+	}
+	current_pfs->bypass_entry = procfs_dir;
+
+	if (bypass_proc_create_entry(&(current_pfs->bypass_info), BYPASS_INFO_ENTRY_SD, NULL,	/* write */
+				     get_bypass_info_pfs,	/* read  */
+				     procfs_dir, pbp_device_block))
+		ret = -1;
+
+	if (pbp_device_block->bp_caps & SW_CTL_CAP) {
+
+		/* Create set param proc's */
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_slave), BYPASS_SLAVE_ENTRY_SD, NULL,	/* write */
+						get_bypass_slave_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_caps), BYPASS_CAPS_ENTRY_SD, NULL,	/* write */
+						get_bypass_caps_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_set_caps), WD_SET_CAPS_ENTRY_SD, NULL,	/* write */
+						get_wd_set_caps_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_wd), BYPASS_WD_ENTRY_SD, set_bypass_wd_pfs,	/* write */
+						get_bypass_wd_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_expire_time), WD_EXPIRE_TIME_ENTRY_SD, NULL,	/* write */
+						get_wd_expire_time_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->reset_bypass_wd), RESET_BYPASS_WD_ENTRY_SD, NULL,	/* write */
+						reset_bypass_wd_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->std_nic), STD_NIC_ENTRY_SD, set_std_nic_pfs,	/* write */
+						get_std_nic_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (pbp_device_block->bp_caps & BP_CAP) {
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass), BYPASS_ENTRY_SD, set_bypass_pfs,	/* write */
+							get_bypass_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_bypass), DIS_BYPASS_ENTRY_SD, set_dis_bypass_pfs,	/* write */
+							get_dis_bypass_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwup), BYPASS_PWUP_ENTRY_SD, set_bypass_pwup_pfs,	/* write */
+							get_bypass_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwoff), BYPASS_PWOFF_ENTRY_SD, set_bypass_pwoff_pfs,	/* write */
+							get_bypass_pwoff_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_change), BYPASS_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_bypass_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+
+		if (pbp_device_block->bp_caps & TAP_CAP) {
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap), TAP_ENTRY_SD, set_tap_pfs,	/* write */
+							get_tap_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_TAP_ENTRY_SD, set_dis_tap_pfs,	/* write */
+							get_dis_tap_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), TAP_PWUP_ENTRY_SD, set_tap_pwup_pfs,	/* write */
+							get_tap_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), TAP_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_tap_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+		if (pbp_device_block->bp_caps & DISC_CAP) {
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap), DISC_ENTRY_SD, set_disc_pfs,	/* write */
+							get_disc_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+#if 1
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_DISC_ENTRY_SD, set_dis_disc_pfs,	/* write */
+							get_dis_disc_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+#endif
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), DISC_PWUP_ENTRY_SD, set_disc_pwup_pfs,	/* write */
+							get_disc_pwup_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+
+			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), DISC_CHANGE_ENTRY_SD, NULL,	/* write */
+							get_disc_change_pfs,	/* read  */
+							procfs_dir,
+							pbp_device_block))
+				ret = -1;
+		}
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_exp_mode), WD_EXP_MODE_ENTRY_SD, set_wd_exp_mode_pfs,	/* write */
+						get_wd_exp_mode_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+		if (bypass_proc_create_entry_sd(&(current_pfs->wd_autoreset), WD_AUTORESET_ENTRY_SD, set_wd_autoreset_pfs,	/* write */
+						get_wd_autoreset_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), TPL_ENTRY_SD, set_tpl_pfs,	/* write */
+						get_tpl_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+#ifdef PMC_FIX_FLAG
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), WAIT_AT_PWUP_ENTRY_SD, set_wait_at_pwup_pfs,	/* write */
+						get_wait_at_pwup_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), HW_RESET_ENTRY_SD, set_hw_reset_pfs,	/* write */
+						get_hw_reset_pfs,	/* read  */
+						procfs_dir, pbp_device_block))
+			ret = -1;
+
+#endif
+
+	}
+	if (ret < 0)
+		printk(KERN_DEBUG "Create proc entry failed\n");
+
+	return ret;
+}
+
+int bypass_proc_remove_dev_sd(bpctl_dev_t * pbp_device_block)
+{
+
+	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
+	struct proc_dir_entry *pde = current_pfs->bypass_entry, *pde_curr =
+	    NULL;
+	char name[256];
+
+	for (pde = pde->subdir; pde;) {
+		strcpy(name, pde->name);
+		pde_curr = pde;
+		pde = pde->next;
+		remove_proc_entry(name, current_pfs->bypass_entry);
+	}
+	if (!pde)
+		remove_proc_entry(current_pfs->dir_name, bp_procfs_dir);
+
+	return 0;
+}
+
+#endif				/* BYPASS_SUPPORT */
diff --git a/drivers/staging/silicom/bypass.h b/drivers/staging/silicom/bypass.h
new file mode 100644
index 000000000000..08fa7a0fc8d8
--- /dev/null
+++ b/drivers/staging/silicom/bypass.h
@@ -0,0 +1,202 @@
+/******************************************************************************/
+/*                                                                            */
+/* Bypass Control utility, Copyright (c) 2005 Silicom                         */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/******************************************************************************/
+
+#ifndef BYPASS_H
+#define BYPASS_H
+
+/* Bypass related */
+
+#define SYNC_CMD_VAL               2	/* 10b */
+#define SYNC_CMD_LEN               2
+
+#define WR_CMD_VAL                 2	/* 10b */
+#define WR_CMD_LEN                 2
+
+#define RD_CMD_VAL                 1	/* 10b */
+#define RD_CMD_LEN                 2
+
+#define ADDR_CMD_LEN               4
+
+#define WR_DATA_LEN                8
+#define RD_DATA_LEN                8
+
+#define PIC_SIGN_REG_ADDR          0x7
+#define PIC_SIGN_VALUE         0xcd
+
+#define STATUS_REG_ADDR           0
+#define WDT_EN_MASK            0x01	/* BIT_0 */
+#define CMND_EN_MASK           0x02	/* BIT_1 */
+#define DIS_BYPASS_CAP_MASK    0x04	/* BIT_2    Bypass Cap is disable*/
+#define DFLT_PWRON_MASK        0x08	/* BIT_3 */
+#define BYPASS_OFF_MASK        0x10	/* BIT_4 */
+#define BYPASS_FLAG_MASK       0x20	/* BIT_5 */
+#define STD_NIC_MASK           (DIS_BYPASS_CAP_MASK | BYPASS_OFF_MASK | DFLT_PWRON_MASK)
+#define WD_EXP_FLAG_MASK       0x40	/* BIT_6 */
+#define DFLT_PWROFF_MASK       0x80	/* BIT_7 */
+#define STD_NIC_PWOFF_MASK     (DIS_BYPASS_CAP_MASK | BYPASS_OFF_MASK | DFLT_PWRON_MASK | DFLT_PWROFF_MASK)
+
+#define PRODUCT_CAP_REG_ADDR   0x5
+#define BYPASS_SUPPORT_MASK    0x01	/* BIT_0 */
+#define TAP_SUPPORT_MASK       0x02	/* BIT_1 */
+#define NORMAL_UNSUPPORT_MASK  0x04	/* BIT_2 */
+#define DISC_SUPPORT_MASK      0x08	/* BIT_3 */
+#define TPL2_SUPPORT_MASK      0x10	/* BIT_4 */
+#define DISC_PORT_SUPPORT_MASK 0x20	/* BIT_5 */
+
+#define STATUS_TAP_REG_ADDR    0x6
+#define WDTE_TAP_BPN_MASK      0x01	/* BIT_1    1 when wdt expired -> TAP, 0 - Bypass */
+#define DIS_TAP_CAP_MASK       0x04	/* BIT_2    TAP Cap is disable*/
+#define DFLT_PWRON_TAP_MASK    0x08	/* BIT_3    */
+#define TAP_OFF_MASK           0x10	/* BIT_4    */
+#define TAP_FLAG_MASK          0x20	/* BIT_5    */
+#define TX_DISA_MASK            0x40
+#define TX_DISB_MASK            0x80
+
+#define STD_NIC_TAP_MASK       (DIS_TAP_CAP_MASK | TAP_OFF_MASK | DFLT_PWRON_TAP_MASK)
+
+#define STATUS_DISC_REG_ADDR    13
+#define WDTE_DISC_BPN_MASK      0x01	/* BIT_0    1 when wdt expired -> TAP, 0 - Bypass */
+#define STD_NIC_ON_MASK         0x02	/* BIT_1    */
+#define DIS_DISC_CAP_MASK       0x04	/* BIT_2    TAP Cap is disable*/
+#define DFLT_PWRON_DISC_MASK    0x08	/* BIT_3    */
+#define DISC_OFF_MASK           0x10	/* BIT_4    */
+#define DISC_FLAG_MASK          0x20	/* BIT_5    */
+#define TPL2_FLAG_MASK          0x40	/* BIT_6    */
+#define STD_NIC_DISC_MASK       DIS_DISC_CAP_MASK
+
+#define CONT_CONFIG_REG_ADDR    12
+#define EN_HW_RESET_MASK       0x2	/* BIT_1 */
+#define WAIT_AT_PWUP_MASK      0x1	/* BIT_0 */
+
+#define VER_REG_ADDR               0x1
+#define BP_FW_VER_A0         0xa0
+#define BP_FW_VER_A1         0xa1
+
+#define INT_VER_MASK           0xf0
+#define EXT_VER_MASK           0xf
+/* */
+#define PXG2BPI_VER            0x0
+#define PXG2TBPI_VER           0x1
+#define PXE2TBPI_VER           0x2
+#define PXG4BPFI_VER           0x4
+#define BP_FW_EXT_VER7         0x6
+#define BP_FW_EXT_VER8         0x8
+#define BP_FW_EXT_VER9         0x9
+
+#define OLD_IF_VER              -1
+
+#define CMND_REG_ADDR              10	/* 1010b */
+#define WDT_REG_ADDR               4
+#define TMRL_REG_ADDR              2
+#define TMRH_REG_ADDR              3
+
+/* NEW_FW */
+#define WDT_INTERVAL               1	/* 5     //8   */
+#define WDT_CMND_INTERVAL          200	/* 50          */
+#define CMND_INTERVAL              200	/* 100    usec */
+#define PULSE_TIME                 100
+
+/* OLD_FW */
+#define INIT_CMND_INTERVAL         40
+#define PULSE_INTERVAL             5
+#define WDT_TIME_CNT               3
+
+/* Intel Commands */
+
+#define CMND_OFF_INT               0xf
+#define PWROFF_BYPASS_ON_INT       0x5
+#define BYPASS_ON_INT              0x6
+#define DIS_BYPASS_CAP_INT         0x4
+#define RESET_WDT_INT              0x1
+
+/* Intel timing */
+
+#define BYPASS_DELAY_INT           4	/* msec */
+#define CMND_INTERVAL_INT          2	/* msec */
+
+/* Silicom Commands */
+#define CMND_ON                    0x4
+#define CMND_OFF                   0x2
+#define BYPASS_ON                  0xa
+#define BYPASS_OFF                 0x8
+#define PORT_LINK_EN               0xe
+#define PORT_LINK_DIS              0xc
+#define WDT_ON                     0x10	/* 0x1f (11111) - max */
+#define TIMEOUT_UNIT           100
+#define TIMEOUT_MAX_STEP       15
+#define WDT_TIMEOUT_MIN        100	/*  msec */
+#define WDT_TIMEOUT_MAX        3276800	/*  msec */
+#define WDT_AUTO_MIN_INT           500
+#define WDT_TIMEOUT_DEF        WDT_TIMEOUT_MIN
+#define WDT_OFF                    0x6
+#define WDT_RELOAD                 0x9
+#define RESET_CONT                 0x20
+#define DIS_BYPASS_CAP             0x22
+#define EN_BYPASS_CAP              0x24
+#define BYPASS_STATE_PWRON         0x26
+#define NORMAL_STATE_PWRON         0x28
+#define BYPASS_STATE_PWROFF        0x27
+#define NORMAL_STATE_PWROFF        0x29
+#define TAP_ON                     0xb
+#define TAP_OFF                    0x9
+#define TAP_STATE_PWRON            0x2a
+#define DIS_TAP_CAP                0x2c
+#define EN_TAP_CAP                 0x2e
+#define STD_NIC_OFF       0x86
+#define STD_NIC_ON       0x84
+#define DISC_ON           0x85
+#define DISC_OFF          0x8a
+#define DISC_STATE_PWRON  0x87
+#define DIS_DISC_CAP      0x88
+#define EN_DISC_CAP       0x89
+#define TPL2_ON                    0x8c
+#define TPL2_OFF                   0x8b
+#define BP_WAIT_AT_PWUP_EN        0x80
+#define BP_WAIT_AT_PWUP_DIS       0x81
+#define BP_HW_RESET_EN             0x82
+#define BP_HW_RESET_DIS            0x83
+
+#define TX_DISA                0x8d
+#define TX_DISB                0x8e
+#define TX_ENA                 0xA0
+#define TX_ENB                 0xA1
+
+#define TX_DISA_PWRUP          0xA2
+#define TX_DISB_PWRUP          0xA3
+#define TX_ENA_PWRUP           0xA4
+#define TX_ENB_PWRUP           0xA5
+
+#define BYPASS_CAP_DELAY           21	/* msec */
+#define DFLT_PWRON_DELAY           10	/* msec */
+#define LATCH_DELAY                13	/* msec */
+#define EEPROM_WR_DELAY             8	/* msec */
+
+#define BP_LINK_MON_DELAY          4	/* sec */
+
+#define BP_FW_EXT_VER0                 0xa0
+#define BP_FW_EXT_VER1                 0xa1
+#define BP_FW_EXT_VER2                0xb1
+
+#define BP_OK        0
+#define BP_NOT_CAP  -1
+#define WDT_STATUS_EXP -2
+#define WDT_STATUS_UNKNOWN -1
+#define WDT_STATUS_EN 1
+#define WDT_STATUS_DIS 0
+
+#ifdef BP_SELF_TEST
+#define ETH_P_BPTEST 0xabba
+
+#define BPTEST_DATA_LEN 60
+#endif
+
+#endif				/* BYPASS_H */
diff --git a/drivers/staging/silicom/bypasslib/Makefile b/drivers/staging/silicom/bypasslib/Makefile
new file mode 100644
index 000000000000..80e8b9bc9357
--- /dev/null
+++ b/drivers/staging/silicom/bypasslib/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the Bypass network device drivers.
+#
+
+obj-$(CONFIG_SBYPASS) += bypass.o
+
diff --git a/drivers/staging/silicom/bypasslib/bp_ioctl.h b/drivers/staging/silicom/bypasslib/bp_ioctl.h
new file mode 100644
index 000000000000..040c6fa8d5ad
--- /dev/null
+++ b/drivers/staging/silicom/bypasslib/bp_ioctl.h
@@ -0,0 +1,198 @@
+/******************************************************************************/
+/*                                                                            */
+/* bypass library, Copyright (c) 2004-2006 Silicom, Ltd                       */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/*                                                                            */
+/******************************************************************************/
+
+#ifndef BP_IOCTL_H
+#define BP_IOCTL_H
+
+#define BP_CAP                   0x01	//BIT_0
+#define BP_STATUS_CAP            0x02	//BIT_1
+#define BP_STATUS_CHANGE_CAP     0x04	//BIT_2
+#define SW_CTL_CAP               0x08	//BIT_3
+#define BP_DIS_CAP               0x10	//BIT_4
+#define BP_DIS_STATUS_CAP        0x20	//BIT_5
+#define STD_NIC_CAP              0x40	//BIT_6
+#define BP_PWOFF_ON_CAP          0x80	//BIT_7
+#define BP_PWOFF_OFF_CAP         0x0100	//BIT_8
+#define BP_PWOFF_CTL_CAP         0x0200	//BIT_9
+#define BP_PWUP_ON_CAP           0x0400	//BIT_10
+#define BP_PWUP_OFF_CAP          0x0800	//BIT_11
+#define BP_PWUP_CTL_CAP          0x1000	//BIT_12
+#define WD_CTL_CAP               0x2000	//BIT_13
+#define WD_STATUS_CAP            0x4000	//BIT_14
+#define WD_TIMEOUT_CAP           0x8000	//BIT_15
+#define TX_CTL_CAP               0x10000	//BIT_16
+#define TX_STATUS_CAP            0x20000	//BIT_17
+#define TAP_CAP                  0x40000	//BIT_18
+#define TAP_STATUS_CAP           0x80000	//BIT_19
+#define TAP_STATUS_CHANGE_CAP    0x100000	//BIT_20
+#define TAP_DIS_CAP              0x200000	//BIT_21
+#define TAP_DIS_STATUS_CAP       0x400000	//BIT_22
+#define TAP_PWUP_ON_CAP          0x800000	//BIT_23
+#define TAP_PWUP_OFF_CAP         0x1000000	//BIT 24
+#define TAP_PWUP_CTL_CAP         0x2000000	//BIT 25
+#define NIC_CAP_NEG              0x4000000	//BIT 26
+#define TPL_CAP                  0x8000000	//BIT 27
+#define DISC_CAP                 0x10000000	//BIT 28
+#define DISC_DIS_CAP             0x20000000	//BIT 29
+#define DISC_PWUP_CTL_CAP        0x40000000	//BIT 30
+
+#define WD_MIN_TIME_MASK(val)      (val & 0xf)
+#define WD_STEP_COUNT_MASK(val)    ((val & 0xf) << 5)
+#define WDT_STEP_TIME              0x10	//BIT_4
+
+#define WD_MIN_TIME_GET(desc)   (desc & 0xf)
+#define WD_STEP_COUNT_GET(desc) (desc>>5) & 0xf
+
+typedef enum {
+	IS_BYPASS = 1,
+	GET_BYPASS_SLAVE,
+	GET_BYPASS_CAPS,
+	GET_WD_SET_CAPS,
+	SET_BYPASS,
+	GET_BYPASS,
+	GET_BYPASS_CHANGE,
+	SET_BYPASS_WD,
+	GET_BYPASS_WD,
+	GET_WD_EXPIRE_TIME,
+	RESET_BYPASS_WD_TIMER,
+	SET_DIS_BYPASS,
+	GET_DIS_BYPASS,
+	SET_BYPASS_PWOFF,
+	GET_BYPASS_PWOFF,
+	SET_BYPASS_PWUP,
+	GET_BYPASS_PWUP,
+	SET_STD_NIC,
+	GET_STD_NIC,
+	SET_TX,
+	GET_TX,
+	SET_TAP,
+	GET_TAP,
+	GET_TAP_CHANGE,
+	SET_DIS_TAP,
+	GET_DIS_TAP,
+	SET_TAP_PWUP,
+	GET_TAP_PWUP,
+	SET_WD_EXP_MODE,
+	GET_WD_EXP_MODE,
+	SET_WD_AUTORESET,
+	GET_WD_AUTORESET,
+	SET_TPL,
+	GET_TPL,
+	SET_DISC,
+	GET_DISC,
+	GET_DISC_CHANGE,
+	SET_DIS_DISC,
+	GET_DIS_DISC,
+	SET_DISC_PWUP,
+	GET_DISC_PWUP,
+
+	GET_BYPASS_INFO = 100,
+	GET_BP_WAIT_AT_PWUP,
+	SET_BP_WAIT_AT_PWUP,
+	GET_BP_HW_RESET,
+	SET_BP_HW_RESET,
+} CMND_TYPE;
+
+typedef enum {
+	IF_SCAN_SD,
+	GET_DEV_NUM_SD,
+	IS_BYPASS_SD,
+	GET_BYPASS_SLAVE_SD,
+	GET_BYPASS_CAPS_SD,
+	GET_WD_SET_CAPS_SD,
+	SET_BYPASS_SD,
+	GET_BYPASS_SD,
+	GET_BYPASS_CHANGE_SD,
+	SET_BYPASS_WD_SD,
+	GET_BYPASS_WD_SD,
+	GET_WD_EXPIRE_TIME_SD,
+	RESET_BYPASS_WD_TIMER_SD,
+	SET_DIS_BYPASS_SD,
+	GET_DIS_BYPASS_SD,
+	SET_BYPASS_PWOFF_SD,
+	GET_BYPASS_PWOFF_SD,
+	SET_BYPASS_PWUP_SD,
+	GET_BYPASS_PWUP_SD,
+	SET_STD_NIC_SD,
+	GET_STD_NIC_SD,
+	SET_TX_SD,
+	GET_TX_SD,
+	SET_TAP_SD,
+	GET_TAP_SD,
+	GET_TAP_CHANGE_SD,
+	SET_DIS_TAP_SD,
+	GET_DIS_TAP_SD,
+	SET_TAP_PWUP_SD,
+	GET_TAP_PWUP_SD,
+	SET_WD_EXP_MODE_SD,
+	GET_WD_EXP_MODE_SD,
+	SET_WD_AUTORESET_SD,
+	GET_WD_AUTORESET_SD,
+	SET_TPL_SD,
+	GET_TPL_SD,
+	SET_DISC_SD,
+	GET_DISC_SD,
+	GET_DISC_CHANGE_SD,
+	SET_DIS_DISC_SD,
+	GET_DIS_DISC_SD,
+	SET_DISC_PWUP_SD,
+	GET_DISC_PWUP_SD,
+
+	GET_BYPASS_INFO_SD = 100,
+	GET_BP_WAIT_AT_PWUP_SD,
+	SET_BP_WAIT_AT_PWUP_SD,
+	GET_BP_HW_RESET_SD,
+	SET_BP_HW_RESET_SD,
+
+} CMND_TYPE_SD;
+
+#define SIOCGIFBYPASS SIOCDEVPRIVATE+10
+
+struct bp_info {
+	char prod_name[14];
+	unsigned char fw_ver;
+};
+
+/* for passing single values */
+struct if_bypass {
+	char if_name[IFNAMSIZ];
+	int cmd;
+	int data;
+};
+struct if_bypass_info {
+	char if_name[IFNAMSIZ];
+	char cmd;
+	struct bp_info bp_info;
+};
+
+/*
+* The major device number. We can't rely on dynamic
+* registration any more, because ioctls need to know
+* it.
+*/
+
+#define MAGIC_NUM 'J'
+
+/* for passing single values */
+struct bpctl_cmd {
+	int status;
+	int data[8];
+	int in_param[8];
+	int out_param[8];
+};
+
+#define IOCTL_TX_MSG(cmd) _IOWR(MAGIC_NUM, cmd, struct bpctl_cmd)
+
+#define DEVICE_NAME "bpctl"
+
+#endif
diff --git a/drivers/staging/silicom/bypasslib/bplibk.h b/drivers/staging/silicom/bypasslib/bplibk.h
new file mode 100644
index 000000000000..a1c85eec02f0
--- /dev/null
+++ b/drivers/staging/silicom/bypasslib/bplibk.h
@@ -0,0 +1,47 @@
+/******************************************************************************/
+/*                                                                            */
+/* bypass library, Copyright (c) 2004 Silicom, Ltd                            */
+/*                                                                            */
+/* This program is free software; you can redistribute it and/or modify       */
+/* it under the terms of the GNU General Public License as published by       */
+/* the Free Software Foundation, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/* bplib.h                                                                    */
+/*                                                                            */
+/******************************************************************************/
+#ifndef BYPASS_H
+#define BYPASS_H
+
+#include "bp_ioctl.h"
+#include "libbp_sd.h"
+
+#define IF_NAME            "eth"
+#define SILICOM_VID        0x1374
+#define SILICOM_BP_PID_MIN 0x24
+#define SILICOM_BP_PID_MAX 0x5f
+#define INTEL_PEG4BPII_PID  0x10a0
+#define INTEL_PEG4BPFII_PID 0x10a1
+
+#define PEGII_IF_SERIES(vid, pid) \
+        ((vid==0x8086)&& \
+        ((pid==INTEL_PEG4BPII_PID)||   \
+          (pid==INTEL_PEG4BPFII_PID)))
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10))
+#define pci_get_class pci_find_class
+
+#define pci_get_device pci_find_device
+
+#endif
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10))
+#define EXPORT_SYMBOL_NOVERS EXPORT_SYMBOL
+#endif
+
+#ifdef BP_VENDOR_SUPPORT
+char *bp_desc_array[] =
+    { "e1000bp", "e1000bpe", "slcm5700", "bnx2xbp", "ixgbp", "ixgbpe", NULL };
+#endif
+
+#endif
diff --git a/drivers/staging/silicom/bypasslib/bypass.c b/drivers/staging/silicom/bypasslib/bypass.c
new file mode 100644
index 000000000000..527829d58133
--- /dev/null
+++ b/drivers/staging/silicom/bypasslib/bypass.c
@@ -0,0 +1,529 @@
+/******************************************************************************/
+/*                                                                            */
+/* bypass library, Copyright (c) 2004-2007 Silicom, Ltd                       */
+/*                                                                            */
+/* This program is free software; you can redistribute it and/or modify       */
+/* it under the terms of the GNU General Public License as published by       */
+/* the Free Software Foundation, located in the file LICENSE.                 */
+/*                                                                            */
+/*                                                                            */
+/* bypass.c                                                                    */
+/*                                                                            */
+/******************************************************************************/
+
+#include <linux/version.h>
+#if defined(CONFIG_SMP) && ! defined(__SMP__)
+#define __SMP__
+#endif
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <asm/unistd.h>
+
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#include <linux/netdevice.h>	// struct device, and other headers
+#include <linux/kernel_stat.h>
+#include <linux/pci.h>
+#include <linux/rtnetlink.h>
+#include <linux/ethtool.h>
+
+#include <net/net_namespace.h>
+
+#include "bplibk.h"
+
+#define MOD_NAME "bypass"
+
+#define VERSION "\n"MOD_NAME" version 9.0.4\n"
+
+MODULE_AUTHOR("www.silicom.co.il");
+
+MODULE_LICENSE("GPL");
+
+int init_lib_module(void);
+void cleanup_lib_module(void);
+
+static int do_cmd(struct net_device *dev, struct ifreq *ifr, int cmd, int *data)
+{
+	int ret = -1;
+	struct if_bypass *bypass_cb;
+	static int (*ioctl) (struct net_device *, struct ifreq *, int);
+
+	bypass_cb = (struct if_bypass *)ifr;
+	bypass_cb->cmd = cmd;
+	bypass_cb->data = *data;
+	if ((dev->netdev_ops) && (ioctl = dev->netdev_ops->ndo_do_ioctl)) {
+		ret = ioctl(dev, ifr, SIOCGIFBYPASS);
+		*data = bypass_cb->data;
+	}
+
+	return ret;
+}
+
+static int doit(int cmd, int if_index, int *data)
+{
+	struct ifreq ifr;
+	int ret = -1;
+	struct net_device *dev;
+	struct net_device *n;
+	for_each_netdev_safe(&init_net, dev, n) {
+
+		if (dev->ifindex == if_index) {
+			ret = do_cmd(dev, &ifr, cmd, data);
+			if (ret < 0)
+				ret = -1;
+
+		}
+	}
+
+	return ret;
+}
+
+#define bp_symbol_get(fn_name) symbol_get(fn_name)
+#define bp_symbol_put(fn_name) symbol_put(fn_name)
+
+#define SET_BPLIB_INT_FN(fn_name, arg_type, arg, ret) \
+    ({ int (* fn_ex)(arg_type)=NULL; \
+    fn_ex=bp_symbol_get(fn_name##_sd); \
+       if(fn_ex) {  \
+        ret= fn_ex(arg); \
+       bp_symbol_put(fn_name##_sd); \
+       } else ret=-1; \
+    })
+
+#define  SET_BPLIB_INT_FN2(fn_name, arg_type, arg, arg_type1, arg1, ret) \
+    ({ int (* fn_ex)(arg_type,arg_type1)=NULL; \
+        fn_ex=bp_symbol_get(fn_name##_sd); \
+       if(fn_ex) {  \
+        ret= fn_ex(arg,arg1); \
+        bp_symbol_put(fn_name##_sd); \
+       } else ret=-1; \
+    })
+#define SET_BPLIB_INT_FN3(fn_name, arg_type, arg, arg_type1, arg1,arg_type2, arg2, ret) \
+    ({ int (* fn_ex)(arg_type,arg_type1, arg_type2)=NULL; \
+        fn_ex=bp_symbol_get(fn_name##_sd); \
+       if(fn_ex) {  \
+        ret= fn_ex(arg,arg1,arg2); \
+        bp_symbol_put(fn_name##_sd); \
+       } else ret=-1; \
+    })
+
+#define DO_BPLIB_GET_ARG_FN(fn_name,ioctl_val, if_index) \
+    ({    int data, ret=0; \
+            if(is_dev_sd(if_index)){ \
+            SET_BPLIB_INT_FN(fn_name, int, if_index, ret); \
+            return ret; \
+            }  \
+            return doit(ioctl_val,if_index, &data); \
+    })
+
+#define DO_BPLIB_SET_ARG_FN(fn_name,ioctl_val,if_index,arg) \
+    ({    int data, ret=0; \
+            if(is_dev_sd(if_index)){ \
+            SET_BPLIB_INT_FN2(fn_name, int, if_index, int, arg, ret); \
+            return ret; \
+            }  \
+	    data=arg; \
+            return doit(ioctl_val,if_index, &data); \
+    })
+
+static int is_dev_sd(int if_index)
+{
+	int ret = 0;
+	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
+	return (ret >= 0 ? 1 : 0);
+}
+
+int is_bypass_dev(int if_index)
+{
+	struct pci_dev *pdev = NULL;
+	struct net_device *dev = NULL;
+	struct ifreq ifr;
+	int ret = 0, data = 0;
+
+	while ((pdev = pci_get_class(PCI_CLASS_NETWORK_ETHERNET << 8, pdev))) {
+		if ((dev = pci_get_drvdata(pdev)) != NULL)
+			if (((dev = pci_get_drvdata(pdev)) != NULL) &&
+			    (dev->ifindex == if_index)) {
+				if ((pdev->vendor == SILICOM_VID) &&
+				    (pdev->device >= SILICOM_BP_PID_MIN) &&
+				    (pdev->device <= SILICOM_BP_PID_MAX))
+					goto send_cmd;
+#if defined(BP_VENDOR_SUPPORT) && defined(ETHTOOL_GDRVINFO)
+				else {
+					struct ethtool_drvinfo info;
+					const struct ethtool_ops *ops =
+					    dev->ethtool_ops;
+					int k = 0;
+
+					if (ops->get_drvinfo) {
+						memset(&info, 0, sizeof(info));
+						info.cmd = ETHTOOL_GDRVINFO;
+						ops->get_drvinfo(dev, &info);
+						for (; bp_desc_array[k]; k++)
+							if (!
+							    (strcmp
+							     (bp_desc_array[k],
+							      info.driver)))
+								goto send_cmd;
+
+					}
+
+				}
+#endif
+				return -1;
+			}
+	}
+ send_cmd:
+	ret = do_cmd(dev, &ifr, IS_BYPASS, &data);
+	return (ret < 0 ? -1 : ret);
+}
+
+int is_bypass(int if_index)
+{
+	int ret = 0;
+	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
+
+	if (ret < 0)
+		return is_bypass_dev(if_index);
+	return ret;
+}
+
+int get_bypass_slave(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass_slave, GET_BYPASS_SLAVE, if_index);
+}
+
+int get_bypass_caps(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass_caps, GET_BYPASS_CAPS, if_index);
+}
+
+int get_wd_set_caps(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_wd_set_caps, GET_WD_SET_CAPS, if_index);
+}
+
+int set_bypass(int if_index, int bypass_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_bypass, SET_BYPASS, if_index, bypass_mode);
+}
+
+int get_bypass(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass, GET_BYPASS, if_index);
+}
+
+int get_bypass_change(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass_change, GET_BYPASS_CHANGE, if_index);
+}
+
+int set_dis_bypass(int if_index, int dis_bypass)
+{
+	DO_BPLIB_SET_ARG_FN(set_dis_bypass, SET_DIS_BYPASS, if_index,
+			    dis_bypass);
+}
+
+int get_dis_bypass(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_dis_bypass, GET_DIS_BYPASS, if_index);
+}
+
+int set_bypass_pwoff(int if_index, int bypass_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_bypass_pwoff, SET_BYPASS_PWOFF, if_index,
+			    bypass_mode);
+}
+
+int get_bypass_pwoff(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass_pwoff, GET_BYPASS_PWOFF, if_index);
+}
+
+int set_bypass_pwup(int if_index, int bypass_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_bypass_pwup, SET_BYPASS_PWUP, if_index,
+			    bypass_mode);
+}
+
+int get_bypass_pwup(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bypass_pwup, GET_BYPASS_PWUP, if_index);
+}
+
+int set_bypass_wd(int if_index, int ms_timeout, int *ms_timeout_set)
+{
+	int data = ms_timeout, ret = 0;
+	if (is_dev_sd(if_index))
+		SET_BPLIB_INT_FN3(set_bypass_wd, int, if_index, int, ms_timeout,
+				  int *, ms_timeout_set, ret);
+	else {
+		ret = doit(SET_BYPASS_WD, if_index, &data);
+		if (ret > 0) {
+			*ms_timeout_set = ret;
+			ret = 0;
+		}
+	}
+	return ret;
+}
+
+int get_bypass_wd(int if_index, int *ms_timeout_set)
+{
+	int *data = ms_timeout_set, ret = 0;
+	if (is_dev_sd(if_index))
+		SET_BPLIB_INT_FN2(get_bypass_wd, int, if_index, int *,
+				  ms_timeout_set, ret);
+	else
+		ret = doit(GET_BYPASS_WD, if_index, data);
+	return ret;
+}
+
+int get_wd_expire_time(int if_index, int *ms_time_left)
+{
+	int *data = ms_time_left, ret = 0;
+	if (is_dev_sd(if_index))
+		SET_BPLIB_INT_FN2(get_wd_expire_time, int, if_index, int *,
+				  ms_time_left, ret);
+	else {
+		ret = doit(GET_WD_EXPIRE_TIME, if_index, data);
+		if ((ret == 0) && (*data != 0))
+			ret = 1;
+	}
+	return ret;
+}
+
+int reset_bypass_wd_timer(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(reset_bypass_wd_timer, RESET_BYPASS_WD_TIMER,
+			    if_index);
+}
+
+int set_std_nic(int if_index, int bypass_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_std_nic, SET_STD_NIC, if_index, bypass_mode);
+}
+
+int get_std_nic(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_std_nic, GET_STD_NIC, if_index);
+}
+
+int set_tx(int if_index, int tx_state)
+{
+	DO_BPLIB_SET_ARG_FN(set_tx, SET_TX, if_index, tx_state);
+}
+
+int get_tx(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tx, GET_TX, if_index);
+}
+
+int set_tap(int if_index, int tap_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_tap, SET_TAP, if_index, tap_mode);
+}
+
+int get_tap(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tap, GET_TAP, if_index);
+}
+
+int get_tap_change(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tap_change, GET_TAP_CHANGE, if_index);
+}
+
+int set_dis_tap(int if_index, int dis_tap)
+{
+	DO_BPLIB_SET_ARG_FN(set_dis_tap, SET_DIS_TAP, if_index, dis_tap);
+}
+
+int get_dis_tap(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_dis_tap, GET_DIS_TAP, if_index);
+}
+
+int set_tap_pwup(int if_index, int tap_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_tap_pwup, SET_TAP_PWUP, if_index, tap_mode);
+}
+
+int get_tap_pwup(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tap_pwup, GET_TAP_PWUP, if_index);
+}
+
+int set_bp_disc(int if_index, int disc_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_bp_disc, SET_DISC, if_index, disc_mode);
+}
+
+int get_bp_disc(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bp_disc, GET_DISC, if_index);
+}
+
+int get_bp_disc_change(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bp_disc_change, GET_DISC_CHANGE, if_index);
+}
+
+int set_bp_dis_disc(int if_index, int dis_disc)
+{
+	DO_BPLIB_SET_ARG_FN(set_bp_dis_disc, SET_DIS_DISC, if_index, dis_disc);
+}
+
+int get_bp_dis_disc(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bp_dis_disc, GET_DIS_DISC, if_index);
+}
+
+int set_bp_disc_pwup(int if_index, int disc_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_bp_disc_pwup, SET_DISC_PWUP, if_index,
+			    disc_mode);
+}
+
+int get_bp_disc_pwup(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_bp_disc_pwup, GET_DISC_PWUP, if_index);
+}
+
+int set_wd_exp_mode(int if_index, int mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_wd_exp_mode, SET_WD_EXP_MODE, if_index, mode);
+}
+
+int get_wd_exp_mode(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_wd_exp_mode, GET_WD_EXP_MODE, if_index);
+}
+
+int set_wd_autoreset(int if_index, int time)
+{
+	DO_BPLIB_SET_ARG_FN(set_wd_autoreset, SET_WD_AUTORESET, if_index, time);
+}
+
+int get_wd_autoreset(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_wd_autoreset, GET_WD_AUTORESET, if_index);
+}
+
+int set_tpl(int if_index, int tpl_mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_tpl, SET_TPL, if_index, tpl_mode);
+}
+
+int get_tpl(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tpl, GET_TPL, if_index);
+}
+
+int set_bp_hw_reset(int if_index, int mode)
+{
+	DO_BPLIB_SET_ARG_FN(set_tpl, SET_BP_HW_RESET, if_index, mode);
+}
+
+int get_bp_hw_reset(int if_index)
+{
+	DO_BPLIB_GET_ARG_FN(get_tpl, GET_BP_HW_RESET, if_index);
+}
+
+int get_bypass_info(int if_index, struct bp_info *bp_info)
+{
+	int ret = 0;
+	if (is_dev_sd(if_index)) {
+		SET_BPLIB_INT_FN2(get_bypass_info, int, if_index,
+				  struct bp_info *, bp_info, ret);
+	} else {
+		static int (*ioctl) (struct net_device *, struct ifreq *, int);
+		struct net_device *dev;
+
+		struct net_device *n;
+		for_each_netdev_safe(&init_net, dev, n) {
+			if (dev->ifindex == if_index) {
+				struct if_bypass_info *bypass_cb;
+				struct ifreq ifr;
+
+				memset(&ifr, 0, sizeof(ifr));
+				bypass_cb = (struct if_bypass_info *)&ifr;
+				bypass_cb->cmd = GET_BYPASS_INFO;
+
+				if ((dev->netdev_ops) &&
+				    (ioctl = dev->netdev_ops->ndo_do_ioctl)) {
+					ret = ioctl(dev, &ifr, SIOCGIFBYPASS);
+				}
+
+				else
+					ret = -1;
+				if (ret == 0)
+					memcpy(bp_info, &bypass_cb->bp_info,
+					       sizeof(struct bp_info));
+				ret = (ret < 0 ? -1 : 0);
+				break;
+			}
+		}
+	}
+	return ret;
+}
+
+int init_lib_module()
+{
+
+	printk(VERSION);
+	return 0;
+}
+
+void cleanup_lib_module()
+{
+}
+
+EXPORT_SYMBOL_NOVERS(is_bypass);
+EXPORT_SYMBOL_NOVERS(get_bypass_slave);
+EXPORT_SYMBOL_NOVERS(get_bypass_caps);
+EXPORT_SYMBOL_NOVERS(get_wd_set_caps);
+EXPORT_SYMBOL_NOVERS(set_bypass);
+EXPORT_SYMBOL_NOVERS(get_bypass);
+EXPORT_SYMBOL_NOVERS(get_bypass_change);
+EXPORT_SYMBOL_NOVERS(set_dis_bypass);
+EXPORT_SYMBOL_NOVERS(get_dis_bypass);
+EXPORT_SYMBOL_NOVERS(set_bypass_pwoff);
+EXPORT_SYMBOL_NOVERS(get_bypass_pwoff);
+EXPORT_SYMBOL_NOVERS(set_bypass_pwup);
+EXPORT_SYMBOL_NOVERS(get_bypass_pwup);
+EXPORT_SYMBOL_NOVERS(set_bypass_wd);
+EXPORT_SYMBOL_NOVERS(get_bypass_wd);
+EXPORT_SYMBOL_NOVERS(get_wd_expire_time);
+EXPORT_SYMBOL_NOVERS(reset_bypass_wd_timer);
+EXPORT_SYMBOL_NOVERS(set_std_nic);
+EXPORT_SYMBOL_NOVERS(get_std_nic);
+EXPORT_SYMBOL_NOVERS(set_tx);
+EXPORT_SYMBOL_NOVERS(get_tx);
+EXPORT_SYMBOL_NOVERS(set_tap);
+EXPORT_SYMBOL_NOVERS(get_tap);
+EXPORT_SYMBOL_NOVERS(get_tap_change);
+EXPORT_SYMBOL_NOVERS(set_dis_tap);
+EXPORT_SYMBOL_NOVERS(get_dis_tap);
+EXPORT_SYMBOL_NOVERS(set_tap_pwup);
+EXPORT_SYMBOL_NOVERS(get_tap_pwup);
+EXPORT_SYMBOL_NOVERS(set_bp_disc);
+EXPORT_SYMBOL_NOVERS(get_bp_disc);
+EXPORT_SYMBOL_NOVERS(get_bp_disc_change);
+EXPORT_SYMBOL_NOVERS(set_bp_dis_disc);
+EXPORT_SYMBOL_NOVERS(get_bp_dis_disc);
+EXPORT_SYMBOL_NOVERS(set_bp_disc_pwup);
+EXPORT_SYMBOL_NOVERS(get_bp_disc_pwup);
+EXPORT_SYMBOL_NOVERS(set_wd_exp_mode);
+EXPORT_SYMBOL_NOVERS(get_wd_exp_mode);
+EXPORT_SYMBOL_NOVERS(set_wd_autoreset);
+EXPORT_SYMBOL_NOVERS(get_wd_autoreset);
+EXPORT_SYMBOL_NOVERS(set_tpl);
+EXPORT_SYMBOL_NOVERS(get_tpl);
+EXPORT_SYMBOL_NOVERS(set_bp_hw_reset);
+EXPORT_SYMBOL_NOVERS(get_bp_hw_reset);
+EXPORT_SYMBOL_NOVERS(get_bypass_info);
+
+module_init(init_lib_module);
+module_exit(cleanup_lib_module);
diff --git a/drivers/staging/silicom/bypasslib/libbp_sd.h b/drivers/staging/silicom/bypasslib/libbp_sd.h
new file mode 100644
index 000000000000..3b4f8364ed18
--- /dev/null
+++ b/drivers/staging/silicom/bypasslib/libbp_sd.h
@@ -0,0 +1,509 @@
+/******************************************************************************/
+/*                                                                            */
+/* bypass library, Copyright (c) 2004 Silicom, Ltd                            */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/* Ver 1.0.0                                                                  */
+/*                                                                            */
+/* libbypass.h                                                                */
+/*                                                                            */
+/******************************************************************************/
+
+/**
+ * is_bypass - check if device is a Bypass controlling device
+ * @if_index: network device index
+ *
+ * Output:
+ *  1 -  if device is bypass controlling device, 
+ *  0 -  if device is bypass slave device
+ * -1 -  device not support Bypass
+ **/
+int is_bypass_sd(int if_index);
+
+/**
+ * get_bypass_slave - get second port participate in the Bypass pair
+ * @if_index: network device index
+ *
+ * Output:
+ *  network device index of the slave device
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int get_bypass_slave_sd(int if_index);
+
+/**
+ * get_bypass_caps - get second port participate in the Bypass pair
+ * @if_index: network device index
+ *
+ * Output:
+ * flags word on success;flag word is a 32-bit mask word with each bit defines different 
+ * capability as described bellow.
+ * Value of 1 for supporting this feature. 0 for not supporting this feature.
+ * -1 - on failure (if the device is not capable of the operation or not a Bypass device)
+ * Bit	feature	                description
+ * 
+ * 0	BP_CAP	                The interface is Bypass capable in general
+ * 
+ * 1	BP_STATUS_CAP	        The interface can report of the current Bypass mode
+ * 
+ * 2	BP_STATUS_CHANGE_CAP	The interface can report on a change to bypass mode from 
+ *                              the last time the mode was defined
+ * 
+ * 3	SW_CTL_CAP	            The interface is Software controlled capable for bypass/non bypass modes.
+ * 
+ * 4	BP_DIS_CAP	            The interface is capable of disabling the Bypass mode at all times. 
+ *                              This mode will retain its mode even during power loss and also after 
+ *                              power recovery. This will overcome on any bypass operation due to 
+ *                              watchdog timeout or set bypass command.
+ * 
+ * 5	BP_DIS_STATUS_CAP	    The interface can report of the current DIS_BP_CAP
+ * 
+ * 6	STD_NIC_CAP	            The interface is capable to be configured to operate as standard, non Bypass, 
+ *                              NIC interface (have direct connection to interfaces at all power modes)
+ * 
+ * 7	BP_PWOFF_NO_CAP	        The interface can be in Bypass mode at power off state
+ * 
+ * 8	BP_PWOFF_OFF_CAP	    The interface can disconnect the Bypass mode at power off state without 
+ *                              effecting all the other states of operation
+ * 
+ * 9	BP_PWOFF_CTL_CAP	    The behavior of the Bypass mode at Power-off state can be controlled by 
+ *                              software without effecting any other state
+ * 
+ *10    BP_PWUP_ON_CAP	        The interface can be in Bypass mode when power is turned on 
+ *                              (until the system take control of the bypass functionality)
+ * 
+ *11	BP_PWUP_OFF_CAP	        The interface can disconnect from Bypass mode when power is turned on 
+ *                              (until the system take control of the bypass functionality)
+ * 
+ *12	BP_PWUP_CTL_CAP	        The behavior of the Bypass mode at Power-up can be controlled by software
+ * 
+ *13	WD_CTL_CAP	            The interface has watchdog capabilities to turn to Bypass mode when not reset 
+ *                              for defined period of time.
+ * 
+ *14	WD_STATUS_CAP	        The interface can report on the watchdog status (Active/inactive)
+ * 
+ *15	WD_TIMEOUT_CAP	        The interface can report the time left till watchdog triggers to Bypass mode.
+ * 
+ *16-31 RESERVED	
+ *
+ * **/
+int get_bypass_caps_sd(int if_index);
+
+/**
+ * get_wd_set_caps - Obtain watchdog timer setting capabilities
+ * @if_index: network device index
+ *
+ * Output:
+ * 
+ * Set of numbers defining the various parameters of the watchdog capable 
+ * to be set to as described bellow.
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ * 
+ * Bit	feature	        description
+ * 
+ * 0-3	WD_MIN_TIME	    The interface WD minimal time period  in 100mS units
+ * 
+ * 4	WD_STEP_TIME	The steps of the WD timer in 
+ *                      0 - for linear steps (WD_MIN_TIME * X)
+ *                      1 - for multiply by 2 from previous step (WD_MIN_TIME * 2^X)
+ * 
+ * 5-8	WD_STEP_COUNT	Number of steps the WD timer supports in 2^X 
+ *                      (X bit available for defining the value)
+ * 
+ * 
+ * 
+ **/
+int get_wd_set_caps_sd(int if_index);
+
+/**
+ * set_bypass - set Bypass state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  bypass mode (1=on, 0=off) 
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int set_bypass_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass - Get Bypass mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int get_bypass_sd(int if_index);
+
+/**
+ * get_bypass_change - Get change of Bypass mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int get_bypass_change_sd(int if_index);
+
+/**
+ * set_dis_bypass - Set Disable Bypass mode
+ * @if_index: network device index of the controlling device
+ * @dis_bypass: disable bypass(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int set_dis_bypass_sd(int if_index, int dis_bypass);
+
+/**
+ * get_dis_bypass - Get Disable Bypass mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal Bypass mode/ Disable bypass)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int get_dis_bypass_sd(int if_index);
+
+/**
+ * set_bypass_pwoff - Set Bypass mode at power-off state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: bypass mode setting at power off state (1=BP en, 0=BP Dis)
+ * Output:
+ *  0 - on success 
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int set_bypass_pwoff_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass_pwoff - Get Bypass mode state at power-off state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable bypass at power off state / normal Bypass mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int get_bypass_pwoff_sd(int if_index);
+
+/**
+ * set_bypass_pwup - Set Bypass mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: bypass mode setting at power up state (1=BP en, 0=BP Dis)
+ * Output:
+ *  0 - on success 
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int set_bypass_pwup_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass_pwup - Get Bypass mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable bypass at power up state / normal Bypass mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int get_bypass_pwup_sd(int if_index);
+
+/**
+ * set_bypass_wd - Set watchdog state
+ * @if_index: network device index of the controlling device
+ * @ms_timeout: requested timeout (in ms units), 0 for disabling the watchdog timer
+ * @ms_timeout_set(output): requested timeout (in ms units), 
+ *                          that the adapter supports and will be used by the watchdog
+ * Output:
+ * 0  - on success 
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int set_bypass_wd_sd(int if_index, int ms_timeout, int *ms_timeout_set);
+
+/**
+ * get_bypass_wd - Get watchdog state
+ * @if_index: network device index of the controlling device
+ * @ms_timeout (output): WDT timeout (in ms units), 
+ *                       -1 for unknown wdt status
+ *                        0 if WDT is disabled
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int get_bypass_wd_sd(int if_index, int *ms_timeout_set);
+
+/**
+ * get_wd_expire_time - Get watchdog expire
+ * @if_index: network device index of the controlling device
+ * @ms_time_left (output): time left till watchdog time expire, 
+ *                       -1 if WDT has expired
+ *                       0  if WDT is disabled
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device or unknown wdt status) 
+ **/
+int get_wd_expire_time_sd(int if_index, int *ms_time_left);
+
+/**
+ * reset_bypass_wd_timer - Reset watchdog timer
+ * @if_index: network device index of the controlling device
+ * 
+ * Output:
+ * 1  - on success
+ * 0 - watchdog is not configured
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device or unknown wdt status) 
+ **/
+int reset_bypass_wd_timer_sd(int if_index);
+
+/**
+ * set_std_nic - Standard NIC mode of operation
+ * @if_index: network device index of the controlling device
+ * @nic_mode: 0/1 (Default Bypass mode / Standard NIC mode)
+ * 
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int set_std_nic_sd(int if_index, int nic_mode);
+
+/**
+ * get_std_nic - Get Standard NIC mode setting
+ * @if_index: network device index of the controlling device
+ * 
+ * Output:
+ * 0/1 (Default Bypass mode / Standard NIC mode) on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device) 
+ **/
+int get_std_nic_sd(int if_index);
+
+/**
+ * set_tx - set transmitter enable/disable 
+ * @if_index: network device index of the controlling device
+ * @tx_state: 0/1 (Transmit Disable / Transmit Enable)
+ * 
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ) 
+ **/
+int set_tx_sd(int if_index, int tx_state);
+
+/**
+ * get_std_nic - get transmitter state (disable / enable)
+ * @if_index: network device index of the controlling device
+ * 
+ * Output:
+ * 0/1 (ransmit Disable / Transmit Enable) on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass) 
+ **/
+int get_tx_sd(int if_index);
+
+/**
+ * set_tap - set TAP state
+ * @if_index: network device index of the controlling device
+ * @tap_mode: 1 tap mode , 0 normal nic mode 
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support TAP or it's a slave device) 
+ **/
+int set_tap_sd(int if_index, int tap_mode);
+
+/**
+ * get_tap - Get TAP mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support TAP or it's a slave device) 
+ **/
+int get_tap_sd(int if_index);
+
+/**
+ * get_tap_change - Get change of TAP mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support TAP or it's a slave device) 
+ **/
+int get_tap_change_sd(int if_index);
+
+/**
+ * set_dis_tap - Set Disable TAP mode
+ * @if_index: network device index of the controlling device
+ * @dis_tap: disable tap(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP 
+ *                  or it's a slave device) 
+ **/
+int set_dis_tap_sd(int if_index, int dis_tap);
+
+/**
+ * get_dis_tap - Get Disable TAP mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal TAP mode/ Disable TAP)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP 
+ *                  or it's a slave device) 
+ **/
+int get_dis_tap_sd(int if_index);
+
+/**
+ * set_tap_pwup - Set TAP mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: tap mode setting at power up state (1=TAP en, 0=TAP Dis)
+ * Output:
+ *  0 - on success 
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP 
+ *                  or it's a slave device) 
+ **/
+int set_tap_pwup_sd(int if_index, int tap_mode);
+
+/**
+ * get_tap_pwup - Get TAP mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable TAP at power up state / normal TAP mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP 
+ *                  or it's a slave device) 
+ **/
+int get_tap_pwup_sd(int if_index);
+
+/**
+ * set_bp_disc - set Disconnect state
+ * @if_index: network device index of the controlling device
+ * @tap_mode: 1 disc mode , 0 non-disc mode 
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support Disconnect or it's a slave device) 
+ **/
+int set_bp_disc_sd(int if_index, int disc_mode);
+
+/**
+ * get_bp_disc - Get Disconnect mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Disconnect or it's a slave device) 
+ **/
+int get_bp_disc_sd(int if_index);
+
+/**
+ * get_bp_disc_change - Get change of Disconnect mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Disconnect or it's a slave device) 
+ **/
+int get_bp_disc_change_sd(int if_index);
+
+/**
+ * set_bp_dis_disc - Set Disable Disconnect mode
+ * @if_index: network device index of the controlling device
+ * @dis_tap: disable tap(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable ofthe operation ordevice not support Disconnect 
+ *                  or it's a slave device) 
+ **/
+int set_bp_dis_disc_sd(int if_index, int dis_disc);
+
+/**
+ * get_dis_tap - Get Disable Disconnect mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal Disconnect mode/ Disable Disconnect)
+ * -1 - on failure (device is not capable of the operation ordevice not support Disconnect 
+ *                  or it's a slave device) 
+ **/
+int get_bp_dis_disc_sd(int if_index);
+
+/**
+ * set_bp_disc_pwup - Set Disconnect mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @disc_mode: tap mode setting at power up state (1=Disc en, 0=Disc Dis)
+ * Output:
+ *  0 - on success 
+ * -1 - on failure (device is not capable of the operation ordevice not support Disconnect 
+ *                  or it's a slave device) 
+ **/
+int set_bp_disc_pwup_sd(int if_index, int disc_mode);
+
+/**
+ * get_bp_disc_pwup - Get Disconnect mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable Disconnect at power up state / normal Disconnect mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP 
+ *                  or it's a slave device) 
+ **/
+int get_bp_disc_pwup_sd(int if_index);
+
+/**
+ * set_wd_exp_mode - Set adapter state when WDT expired.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode) 
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int set_wd_exp_mode_sd(int if_index, int bypass_mode);
+
+/**
+ * get_wd_exp_mode - Get adapter state when WDT expired.
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (bypass/tap) on success
+ * -1 - on failure (device not support Bypass or it's a slave device) 
+ **/
+int get_wd_exp_mode_sd(int if_index);
+
+/**
+ * set_wd_autoreset - reset WDT periodically.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode) 
+ * Output:
+ * 1  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device or unknown wdt status) 
+ **/
+int set_wd_autoreset_sd(int if_index, int time);
+
+/**
+ * set_wd_autoreset - reset WDT periodically.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode) 
+ * Output:
+ * 1  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass 
+ *                  or it's a slave device or unknown wdt status) 
+ **/
+int get_wd_autoreset_sd(int if_index);
+
+/**
+ * set_tpl - set TPL state
+ * @if_index: network device index of the controlling device
+ * @tpl_mode: 1 tpl mode , 0 normal nic mode 
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support TPL) 
+ **/
+int set_tpl_sd(int if_index, int tpl_mode);
+
+/**
+ * get_tpl - Get TPL mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support TPL or it's a slave device) 
+ **/
+int get_tpl_sd(int if_index);
+
+int get_bypass_info_sd(int if_index, struct bp_info *bp_info);
+int bp_if_scan_sd(void);
+/*int get_dev_num_sd(void);*/
diff --git a/drivers/staging/silicom/libbp_sd.h b/drivers/staging/silicom/libbp_sd.h
new file mode 100644
index 000000000000..065277f81c78
--- /dev/null
+++ b/drivers/staging/silicom/libbp_sd.h
@@ -0,0 +1,550 @@
+/******************************************************************************/
+/*                                                                            */
+/* bypass library, Copyright (c) 2004 Silicom, Ltd                            */
+/* 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, located in the file LICENSE.                 */
+/*                                                                            */
+/* Ver 1.0.0                                                                  */
+/*                                                                            */
+/* libbypass.h                                                                */
+/*                                                                            */
+/******************************************************************************/
+
+#define BP_CAP                   0x01	/* BIT_0 */
+#define BP_STATUS_CAP            0x02
+#define BP_STATUS_CHANGE_CAP     0x04
+#define SW_CTL_CAP               0x08
+#define BP_DIS_CAP               0x10
+#define BP_DIS_STATUS_CAP        0x20
+#define STD_NIC_CAP              0x40
+#define BP_PWOFF_ON_CAP          0x80
+#define BP_PWOFF_OFF_CAP         0x0100
+#define BP_PWOFF_CTL_CAP         0x0200
+#define BP_PWUP_ON_CAP           0x0400
+#define BP_PWUP_OFF_CAP          0x0800
+#define BP_PWUP_CTL_CAP          0x1000
+#define WD_CTL_CAP               0x2000
+#define WD_STATUS_CAP            0x4000
+#define WD_TIMEOUT_CAP           0x8000
+#define TX_CTL_CAP               0x10000
+#define TX_STATUS_CAP            0x20000
+#define TAP_CAP                  0x40000
+#define TAP_STATUS_CAP           0x80000
+#define TAP_STATUS_CHANGE_CAP    0x100000
+#define TAP_DIS_CAP              0x200000
+#define TAP_DIS_STATUS_CAP       0x400000
+#define TAP_PWUP_ON_CAP          0x800000
+#define TAP_PWUP_OFF_CAP         0x1000000
+#define TAP_PWUP_CTL_CAP         0x2000000
+#define NIC_CAP_NEG              0x4000000	/* BIT 26 */
+
+#define WD_MIN_TIME_GET(desc)   (desc & 0xf)
+#define WDT_STEP_TIME           0x10
+
+struct bp_info {
+	char prod_name[14];
+	unsigned char fw_ver;
+};
+
+/**
+ * is_bypass - check if device is a Bypass controlling device
+ * @if_index: network device index
+ *
+ * Output:
+ *  1 -  if device is bypass controlling device,
+ *  0 -  if device is bypass slave device
+ * -1 -  device not support Bypass
+ **/
+int is_bypass_sd(int if_index);
+
+/**
+ * get_bypass_slave - get second port participate in the Bypass pair
+ * @if_index: network device index
+ *
+ * Output:
+ *  network device index of the slave device
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int get_bypass_slave_sd(int if_index);
+
+/**
+ * get_bypass_caps - get second port participate in the Bypass pair
+ * @if_index: network device index
+ *
+ * Output:
+ * flags word on success;flag word is a 32-bit mask word with each bit defines different
+ * capability as described bellow.
+ * Value of 1 for supporting this feature. 0 for not supporting this feature.
+ * -1 - on failure (if the device is not capable of the operation or not a Bypass device)
+ * Bit	feature	                description
+ *
+ * 0	BP_CAP	                The interface is Bypass capable in general
+ *
+ * 1	BP_STATUS_CAP	        The interface can report of the current Bypass mode
+ *
+ * 2	BP_STATUS_CHANGE_CAP	The interface can report on a change to bypass mode from
+ *                              the last time the mode was defined
+ *
+ * 3	SW_CTL_CAP	            The interface is Software controlled capable for bypass/non bypass modes.
+ *
+ * 4	BP_DIS_CAP	            The interface is capable of disabling the Bypass mode at all times.
+ *                              This mode will retain its mode even during power loss and also after
+ *                              power recovery. This will overcome on any bypass operation due to
+ *                              watchdog timeout or set bypass command.
+ *
+ * 5	BP_DIS_STATUS_CAP	    The interface can report of the current DIS_BP_CAP
+ *
+ * 6	STD_NIC_CAP	            The interface is capable to be configured to operate as standard, non Bypass,
+ *                              NIC interface (have direct connection to interfaces at all power modes)
+ *
+ * 7	BP_PWOFF_NO_CAP	        The interface can be in Bypass mode at power off state
+ *
+ * 8	BP_PWOFF_OFF_CAP	    The interface can disconnect the Bypass mode at power off state without
+ *                              effecting all the other states of operation
+ *
+ * 9	BP_PWOFF_CTL_CAP	    The behavior of the Bypass mode at Power-off state can be controlled by
+ *                              software without effecting any other state
+ *
+ *10    BP_PWUP_ON_CAP	        The interface can be in Bypass mode when power is turned on
+ *                              (until the system take control of the bypass functionality)
+ *
+ *11	BP_PWUP_OFF_CAP	        The interface can disconnect from Bypass mode when power is turned on
+ *                              (until the system take control of the bypass functionality)
+ *
+ *12	BP_PWUP_CTL_CAP	        The behavior of the Bypass mode at Power-up can be controlled by software
+ *
+ *13	WD_CTL_CAP	            The interface has watchdog capabilities to turn to Bypass mode when not reset
+ *                              for defined period of time.
+ *
+ *14	WD_STATUS_CAP	        The interface can report on the watchdog status (Active/inactive)
+ *
+ *15	WD_TIMEOUT_CAP	        The interface can report the time left till watchdog triggers to Bypass mode.
+ *
+ *16-31 RESERVED
+ *
+ * **/
+int get_bypass_caps_sd(int if_index);
+
+/**
+ * get_wd_set_caps - Obtain watchdog timer setting capabilities
+ * @if_index: network device index
+ *
+ * Output:
+ *
+ * Set of numbers defining the various parameters of the watchdog capable
+ * to be set to as described bellow.
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ *
+ * Bit	feature	        description
+ *
+ * 0-3	WD_MIN_TIME	    The interface WD minimal time period  in 100mS units
+ *
+ * 4	WD_STEP_TIME	The steps of the WD timer in
+ *                      0 - for linear steps (WD_MIN_TIME * X)
+ *                      1 - for multiply by 2 from previous step (WD_MIN_TIME * 2^X)
+ *
+ * 5-8	WD_STEP_COUNT	Number of steps the WD timer supports in 2^X
+ *                      (X bit available for defining the value)
+ *
+ *
+ *
+ **/
+int get_wd_set_caps_sd(int if_index);
+
+/**
+ * set_bypass - set Bypass state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  bypass mode (1=on, 0=off)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int set_bypass_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass - Get Bypass mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int get_bypass_sd(int if_index);
+
+/**
+ * get_bypass_change - Get change of Bypass mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int get_bypass_change_sd(int if_index);
+
+/**
+ * set_dis_bypass - Set Disable Bypass mode
+ * @if_index: network device index of the controlling device
+ * @dis_bypass: disable bypass(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int set_dis_bypass_sd(int if_index, int dis_bypass);
+
+/**
+ * get_dis_bypass - Get Disable Bypass mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal Bypass mode/ Disable bypass)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int get_dis_bypass_sd(int if_index);
+
+/**
+ * set_bypass_pwoff - Set Bypass mode at power-off state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: bypass mode setting at power off state (1=BP en, 0=BP Dis)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int set_bypass_pwoff_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass_pwoff - Get Bypass mode state at power-off state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable bypass at power off state / normal Bypass mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int get_bypass_pwoff_sd(int if_index);
+
+/**
+ * set_bypass_pwup - Set Bypass mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: bypass mode setting at power up state (1=BP en, 0=BP Dis)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int set_bypass_pwup_sd(int if_index, int bypass_mode);
+
+/**
+ * get_bypass_pwup - Get Bypass mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable bypass at power up state / normal Bypass mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int get_bypass_pwup_sd(int if_index);
+
+/**
+ * set_bypass_wd - Set watchdog state
+ * @if_index: network device index of the controlling device
+ * @ms_timeout: requested timeout (in ms units), 0 for disabling the watchdog timer
+ * @ms_timeout_set(output): requested timeout (in ms units),
+ *                          that the adapter supports and will be used by the watchdog
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int set_bypass_wd_sd(int if_index, int ms_timeout, int *ms_timeout_set);
+
+/**
+ * get_bypass_wd - Get watchdog state
+ * @if_index: network device index of the controlling device
+ * @ms_timeout (output): WDT timeout (in ms units),
+ *                       -1 for unknown wdt status
+ *                        0 if WDT is disabled
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int get_bypass_wd_sd(int if_index, int *ms_timeout_set);
+
+/**
+ * get_wd_expire_time - Get watchdog expire
+ * @if_index: network device index of the controlling device
+ * @ms_time_left (output): time left till watchdog time expire,
+ *                       -1 if WDT has expired
+ *                       0  if WDT is disabled
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device or unknown wdt status)
+ **/
+int get_wd_expire_time_sd(int if_index, int *ms_time_left);
+
+/**
+ * reset_bypass_wd_timer - Reset watchdog timer
+ * @if_index: network device index of the controlling device
+ *
+ * Output:
+ * 1  - on success
+ * 0 - watchdog is not configured
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device or unknown wdt status)
+ **/
+int reset_bypass_wd_timer_sd(int if_index);
+
+/**
+ * set_std_nic - Standard NIC mode of operation
+ * @if_index: network device index of the controlling device
+ * @nic_mode: 0/1 (Default Bypass mode / Standard NIC mode)
+ *
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int set_std_nic_sd(int if_index, int nic_mode);
+
+/**
+ * get_std_nic - Get Standard NIC mode setting
+ * @if_index: network device index of the controlling device
+ *
+ * Output:
+ * 0/1 (Default Bypass mode / Standard NIC mode) on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device)
+ **/
+int get_std_nic_sd(int if_index);
+
+/**
+ * set_tx - set transmitter enable/disable
+ * @if_index: network device index of the controlling device
+ * @tx_state: 0/1 (Transmit Disable / Transmit Enable)
+ *
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation )
+ **/
+int set_tx_sd(int if_index, int tx_state);
+
+/**
+ * get_tx - get transmitter state (disable / enable)
+ * @if_index: network device index of the controlling device
+ *
+ * Output:
+ * 0/1 (ransmit Disable / Transmit Enable) on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass)
+ **/
+int get_tx_sd(int if_index);
+
+/**
+ * set_tpl - set TPL enable/disable
+ * @if_index: network device index of the controlling device
+ * @tx_state: 0/1 (TPL Disable / TPL Enable)
+ *
+ * Output:
+ * 0  - on success
+ * -1 - on failure (device is not capable of the operation )
+ **/
+int set_tpl_sd(int if_index, int tpl_state);
+
+/**
+ * get_tpl - get TPL state (disable / enable)
+ * @if_index: network device index of the controlling device
+ *
+ * Output:
+ * 0/1 (TPL Disable / TPL Enable) on success
+ * -1 - on failure (device is not capable of the operation)
+ **/
+int get_tpl_sd(int if_index);
+
+int get_bp_hw_reset_sd(int if_index);
+
+int set_bp_hw_reset_sd(int if_index, int status);
+
+/**
+ * set_tap - set TAP state
+ * @if_index: network device index of the controlling device
+ * @tap_mode: 1 tap mode , 0 normal nic mode
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support TAP or it's a slave device)
+ **/
+int set_tap_sd(int if_index, int tap_mode);
+
+/**
+ * get_tap - Get TAP mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support TAP or it's a slave device)
+ **/
+int get_tap_sd(int if_index);
+
+/**
+ * get_tap_change - Get change of TAP mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support TAP or it's a slave device)
+ **/
+int get_tap_change_sd(int if_index);
+
+/**
+ * set_dis_tap - Set Disable TAP mode
+ * @if_index: network device index of the controlling device
+ * @dis_tap: disable tap(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP
+ *                  or it's a slave device)
+ **/
+int set_dis_tap_sd(int if_index, int dis_tap);
+
+/**
+ * get_dis_tap - Get Disable TAP mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal TAP mode/ Disable TAP)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP
+ *                  or it's a slave device)
+ **/
+int get_dis_tap_sd(int if_index);
+
+/**
+ * set_tap_pwup - Set TAP mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @bypass_mode: tap mode setting at power up state (1=TAP en, 0=TAP Dis)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP
+ *                  or it's a slave device)
+ **/
+int set_tap_pwup_sd(int if_index, int tap_mode);
+
+/**
+ * get_tap_pwup - Get TAP mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable TAP at power up state / normal TAP mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP
+ *                  or it's a slave device)
+ **/
+int get_tap_pwup_sd(int if_index);
+
+/**
+ * set_wd_exp_mode - Set adapter state when WDT expired.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int set_wd_exp_mode_sd(int if_index, int bypass_mode);
+
+/**
+ * get_wd_exp_mode - Get adapter state when WDT expired.
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (bypass/tap) on success
+ * -1 - on failure (device not support Bypass or it's a slave device)
+ **/
+int get_wd_exp_mode_sd(int if_index);
+
+/**
+ * set_wd_autoreset - reset WDT periodically.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode)
+ * Output:
+ * 1  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device or unknown wdt status)
+ **/
+int set_wd_autoreset_sd(int if_index, int time);
+
+/**
+ * set_wd_autoreset - reset WDT periodically.
+ * @if_index: network device index of the controlling device
+ * @bypass_mode:  adapter mode (1=tap mode, 0=bypass mode)
+ * Output:
+ * 1  - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support Bypass
+ *                  or it's a slave device or unknown wdt status)
+ **/
+int get_wd_autoreset_sd(int if_index);
+/**
+ * set_disc - set DISC state
+ * @if_index: network device index of the controlling device
+ * @tap_mode: 1 DISC mode , 0 normal nic mode
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device not support disconnect or it's a slave device)
+ **/
+int set_bp_disc_sd(int if_index, int disc_mode);
+
+/**
+ * get_disc - Get disc mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support disconnect or it's a slave device)
+ **/
+int get_bp_disc_sd(int if_index);
+
+/**
+ * get_disc_change - Get change of DISC mode state from last status check
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - (off/on) on success
+ * -1 - on failure (device not support disconnect or it's a slave device)
+ **/
+int get_bp_disc_change_sd(int if_index);
+
+/**
+ * set_dis_disc - Set Disable DISC mode
+ * @if_index: network device index of the controlling device
+ * @dis_disc: disable disconnect(1=dis, 0=en)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support DISC
+ *                  or it's a slave device)
+ **/
+int set_bp_dis_disc_sd(int if_index, int dis_disc);
+
+/**
+ * get_dis_disc - Get Disable DISC mode state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (normal DISC mode/ Disable DISC)
+ * -1 - on failure (device is not capable of the operation ordevice not support TAP
+ *                  or it's a slave device)
+ **/
+int get_bp_dis_disc_sd(int if_index);
+
+/**
+ * set_disc_pwup - Set DISC mode at power-up state
+ * @if_index: network device index of the controlling device
+ * @disc_mode: DISC mode setting at power up state (1= en, 0= Dis)
+ * Output:
+ *  0 - on success
+ * -1 - on failure (device is not capable of the operation ordevice not support DISC
+ *                  or it's a slave device)
+ **/
+int set_bp_disc_pwup_sd(int if_index, int disc_mode);
+
+/**
+ * get_disc_pwup - Get DISC mode state at power-up state
+ * @if_index: network device index of the controlling device
+ * Output:
+ *  0/1 - on success (Disable DISC at power up state / normal DISC mode)
+ * -1 - on failure (device is not capable of the operation ordevice not support DISC
+ *                  or it's a slave device)
+ **/
+int get_bp_disc_pwup_sd(int if_index);
+
+int get_bypass_info_sd(int if_index, struct bp_info *bp_info);
+int bp_if_scan_sd(void);
+/*int get_dev_num_sd(void);*/
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 56829fc032ff..cd920dad85cd 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -514,8 +514,7 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 		file = "slicoss/gbrcvucode.sys";
 		break;
 	default:
-		ASSERT(0);
-		break;
+		return -ENOENT;
 	}
 
 	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
@@ -529,15 +528,16 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 	index += 4;
 	switch (adapter->devid) {
 	case SLIC_2GB_DEVICE_ID:
-		if (rcvucodelen != OasisRcvUCodeLen)
+		if (rcvucodelen != OasisRcvUCodeLen) {
+			release_firmware(fw);
 			return -EINVAL;
+		}
 		break;
 	case SLIC_1GB_DEVICE_ID:
-		if (rcvucodelen != GBRcvUCodeLen)
+		if (rcvucodelen != GBRcvUCodeLen) {
+			release_firmware(fw);
 			return -EINVAL;
-		break;
-	default:
-		ASSERT(0);
+		}
 		break;
 	}
 	/* start download */
@@ -2552,7 +2552,6 @@ static void slic_mcast_set_list(struct net_device *dev)
 		if (status == 0)
 			slic_mcast_set_mask(adapter);
 	}
-	return;
 }
 
 #define  XMIT_FAIL_LINK_STATE               1
@@ -3132,7 +3131,6 @@ static int slic_entry_open(struct net_device *dev)
 {
 	struct adapter *adapter = netdev_priv(dev);
 	struct sliccard *card = adapter->card;
-	u32 locked = 0;
 	int status;
 
 	ASSERT(adapter);
@@ -3142,7 +3140,6 @@ static int slic_entry_open(struct net_device *dev)
 
 	spin_lock_irqsave(&slic_global.driver_lock.lock,
 				slic_global.driver_lock.flags);
-	locked = 1;
 	if (!adapter->activated) {
 		card->adapters_activated++;
 		slic_global.num_slic_ports_active++;
@@ -3156,23 +3153,15 @@ static int slic_entry_open(struct net_device *dev)
 			slic_global.num_slic_ports_active--;
 			adapter->activated = 0;
 		}
-		if (locked) {
-			spin_unlock_irqrestore(&slic_global.driver_lock.lock,
-						slic_global.driver_lock.flags);
-			locked = 0;
-		}
-		return status;
+		goto spin_unlock;
 	}
 	if (!card->master)
 		card->master = adapter;
 
-	if (locked) {
-		spin_unlock_irqrestore(&slic_global.driver_lock.lock,
-					slic_global.driver_lock.flags);
-		locked = 0;
-	}
-
-	return 0;
+spin_unlock:
+	spin_unlock_irqrestore(&slic_global.driver_lock.lock,
+			       slic_global.driver_lock.flags);
+	return status;
 }
 
 static void slic_card_cleanup(struct sliccard *card)
@@ -3712,9 +3701,8 @@ static void slic_init_adapter(struct net_device *netdev,
 					phys_shmem);
 	ASSERT(adapter->pshmem);
 
-	memset(adapter->pshmem, 0, sizeof(struct slic_shmem));
-
-	return;
+	if (adapter->pshmem)
+		memset(adapter->pshmem, 0, sizeof(struct slic_shmem));
 }
 
 static const struct net_device_ops slic_netdev_ops = {
diff --git a/drivers/staging/sm7xxfb/sm7xx.h b/drivers/staging/sm7xxfb/sm7xx.h
index 333f33c3dc66..85998615b801 100644
--- a/drivers/staging/sm7xxfb/sm7xx.h
+++ b/drivers/staging/sm7xxfb/sm7xx.h
@@ -29,7 +29,7 @@
 #define dac_reg	(0x3c8)
 #define dac_val	(0x3c9)
 
-extern char __iomem *smtc_RegBaseAddress;
+extern void __iomem *smtc_RegBaseAddress;
 #define smtc_mmiowb(dat, reg)	writeb(dat, smtc_RegBaseAddress + reg)
 #define smtc_mmioww(dat, reg)	writew(dat, smtc_RegBaseAddress + reg)
 #define smtc_mmiowl(dat, reg)	writel(dat, smtc_RegBaseAddress + reg)
diff --git a/drivers/staging/sm7xxfb/sm7xxfb.c b/drivers/staging/sm7xxfb/sm7xxfb.c
index 1c1780c70fbb..f27182d4dea6 100644
--- a/drivers/staging/sm7xxfb/sm7xxfb.c
+++ b/drivers/staging/sm7xxfb/sm7xxfb.c
@@ -43,11 +43,11 @@ struct smtcfb_info {
 	u16 chip_id;
 	u8  chip_rev_id;
 
-	unsigned char __iomem *m_pMMIO;
-	char __iomem *m_pLFB;
-	char *m_pDPR;
-	char *m_pVPR;
-	char *m_pCPR;
+	void __iomem *lfb;	/* linear frame buffer */
+	void __iomem *dp_regs;	/* drawing processor control regs */
+	void __iomem *vp_regs;	/* video processor control regs */
+	void __iomem *cp_regs;	/* capture processor control regs */
+	void __iomem *mmio;	/* memory map IO port */
 
 	u_int width;
 	u_int height;
@@ -56,8 +56,7 @@ struct smtcfb_info {
 	u32 colreg[17];
 };
 
-char __iomem *smtc_RegBaseAddress;	/* Memory Map IO starting address */
-char __iomem *smtc_VRAMBaseAddress;	/* video memory starting address */
+void __iomem *smtc_RegBaseAddress;	/* Memory Map IO starting address */
 
 static struct fb_var_screeninfo smtcfb_var = {
 	.xres           = 1024,
@@ -72,14 +71,20 @@ static struct fb_var_screeninfo smtcfb_var = {
 	.height         = -1,
 	.width          = -1,
 	.vmode          = FB_VMODE_NONINTERLACED,
+	.nonstd         = 0,
+	.accel_flags    = FB_ACCELF_TEXT,
 };
 
 static struct fb_fix_screeninfo smtcfb_fix = {
-	.id             = "sm712fb",
+	.id             = "smXXXfb",
 	.type           = FB_TYPE_PACKED_PIXELS,
 	.visual         = FB_VISUAL_TRUECOLOR,
 	.line_length    = 800 * 3,
 	.accel          = FB_ACCEL_SMI_LYNX,
+	.type_aux       = 0,
+	.xpanstep       = 0,
+	.ypanstep       = 0,
+	.ywrapstep      = 0,
 };
 
 struct vesa_mode {
@@ -545,28 +550,28 @@ static void sm7xx_set_timing(struct smtcfb_info *sfb)
 	smtc_mmiowb(0x67, 0x3c2);
 
 	/* set VPR registers */
-	writel(0x0, sfb->m_pVPR + 0x0C);
-	writel(0x0, sfb->m_pVPR + 0x40);
+	writel(0x0, sfb->vp_regs + 0x0C);
+	writel(0x0, sfb->vp_regs + 0x40);
 
 	/* set data width */
 	m_nScreenStride =
 		(sfb->width * sfb->fb.var.bits_per_pixel) / 64;
 	switch (sfb->fb.var.bits_per_pixel) {
 	case 8:
-		writel(0x0, sfb->m_pVPR + 0x0);
+		writel(0x0, sfb->vp_regs + 0x0);
 		break;
 	case 16:
-		writel(0x00020000, sfb->m_pVPR + 0x0);
+		writel(0x00020000, sfb->vp_regs + 0x0);
 		break;
 	case 24:
-		writel(0x00040000, sfb->m_pVPR + 0x0);
+		writel(0x00040000, sfb->vp_regs + 0x0);
 		break;
 	case 32:
-		writel(0x00030000, sfb->m_pVPR + 0x0);
+		writel(0x00030000, sfb->vp_regs + 0x0);
 		break;
 	}
 	writel((u32) (((m_nScreenStride + 2) << 16) | m_nScreenStride),
-	       sfb->m_pVPR + 0x10);
+	       sfb->vp_regs + 0x10);
 
 }
 
@@ -673,9 +678,9 @@ static struct fb_ops smtcfb_ops = {
 };
 
 /*
- * alloc struct smtcfb_info and assign the default value
+ * alloc struct smtcfb_info and assign default values
  */
-static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev, char *name)
+static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev)
 {
 	struct smtcfb_info *sfb;
 
@@ -686,32 +691,12 @@ static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev, char *name)
 
 	sfb->pdev = pdev;
 
-	/* init sfb->fb with default value */
-
-	sfb->fb.flags = FBINFO_FLAG_DEFAULT;
-	sfb->fb.fbops = &smtcfb_ops;
-
-	sfb->fb.fix = smtcfb_fix;
-	strcpy(sfb->fb.fix.id, name);
-
-	sfb->fb.fix.type        = FB_TYPE_PACKED_PIXELS;
-	sfb->fb.fix.type_aux    = 0;
-	sfb->fb.fix.xpanstep    = 0;
-	sfb->fb.fix.ypanstep    = 0;
-	sfb->fb.fix.ywrapstep   = 0;
-	sfb->fb.fix.accel       = FB_ACCEL_SMI_LYNX;
-
-	sfb->fb.var             = smtcfb_var;
-	sfb->fb.var.nonstd      = 0;
-	sfb->fb.var.activate    = FB_ACTIVATE_NOW;
-	sfb->fb.var.height      = -1;
-	sfb->fb.var.width       = -1;
-	sfb->fb.var.accel_flags = FB_ACCELF_TEXT;
-	sfb->fb.var.vmode       = FB_VMODE_NONINTERLACED;
-
-	sfb->fb.pseudo_palette  = sfb->colreg;
-
-	sfb->fb.par = sfb;
+	sfb->fb.flags          = FBINFO_FLAG_DEFAULT;
+	sfb->fb.fbops          = &smtcfb_ops;
+	sfb->fb.fix            = smtcfb_fix;
+	sfb->fb.var            = smtcfb_var;
+	sfb->fb.pseudo_palette = sfb->colreg;
+	sfb->fb.par            = sfb;
 
 	return sfb;
 }
@@ -751,7 +736,7 @@ static int smtc_map_smem(struct smtcfb_info *sfb,
 
 	sfb->fb.fix.smem_len = smem_len;
 
-	sfb->fb.screen_base = smtc_VRAMBaseAddress;
+	sfb->fb.screen_base = sfb->lfb;
 
 	if (!sfb->fb.screen_base) {
 		dev_err(&pdev->dev,
@@ -788,9 +773,8 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 {
 	struct smtcfb_info *sfb;
 	u_long smem_size = 0x00800000;	/* default 8MB */
-	char name[16];
 	int err;
-	unsigned long pFramebufferPhysical;
+	unsigned long mmio_base;
 
 	dev_info(&pdev->dev, "Silicon Motion display driver.");
 
@@ -798,7 +782,9 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 	if (err)
 		return err;
 
-	sfb = smtc_alloc_fb_info(pdev, name);
+	sprintf(smtcfb_fix.id, "sm%Xfb", ent->device);
+
+	sfb = smtc_alloc_fb_info(pdev);
 
 	if (!sfb) {
 		err = -ENOMEM;
@@ -806,7 +792,6 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 	}
 
 	sfb->chip_id = ent->device;
-	sprintf(name, "sm%Xfb", sfb->chip_id);
 
 	pci_set_drvdata(pdev, sfb);
 
@@ -829,33 +814,28 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 		sfb->fb.var.bits_per_pixel = (smtc_scr_info.lfb_depth = 32);
 #endif
 	/* Map address and memory detection */
-	pFramebufferPhysical = pci_resource_start(pdev, 0);
+	mmio_base = pci_resource_start(pdev, 0);
 	pci_read_config_byte(pdev, PCI_REVISION_ID, &sfb->chip_rev_id);
 
 	switch (sfb->chip_id) {
 	case 0x710:
 	case 0x712:
-		sfb->fb.fix.mmio_start = pFramebufferPhysical + 0x00400000;
+		sfb->fb.fix.mmio_start = mmio_base + 0x00400000;
 		sfb->fb.fix.mmio_len = 0x00400000;
 		smem_size = SM712_VIDEOMEMORYSIZE;
 #ifdef __BIG_ENDIAN
-		sfb->m_pLFB = (smtc_VRAMBaseAddress =
-		    ioremap(pFramebufferPhysical, 0x00c00000));
+		sfb->lfb = ioremap(mmio_base, 0x00c00000);
 #else
-		sfb->m_pLFB = (smtc_VRAMBaseAddress =
-		    ioremap(pFramebufferPhysical, 0x00800000));
+		sfb->lfb = ioremap(mmio_base, 0x00800000);
 #endif
-		sfb->m_pMMIO = (smtc_RegBaseAddress =
-		    smtc_VRAMBaseAddress + 0x00700000);
-		sfb->m_pDPR = smtc_VRAMBaseAddress + 0x00408000;
-		sfb->m_pVPR = sfb->m_pLFB + 0x0040c000;
+		sfb->mmio = (smtc_RegBaseAddress =
+		    sfb->lfb + 0x00700000);
+		sfb->dp_regs = sfb->lfb + 0x00408000;
+		sfb->vp_regs = sfb->lfb + 0x0040c000;
 #ifdef __BIG_ENDIAN
 		if (sfb->fb.var.bits_per_pixel == 32) {
-			smtc_VRAMBaseAddress += 0x800000;
-			sfb->m_pLFB += 0x800000;
-			dev_info(&pdev->dev,
-				 "smtc_VRAMBaseAddress=%p sfb->m_pLFB=%p",
-				  smtc_VRAMBaseAddress, sfb->m_pLFB);
+			sfb->lfb += 0x800000;
+			dev_info(&pdev->dev, "sfb->lfb=%p", sfb->lfb);
 		}
 #endif
 		if (!smtc_RegBaseAddress) {
@@ -879,15 +859,14 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 #endif
 		break;
 	case 0x720:
-		sfb->fb.fix.mmio_start = pFramebufferPhysical;
+		sfb->fb.fix.mmio_start = mmio_base;
 		sfb->fb.fix.mmio_len = 0x00200000;
 		smem_size = SM722_VIDEOMEMORYSIZE;
-		sfb->m_pDPR = ioremap(pFramebufferPhysical, 0x00a00000);
-		sfb->m_pLFB = (smtc_VRAMBaseAddress =
-		    sfb->m_pDPR + 0x00200000);
-		sfb->m_pMMIO = (smtc_RegBaseAddress =
-		    sfb->m_pDPR + 0x000c0000);
-		sfb->m_pVPR = sfb->m_pDPR + 0x800;
+		sfb->dp_regs = ioremap(mmio_base, 0x00a00000);
+		sfb->lfb = sfb->dp_regs + 0x00200000;
+		sfb->mmio = (smtc_RegBaseAddress =
+		    sfb->dp_regs + 0x000c0000);
+		sfb->vp_regs = sfb->dp_regs + 0x800;
 
 		smtc_seqw(0x62, 0xff);
 		smtc_seqw(0x6a, 0x0d);
diff --git a/drivers/staging/speakup/i18n.c b/drivers/staging/speakup/i18n.c
index ca01734d13c5..7c1658b971dc 100644
--- a/drivers/staging/speakup/i18n.c
+++ b/drivers/staging/speakup/i18n.c
@@ -555,6 +555,7 @@ ssize_t msg_set(enum msg_index_t index, char *text, size_t length)
 			&& index <= MSG_FORMATTED_END)
 				&& !fmt_validate(speakup_default_msgs[index],
 				newstr)) {
+				kfree(newstr);
 				return -EINVAL;
 			}
 			spk_lock(flags);
diff --git a/drivers/staging/speakup/serialio.h b/drivers/staging/speakup/serialio.h
index 614271f9b99f..55d68b5ad165 100644
--- a/drivers/staging/speakup/serialio.h
+++ b/drivers/staging/speakup/serialio.h
@@ -1,8 +1,7 @@
 #ifndef _SPEAKUP_SERIAL_H
 #define _SPEAKUP_SERIAL_H
 
-#include <linux/serial.h>	/* for rs_table, serial constants &
-				   serial_uart_config */
+#include <linux/serial.h>	/* for rs_table, serial constants */
 #include <linux/serial_reg.h>	/* for more serial constants */
 #ifndef __sparc__
 #include <asm/serial.h>
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index 42cdafeea35e..e2f5c81e7548 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -40,13 +40,13 @@ static int softsynth_is_alive(struct spk_synth *synth);
 static unsigned char get_index(void);
 
 static struct miscdevice synth_device;
-static int initialized;
+static int init_pos;
 static int misc_registered;
 
 static struct var_t vars[] = {
 	{ CAPS_START, .u.s = {"\x01+3p" } },
 	{ CAPS_STOP, .u.s = {"\x01-3p" } },
-	{ RATE, .u.n = {"\x01%ds", 5, 0, 9, 0, 0, NULL } },
+	{ RATE, .u.n = {"\x01%ds", 2, 0, 9, 0, 0, NULL } },
 	{ PITCH, .u.n = {"\x01%dp", 5, 0, 9, 0, 0, NULL } },
 	{ VOL, .u.n = {"\x01%dv", 5, 0, 9, 0, 0, NULL } },
 	{ TONE, .u.n = {"\x01%dx", 1, 0, 2, 0, 0, NULL } },
@@ -194,7 +194,7 @@ static int softsynth_close(struct inode *inode, struct file *fp)
 	unsigned long flags;
 	spk_lock(flags);
 	synth_soft.alive = 0;
-	initialized = 0;
+	init_pos = 0;
 	spk_unlock(flags);
 	/* Make sure we let applications go before leaving */
 	speakup_start_ttys();
@@ -239,13 +239,8 @@ static ssize_t softsynth_read(struct file *fp, char *buf, size_t count,
 			ch = '\x18';
 		} else if (synth_buffer_empty()) {
 			break;
-		} else if (!initialized) {
-			if (*init) {
-				ch = *init;
-				init++;
-			} else {
-				initialized = 1;
-			}
+		} else if (init[init_pos]) {
+			ch = init[init_pos++];
 		} else {
 			ch = synth_buffer_getc();
 		}
diff --git a/drivers/staging/telephony/ixj.c b/drivers/staging/telephony/ixj.c
index b303c9192e17..1cfa0b07d725 100644
--- a/drivers/staging/telephony/ixj.c
+++ b/drivers/staging/telephony/ixj.c
@@ -7057,7 +7057,7 @@ static int ixj_selfprobe(IXJ *j)
 		printk(KERN_INFO "Enable Line Monitor\n");
 
 	if (ixjdebug & 0x0002)
-		printk(KERN_INFO "Set Line Monitor to Asyncronous Mode\n");
+		printk(KERN_INFO "Set Line Monitor to Asynchronous Mode\n");
 
 	if (ixj_WriteDSPCommand(0x7E01, j))		/* Asynchronous Line Monitor */
 		return -1;
@@ -7068,7 +7068,7 @@ static int ixj_selfprobe(IXJ *j)
 	if (ixj_WriteDSPCommand(0x5151, j))		/* Enable DTMF detection */
 		return -1;
 
-	if (ixj_WriteDSPCommand(0x6E01, j))		/* Set Asyncronous Tone Generation */
+	if (ixj_WriteDSPCommand(0x6E01, j))		/* Set Asynchronous Tone Generation */
 		return -1;
 
 	set_rec_depth(j, 2);	/* Set Record Channel Limit to 2 frames */
diff --git a/drivers/staging/tidspbridge/Documentation/error-codes b/drivers/staging/tidspbridge/Documentation/error-codes
index 12826e2a3aaa..ad73cba058eb 100644
--- a/drivers/staging/tidspbridge/Documentation/error-codes
+++ b/drivers/staging/tidspbridge/Documentation/error-codes
@@ -69,7 +69,7 @@ The error codes used by this driver are:
     Invalid pointer or handler.
 
 [EEXIST]
-    Attempted to create a channel manager  when one already exists.
+    Attempted to create a channel manager when one already exists.
 
 [EINVAL]
     Invalid argument.
diff --git a/drivers/staging/tidspbridge/core/_tiomap.h b/drivers/staging/tidspbridge/core/_tiomap.h
index 7cb587103975..543a127c7d4d 100644
--- a/drivers/staging/tidspbridge/core/_tiomap.h
+++ b/drivers/staging/tidspbridge/core/_tiomap.h
@@ -219,7 +219,7 @@ static const struct map_l4_peripheral l4_peripheral_table[] = {
 /* MBX_PM_MAX_RESOURCES: CORE 2 Clock Resources. */
 #define MBX_CORE2_RESOURCES 1
 
-/* MBX_PM_MAX_RESOURCES: TOTAL Clock Reosurces. */
+/* MBX_PM_MAX_RESOURCES: TOTAL Clock Resources. */
 #define MBX_PM_MAX_RESOURCES 11
 
 /*  Power Management Commands */
diff --git a/drivers/staging/tidspbridge/core/chnl_sm.c b/drivers/staging/tidspbridge/core/chnl_sm.c
index e0c7e4c470c8..16fa3462fbbe 100644
--- a/drivers/staging/tidspbridge/core/chnl_sm.c
+++ b/drivers/staging/tidspbridge/core/chnl_sm.c
@@ -20,7 +20,7 @@
  *      The lower edge functions must be implemented by the Bridge driver
  *      writer, and are declared in chnl_sm.h.
  *
- *      Care is taken in this code to prevent simulataneous access to channel
+ *      Care is taken in this code to prevent simultaneous access to channel
  *      queues from
  *      1. Threads.
  *      2. io_dpc(), scheduled from the io_isr() as an event.
@@ -34,7 +34,7 @@
  *  Channel Invariant:
  *      There is an important invariant condition which must be maintained per
  *      channel outside of bridge_chnl_get_ioc() and IO_Dispatch(), violation of
- *      which may cause timeouts and/or failure offunction sync_wait_on_event.
+ *      which may cause timeouts and/or failure of function sync_wait_on_event.
  *      This invariant condition is:
  *
  *          list_empty(&pchnl->io_completions) ==> pchnl->sync_event is reset
@@ -94,7 +94,7 @@ int bridge_chnl_add_io_req(struct chnl_object *chnl_obj, void *host_buf,
 	struct dev_object *dev_obj;
 	u8 dw_state;
 	bool is_eos;
-	struct chnl_mgr *chnl_mgr_obj = pchnl->chnl_mgr_obj;
+	struct chnl_mgr *chnl_mgr_obj;
 	u8 *host_sys_buf = NULL;
 	bool sched_dpc = false;
 	u16 mb_val = 0;
@@ -153,6 +153,7 @@ func_cont:
 	 * If DPC is scheduled in process context (iosm_schedule) and any
 	 * non-mailbox interrupt occurs, that DPC will run and break CS. Hence
 	 * we disable ALL DPCs. We will try to disable ONLY IO DPC later. */
+	chnl_mgr_obj = pchnl->chnl_mgr_obj;
 	spin_lock_bh(&chnl_mgr_obj->chnl_mgr_lock);
 	omap_mbox_disable_irq(dev_ctxt->mbox, IRQ_RX);
 	if (pchnl->chnl_type == CHNL_PCPY) {
@@ -602,7 +603,7 @@ int bridge_chnl_get_ioc(struct chnl_object *chnl_obj, u32 timeout,
 		/*  Since DSPStream_Reclaim() does not take a timeout
 		 *  parameter, we pass the stream's timeout value to
 		 *  bridge_chnl_get_ioc. We cannot determine whether or not
-		 *  we have waited in User mode. Since the stream's timeout
+		 *  we have waited in user mode. Since the stream's timeout
 		 *  value may be non-zero, we still have to set the event.
 		 *  Therefore, this optimization is taken out.
 		 *
diff --git a/drivers/staging/tidspbridge/core/dsp-clock.c b/drivers/staging/tidspbridge/core/dsp-clock.c
index c7df34e6b60b..7eac01e5fe0b 100644
--- a/drivers/staging/tidspbridge/core/dsp-clock.c
+++ b/drivers/staging/tidspbridge/core/dsp-clock.c
@@ -16,6 +16,8 @@
  * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  */
 
+#define L4_34XX_BASE		0x48000000
+
 #include <linux/types.h>
 
 /*  ----------------------------------- Host OS */
diff --git a/drivers/staging/tidspbridge/core/io_sm.c b/drivers/staging/tidspbridge/core/io_sm.c
index 480a3845a24c..e322fb7aebe1 100644
--- a/drivers/staging/tidspbridge/core/io_sm.c
+++ b/drivers/staging/tidspbridge/core/io_sm.c
@@ -837,8 +837,8 @@ static void io_dispatch_pm(struct io_mgr *pio_mgr)
 /*
  *  ======== io_dpc ========
  *      Deferred procedure call for shared memory channel driver ISR.  Carries
- *      out the dispatch of I/O as a non-preemptible event.It can only be
- *      pre-empted      by an ISR.
+ *      out the dispatch of I/O as a non-preemptible event. It can only be
+ *      pre-empted by an ISR.
  */
 void io_dpc(unsigned long ref_data)
 {
@@ -877,7 +877,7 @@ void io_dpc(unsigned long ref_data)
 						  pio_mgr->intr_val);
 			}
 		}
-		/* Proc-copy chanel dispatch */
+		/* Proc-copy channel dispatch */
 		input_chnl(pio_mgr, NULL, IO_SERVICE);
 		output_chnl(pio_mgr, NULL, IO_SERVICE);
 
@@ -938,7 +938,7 @@ int io_mbox_msg(struct notifier_block *self, unsigned long len, void *msg)
 /*
  *  ======== io_request_chnl ========
  *  Purpose:
- *      Request chanenel I/O from the DSP. Sets flags in shared memory, then
+ *      Request channel I/O from the DSP. Sets flags in shared memory, then
  *      interrupts the DSP.
  */
 void io_request_chnl(struct io_mgr *io_manager, struct chnl_object *pchnl,
@@ -2208,7 +2208,7 @@ void dump_dl_modules(struct bridge_dev_context *bridge_context)
 			module_struct->num_sects);
 
 		/*
-		 * The section name strings start immedialty following
+		 * The section name strings start immediately following
 		 * the array of dll_sect structures.
 		 */
 		sect_str = (char *) &module_struct->
diff --git a/drivers/staging/tidspbridge/core/sync.c b/drivers/staging/tidspbridge/core/sync.c
index 995986a9d03b..7bb550acaf4a 100644
--- a/drivers/staging/tidspbridge/core/sync.c
+++ b/drivers/staging/tidspbridge/core/sync.c
@@ -49,7 +49,7 @@ void sync_set_event(struct sync_object *event)
  * @timeout	timeout on waiting for the evetns.
  * @pu_index	index of the event set.
  *
- * This functios will wait until any of the array element is set or until
+ * These functions will wait until any of the array element is set or until
  * timeout. In case of success the function will return 0 and
  * @pu_index will store the index of the array element set or in case
  * of timeout the function will return -ETIME or in case of
diff --git a/drivers/staging/tidspbridge/core/tiomap3430.c b/drivers/staging/tidspbridge/core/tiomap3430.c
index f9609ce2c163..012c5a0cc6c8 100644
--- a/drivers/staging/tidspbridge/core/tiomap3430.c
+++ b/drivers/staging/tidspbridge/core/tiomap3430.c
@@ -328,7 +328,7 @@ static int bridge_brd_read(struct bridge_dev_context *dev_ctxt,
 					   ul_num_bytes, mem_type);
 		return status;
 	}
-	/* copy the data from  DSP memory, */
+	/* copy the data from DSP memory */
 	memcpy(host_buff, (void *)(dsp_base_addr + offset), ul_num_bytes);
 	return status;
 }
@@ -415,10 +415,10 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 		/* Assert RST1 i.e only the RST only for DSP megacell */
 		if (!status) {
 			/*
-			 * XXX: ioremapping  MUST be removed once ctrl
+			 * XXX: OMAP343X_CTRL_BASE ioremapping  MUST be removed once ctrl
 			 * function is made available.
 			 */
-			void __iomem *ctrl = ioremap(OMAP343X_CTRL_BASE, SZ_4K);
+			void __iomem *ctrl = ioremap(0x48002000, SZ_4K);
 			if (!ctrl)
 				return -ENOMEM;
 
@@ -1745,7 +1745,7 @@ static int mem_map_vmalloc(struct bridge_dev_context *dev_context,
 	pa_next = page_to_phys(page[0]);
 	while (!status && (i < num_pages)) {
 		/*
-		 * Reuse pa_next from the previous iteraion to avoid
+		 * Reuse pa_next from the previous iteration to avoid
 		 * an extra va2pa call
 		 */
 		pa_curr = pa_next;
diff --git a/drivers/staging/tidspbridge/core/tiomap3430_pwr.c b/drivers/staging/tidspbridge/core/tiomap3430_pwr.c
index 16a4aafa86ae..58a1d6dcf098 100644
--- a/drivers/staging/tidspbridge/core/tiomap3430_pwr.c
+++ b/drivers/staging/tidspbridge/core/tiomap3430_pwr.c
@@ -356,7 +356,7 @@ int pre_scale_dsp(struct bridge_dev_context *dev_context, void *pargs)
 		dev_dbg(bridge, "OPP: %s IVA in sleep. No message to DSP\n");
 		return 0;
 	} else if ((dev_context->brd_state == BRD_RUNNING)) {
-		/* Send a prenotificatio to DSP */
+		/* Send a prenotification to DSP */
 		dev_dbg(bridge, "OPP: %s sent notification to DSP\n", __func__);
 		sm_interrupt_dsp(dev_context, MBX_PM_SETPOINT_PRENOTIFY);
 		return 0;
diff --git a/drivers/staging/tidspbridge/core/wdt.c b/drivers/staging/tidspbridge/core/wdt.c
index 870f934f4f3b..e5adad08f1c4 100644
--- a/drivers/staging/tidspbridge/core/wdt.c
+++ b/drivers/staging/tidspbridge/core/wdt.c
@@ -25,7 +25,8 @@
 #include <dspbridge/host_os.h>
 
 
-#define OMAP34XX_WDT3_BASE 		(L4_PER_34XX_BASE + 0x30000)
+#define OMAP34XX_WDT3_BASE 		(0x49000000 + 0x30000)
+#define INT_34XX_WDT3_IRQ 		36
 
 static struct dsp_wdt_setting dsp_wdt;
 
@@ -61,9 +62,9 @@ int dsp_wdt_init(void)
 
 	dsp_wdt.fclk = clk_get(NULL, "wdt3_fck");
 
-	if (dsp_wdt.fclk) {
+	if (!IS_ERR(dsp_wdt.fclk)) {
 		dsp_wdt.iclk = clk_get(NULL, "wdt3_ick");
-		if (!dsp_wdt.iclk) {
+		if (IS_ERR(dsp_wdt.iclk)) {
 			clk_put(dsp_wdt.fclk);
 			dsp_wdt.fclk = NULL;
 			ret = -EFAULT;
diff --git a/drivers/staging/tidspbridge/dynload/tramp.c b/drivers/staging/tidspbridge/dynload/tramp.c
index 60d22ea47055..404af1895980 100644
--- a/drivers/staging/tidspbridge/dynload/tramp.c
+++ b/drivers/staging/tidspbridge/dynload/tramp.c
@@ -81,7 +81,7 @@ static u8 priv_h2a(u8 value)
  * Description: Generate a trampoline symbol name (ASCII) using the value
  *	  of the symbol.  This places the new name into the user buffer.
  *	  The name is fixed in length and of the form: __$dbTR__xxxxxxxx
- *	  (where "xxxxxxxx" is the hex value.
+ *	  (where "xxxxxxxx" is the hex value).
  */
 static void priv_tramp_sym_gen_name(u32 value, char *dst)
 {
@@ -414,7 +414,7 @@ static int priv_tramp_sym_finalize(struct dload_state *dlthis)
 			/*  Copy the symbol contents into the flat table */
 			*new_sym = cur_sym->sym_info;
 
-			/*  Now finaize the symbol.  If it is in the tramp
+			/*  Now finalize the symbol.  If it is in the tramp
 			 * section, we need to adjust for the section start.
 			 * If it is external then we don't need to adjust at
 			 * all.
@@ -773,7 +773,7 @@ static int priv_img_pkt_dup(struct dload_state *dlthis,
 	int ret_val = 0;
 	struct tramp_img_dup_relo *dup_relo = NULL;
 
-	/*  Determinne if this image packet is already being tracked in the
+	/*  Determine if this image packet is already being tracked in the
 	   dup list for other trampolines. */
 	dup_pkt = priv_dup_find(dlthis, secnn, image_offset);
 
@@ -998,7 +998,7 @@ int dload_tramp_generate(struct dload_state *dlthis, s16 secnn,
 /*
  * Function:	dload_tramp_pkt_update
  * Description: Update the duplicate copy of this image packet, which the
- *	  trampoline layer is already tracking.  This is call is critical
+ *	  trampoline layer is already tracking.  This call is critical
  *	  to make if trampolines were generated anywhere within the
  *	  packet and first pass relo continued on the remainder.  The
  *	  trampoline layer needs the updates image data so when 2nd
diff --git a/drivers/staging/tidspbridge/gen/uuidutil.c b/drivers/staging/tidspbridge/gen/uuidutil.c
index b44656cf7858..b7d8313d1acb 100644
--- a/drivers/staging/tidspbridge/gen/uuidutil.c
+++ b/drivers/staging/tidspbridge/gen/uuidutil.c
@@ -26,27 +26,6 @@
 /*  ----------------------------------- This */
 #include <dspbridge/uuidutil.h>
 
-/*
- *  ======== uuid_uuid_to_string ========
- *  Purpose:
- *      Converts a struct dsp_uuid to a string.
- *      Note: snprintf format specifier is:
- *      %[flags] [width] [.precision] [{h | l | I64 | L}]type
- */
-void uuid_uuid_to_string(struct dsp_uuid *uuid_obj, char *sz_uuid,
-			 s32 size)
-{
-	s32 i;			/* return result from snprintf. */
-
-	i = snprintf(sz_uuid, size,
-		     "%.8X_%.4X_%.4X_%.2X%.2X_%.2X%.2X%.2X%.2X%.2X%.2X",
-		     uuid_obj->data1, uuid_obj->data2, uuid_obj->data3,
-		     uuid_obj->data4, uuid_obj->data5,
-		     uuid_obj->data6[0], uuid_obj->data6[1],
-		     uuid_obj->data6[2], uuid_obj->data6[3],
-		     uuid_obj->data6[4], uuid_obj->data6[5]);
-}
-
 static s32 uuid_hex_to_bin(char *buf, s32 len)
 {
 	s32 i;
diff --git a/drivers/staging/tidspbridge/hw/hw_mmu.c b/drivers/staging/tidspbridge/hw/hw_mmu.c
index 8a93d55ca596..71cb82293649 100644
--- a/drivers/staging/tidspbridge/hw/hw_mmu.c
+++ b/drivers/staging/tidspbridge/hw/hw_mmu.c
@@ -61,7 +61,7 @@ enum hw_mmu_page_size_t {
  *       Type		: hw_status
  *       Description     : 0		 -- No errors occurred
  *			 RET_BAD_NULL_PARAM     -- A Pointer
- *						Paramater was set to NULL
+ *						Parameter was set to NULL
  *
  * PURPOSE:	      : Flush the TLB entry pointed by the
  *			lock counter register
@@ -103,7 +103,7 @@ static hw_status mmu_flush_entry(const void __iomem *base_address);
  *
  *       Type	    	: hw_status
  *       Description     : 0		 -- No errors occurred
- *			 RET_BAD_NULL_PARAM     -- A Pointer Paramater
+ *			 RET_BAD_NULL_PARAM     -- A Pointer Parameter
  *						   was set to NULL
  *			 RET_PARAM_OUT_OF_RANGE -- Input Parameter out
  *						   of Range
@@ -148,7 +148,7 @@ static hw_status mmu_set_cam_entry(const void __iomem *base_address,
  *
  *       Type	    	: hw_status
  *       Description     : 0		 -- No errors occurred
- *			 RET_BAD_NULL_PARAM     -- A Pointer Paramater
+ *			 RET_BAD_NULL_PARAM     -- A Pointer Parameter
  *							was set to NULL
  *			 RET_PARAM_OUT_OF_RANGE -- Input Parameter
  *							out of Range
diff --git a/drivers/staging/tidspbridge/include/dspbridge/dspioctl.h b/drivers/staging/tidspbridge/include/dspbridge/dspioctl.h
index 0c7ec04448f1..0fcda1978921 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/dspioctl.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/dspioctl.h
@@ -51,7 +51,7 @@
 #define BRDIOCTL_POSTSCALE_NOTIFY (BRDIOCTL_PWRCONTROL + 0xA)
 #define BRDIOCTL_CONSTRAINT_REQUEST (BRDIOCTL_PWRCONTROL + 0xB)
 
-/* Number of actual DSP-MMU TLB entrries */
+/* Number of actual DSP-MMU TLB entries */
 #define BRDIOCTL_NUMOFMMUTLB        32
 
 struct bridge_ioctl_extproc {
diff --git a/drivers/staging/tidspbridge/include/dspbridge/mbx_sh.h b/drivers/staging/tidspbridge/include/dspbridge/mbx_sh.h
index 7424c888d637..d4cb3948baba 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/mbx_sh.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/mbx_sh.h
@@ -22,7 +22,7 @@
  *  mailbox interrupt's cmd value received. The class value are defined
  *  as a bit (10 thru 15) being set.
  *
- *  Note: Only 16 bits of each  is used. Other 16 bit data reg available.
+ *  Note: Only 16 bits of each is used. Other 16 bit data reg available.
  *
  *   16 bit Mbx bit defns:
  *
diff --git a/drivers/staging/tidspbridge/include/dspbridge/node.h b/drivers/staging/tidspbridge/include/dspbridge/node.h
index 7397b7a12f7a..68ed74a86c95 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/node.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/node.h
@@ -220,7 +220,7 @@ extern int node_create_mgr(struct node_mgr **node_man,
  *  Parameters:
  *      noderes:              Node resource info handle returned from
  *                                 node_allocate().
- *      pr_ctxt:                Poninter to process context data.
+ *      pr_ctxt:                Pointer to process context data.
  *  Returns:
  *      0:            Success.
  *      -EFAULT:        Invalid hnode.
diff --git a/drivers/staging/tidspbridge/include/dspbridge/ntfy.h b/drivers/staging/tidspbridge/include/dspbridge/ntfy.h
index cbc8819c61cc..6bb94d20e99a 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/ntfy.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/ntfy.h
@@ -78,7 +78,7 @@ static inline void ntfy_init(struct ntfy_object *no)
  * ntfy_delete() - delete list of nofy events registered.
  * @ntfy_obj:	Pointer to the ntfy object structure.
  *
- * This function is used to remove all the notify events  registered.
+ * This function is used to remove all the notify events registered.
  * unregister function is not needed in this function, to unregister
  * a ntfy_event please look at ntfy_register function.
  *
diff --git a/drivers/staging/tidspbridge/include/dspbridge/proc.h b/drivers/staging/tidspbridge/include/dspbridge/proc.h
index a82380ebc041..851b356d7a51 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/proc.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/proc.h
@@ -263,7 +263,7 @@ extern int proc_get_processor_id(void *proc, u32 * proc_id);
  *  Returns:
  *      0     :   Success.
  *      -EFAULT :   Invalid processor handle.
- *      -EPERM   :   General failure while retireving processor trace
+ *      -EPERM   :   General failure while retrieving processor trace
  *		      Buffer.
  *  Requires:
  *      pbuf is not NULL
diff --git a/drivers/staging/tidspbridge/include/dspbridge/strm.h b/drivers/staging/tidspbridge/include/dspbridge/strm.h
index dacf0c234fd1..97aee4c63d24 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/strm.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/strm.h
@@ -203,7 +203,7 @@ extern int strm_issue(struct strm_object *stream_obj, u8 * pbuf,
  *      index:         Stream index.
  *      pattr:          Pointer to structure containing attributes to be
  *                      applied to stream. Cannot be NULL.
- *      strmres:     Location to store stream resuorce info handle on output.
+ *      strmres:     Location to store stream resource info handle on output.
  *  Returns:
  *      0:        Success.
  *      -EFAULT:    Invalid hnode.
diff --git a/drivers/staging/tidspbridge/include/dspbridge/sync.h b/drivers/staging/tidspbridge/include/dspbridge/sync.h
index b1e75eb8847c..58a0d5c5543d 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/sync.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/sync.h
@@ -78,7 +78,7 @@ void sync_set_event(struct sync_object *event);
  * @event:	events to wait for it.
  * @timeout	timeout on waiting for the evetn.
  *
- * This functios will wait until @event is set or until timeout. In case of
+ * This function will wait until @event is set or until timeout. In case of
  * success the function will return 0 and
  * in case of timeout the function will return -ETIME
  * in case of signal the function will return -ERESTARTSYS
@@ -106,7 +106,7 @@ static inline int sync_wait_on_event(struct sync_object *event,
  * @timeout	timeout on waiting for the evetns.
  * @pu_index	index of the event set.
  *
- * This functios will wait until any of the array element is set or until
+ * This function will wait until any of the array element is set or until
  * timeout. In case of success the function will return 0 and
  * @pu_index will store the index of the array element set and in case
  * of timeout the function will return -ETIME.
diff --git a/drivers/staging/tidspbridge/include/dspbridge/uuidutil.h b/drivers/staging/tidspbridge/include/dspbridge/uuidutil.h
index 9a994753e9ba..414bf71d652d 100644
--- a/drivers/staging/tidspbridge/include/dspbridge/uuidutil.h
+++ b/drivers/staging/tidspbridge/include/dspbridge/uuidutil.h
@@ -22,26 +22,6 @@
 #define MAXUUIDLEN  37
 
 /*
- *  ======== uuid_uuid_to_string ========
- *  Purpose:
- *      Converts a dsp_uuid to an ANSI string.
- *  Parameters:
- *      uuid_obj:      Pointer to a dsp_uuid object.
- *      sz_uuid:    Pointer to a buffer to receive a NULL-terminated UUID
- *                  string.
- *      size:	    Maximum size of the sz_uuid string.
- *  Returns:
- *  Requires:
- *      uuid_obj & sz_uuid are non-NULL values.
- *  Ensures:
- *      Lenghth of sz_uuid is less than MAXUUIDLEN.
- *  Details:
- *      UUID string limit currently set at MAXUUIDLEN.
- */
-void uuid_uuid_to_string(struct dsp_uuid *uuid_obj, char *sz_uuid,
-			 s32 size);
-
-/*
  *  ======== uuid_uuid_from_string ========
  *  Purpose:
  *      Converts an ANSI string to a dsp_uuid.
diff --git a/drivers/staging/tidspbridge/rmgr/dbdcd.c b/drivers/staging/tidspbridge/rmgr/dbdcd.c
index 12a1d34b3954..9d52c3cb92f0 100644
--- a/drivers/staging/tidspbridge/rmgr/dbdcd.c
+++ b/drivers/staging/tidspbridge/rmgr/dbdcd.c
@@ -346,11 +346,13 @@ int dcd_get_object_def(struct dcd_manager *hdcd_mgr,
 	struct dcd_manager *dcd_mgr_obj = hdcd_mgr;	/* ptr to DCD mgr */
 	struct cod_libraryobj *lib = NULL;
 	int status = 0;
+	int len;
 	u32 ul_addr = 0;	/* Used by cod_get_section */
 	u32 ul_len = 0;		/* Used by cod_get_section */
 	u32 dw_buf_size;	/* Used by REG functions */
 	char sz_reg_key[DCD_MAXPATHLENGTH];
 	char *sz_uuid;		/*[MAXUUIDLEN]; */
+	char *tmp;
 	struct dcd_key_elem *dcd_key = NULL;
 	char sz_sect_name[MAXUUIDLEN + 2];	/* ".[UUID]\0" */
 	char *psz_coff_buf;
@@ -395,7 +397,7 @@ int dcd_get_object_def(struct dcd_manager *hdcd_mgr,
 		}
 
 		/* Create UUID value to set in registry. */
-		uuid_uuid_to_string(obj_uuid, sz_uuid, MAXUUIDLEN);
+		snprintf(sz_uuid, MAXUUIDLEN, "%pUL", obj_uuid);
 
 		if ((strlen(sz_reg_key) + MAXUUIDLEN) < DCD_MAXPATHLENGTH)
 			strncat(sz_reg_key, sz_uuid, MAXUUIDLEN);
@@ -429,12 +431,27 @@ int dcd_get_object_def(struct dcd_manager *hdcd_mgr,
 	}
 
 	/* Ensure sz_uuid + 1 is not greater than sizeof sz_sect_name. */
+	len = strlen(sz_uuid);
+	if (len + 1 > sizeof(sz_sect_name)) {
+		status = -EPERM;
+		goto func_end;
+	}
 
 	/* Create section name based on node UUID. A period is
 	 * pre-pended to the UUID string to form the section name.
 	 * I.e. ".24BC8D90_BB45_11d4_B756_006008BDB66F" */
+
+	len -= 4;	/* uuid has 4 delimiters '-' */
+	tmp = sz_uuid;
+
 	strncpy(sz_sect_name, ".", 2);
-	strncat(sz_sect_name, sz_uuid, strlen(sz_uuid));
+	do {
+		char *uuid = strsep(&tmp, "-");
+		if (!uuid)
+			break;
+		len -= strlen(uuid);
+		strncat(sz_sect_name, uuid, strlen(uuid) + 1);
+	} while (len && strncat(sz_sect_name, "_", 2));
 
 	/* Get section information. */
 	status = cod_get_section(lib, sz_sect_name, &ul_addr, &ul_len);
@@ -463,7 +480,7 @@ int dcd_get_object_def(struct dcd_manager *hdcd_mgr,
 	status = cod_read_section(lib, sz_sect_name, psz_coff_buf, ul_len);
 #endif
 	if (!status) {
-		/* Compres DSP buffer to conform to PC format. */
+		/* Compress DSP buffer to conform to PC format. */
 		if (strstr(dcd_key->path, "iva") == NULL) {
 			compress_buf(psz_coff_buf, ul_len, DSPWORDSIZE);
 		} else {
@@ -666,7 +683,7 @@ int dcd_get_library_name(struct dcd_manager *hdcd_mgr,
 			status = -EPERM;
 		}
 		/* Create UUID value to find match in registry. */
-		uuid_uuid_to_string(uuid_obj, sz_uuid, MAXUUIDLEN);
+		snprintf(sz_uuid, MAXUUIDLEN, "%pUL", uuid_obj);
 		if ((strlen(sz_reg_key) + MAXUUIDLEN) < DCD_MAXPATHLENGTH)
 			strncat(sz_reg_key, sz_uuid, MAXUUIDLEN);
 		else
@@ -706,7 +723,7 @@ int dcd_get_library_name(struct dcd_manager *hdcd_mgr,
 		} else {
 			status = -EPERM;
 		}
-		uuid_uuid_to_string(uuid_obj, sz_uuid, MAXUUIDLEN);
+		snprintf(sz_uuid, MAXUUIDLEN, "%pUL", uuid_obj);
 		if ((strlen(sz_reg_key) + MAXUUIDLEN) < DCD_MAXPATHLENGTH)
 			strncat(sz_reg_key, sz_uuid, MAXUUIDLEN);
 		else
@@ -797,7 +814,7 @@ int dcd_register_object(struct dsp_uuid *uuid_obj,
 			status = -EPERM;
 
 		/* Create UUID value to set in registry. */
-		uuid_uuid_to_string(uuid_obj, sz_uuid, MAXUUIDLEN);
+		snprintf(sz_uuid, MAXUUIDLEN, "%pUL", uuid_obj);
 		if ((strlen(sz_reg_key) + MAXUUIDLEN) < DCD_MAXPATHLENGTH)
 			strncat(sz_reg_key, sz_uuid, MAXUUIDLEN);
 		else
diff --git a/drivers/staging/tidspbridge/rmgr/drv_interface.c b/drivers/staging/tidspbridge/rmgr/drv_interface.c
index 3cac01492063..6acea2b56aa4 100644
--- a/drivers/staging/tidspbridge/rmgr/drv_interface.c
+++ b/drivers/staging/tidspbridge/rmgr/drv_interface.c
@@ -613,16 +613,6 @@ static struct platform_driver bridge_driver = {
 #endif
 };
 
-static int __init bridge_init(void)
-{
-	return platform_driver_register(&bridge_driver);
-}
-
-static void __exit bridge_exit(void)
-{
-	platform_driver_unregister(&bridge_driver);
-}
-
 /* To remove all process resources before removing the process from the
  * process context list */
 int drv_remove_all_resources(void *process_ctxt)
@@ -636,6 +626,4 @@ int drv_remove_all_resources(void *process_ctxt)
 	return status;
 }
 
-/* Bridge driver initialization and de-initialization functions */
-module_init(bridge_init);
-module_exit(bridge_exit);
+module_platform_driver(bridge_driver);
diff --git a/drivers/staging/tidspbridge/rmgr/dspdrv.c b/drivers/staging/tidspbridge/rmgr/dspdrv.c
index dc767b183cdf..d460f5823c6b 100644
--- a/drivers/staging/tidspbridge/rmgr/dspdrv.c
+++ b/drivers/staging/tidspbridge/rmgr/dspdrv.c
@@ -72,7 +72,7 @@ u32 dsp_init(u32 *init_status)
 
 	/* Unwind whatever was loaded */
 	if (status) {
-		/* irrespective of the status of dev_remove_device we conitinue
+		/* irrespective of the status of dev_remove_device we continue
 		 * unloading. Get the Driver Object iterate through and remove.
 		 * Reset the status to E_FAIL to avoid going through
 		 * api_init_complete2. */
@@ -92,7 +92,7 @@ u32 dsp_init(u32 *init_status)
 func_cont:
 	/* Attempt to Start the Board */
 	if (!status) {
-		/* BRD_AutoStart could fail if the dsp execuetable is not the
+		/* BRD_AutoStart could fail if the dsp executable is not the
 		 * correct one. We should not propagate that error
 		 * into the device loader. */
 		(void)api_init_complete2();
diff --git a/drivers/staging/tidspbridge/rmgr/mgr.c b/drivers/staging/tidspbridge/rmgr/mgr.c
index 8a1e9287cff6..b32ba0ad2a07 100644
--- a/drivers/staging/tidspbridge/rmgr/mgr.c
+++ b/drivers/staging/tidspbridge/rmgr/mgr.c
@@ -262,8 +262,8 @@ int mgr_enum_processor_info(u32 processor_id,
 				    IVAPROCTYPE_ARM7)
 					proc_detect = true;
 			}
-			/* User applciatiuons aonly check for chip type, so
-			 * this clumsy overwrite */
+			/* User applications only check for chip type, so
+			 * this is a clumsy overwrite */
 			processor_info->processor_type = DSPTYPE64;
 		} else {
 			dev_dbg(bridge, "%s: Failed to get DCD processor info "
diff --git a/drivers/staging/tidspbridge/rmgr/nldr.c b/drivers/staging/tidspbridge/rmgr/nldr.c
index 30d5480fcdcc..6309221b64a5 100644
--- a/drivers/staging/tidspbridge/rmgr/nldr.c
+++ b/drivers/staging/tidspbridge/rmgr/nldr.c
@@ -898,7 +898,7 @@ static int add_ovly_info(void *handle, struct dbll_sect_info *sect_info,
 				nldr_obj->ovly_table[i].execute_sects++;
 
 		} else {
-			/* Put in "other" sectins */
+			/* Put in "other" sections */
 			status =
 			    add_ovly_sect(nldr_obj,
 					  &nldr_obj->
diff --git a/drivers/staging/tidspbridge/rmgr/node.c b/drivers/staging/tidspbridge/rmgr/node.c
index 7fb426c5251c..c2fc6137c770 100644
--- a/drivers/staging/tidspbridge/rmgr/node.c
+++ b/drivers/staging/tidspbridge/rmgr/node.c
@@ -1613,7 +1613,7 @@ int node_get_attr(struct node_object *hnode,
 		return -EFAULT;
 
 	hnode_mgr = hnode->node_mgr;
-	/* Enter hnode_mgr critical section (since we're accessing
+	/* Enter hnode_mgr critical section since we're accessing
 	 * data that could be changed by node_change_priority() and
 	 * node_connect(). */
 	mutex_lock(&hnode_mgr->node_mgr_lock);
@@ -2714,8 +2714,7 @@ static int get_node_props(struct dcd_manager *hdcd_mgr,
 		hnode->ntype = node_type = pndb_props->ntype;
 
 		/* Create UUID value to set in registry. */
-		uuid_uuid_to_string((struct dsp_uuid *)node_uuid, sz_uuid,
-				    MAXUUIDLEN);
+		snprintf(sz_uuid, MAXUUIDLEN, "%pUL", node_uuid);
 		dev_dbg(bridge, "(node) UUID: %s\n", sz_uuid);
 
 		/* Fill in message args that come from NDB */
diff --git a/drivers/staging/tidspbridge/rmgr/proc.c b/drivers/staging/tidspbridge/rmgr/proc.c
index 7e4f12f6be42..5e43938ab7fa 100644
--- a/drivers/staging/tidspbridge/rmgr/proc.c
+++ b/drivers/staging/tidspbridge/rmgr/proc.c
@@ -300,7 +300,7 @@ proc_attach(u32 processor_id,
 	if (status)
 		goto func_end;
 
-	/* If we made it this far, create the Proceesor object: */
+	/* If we made it this far, create the Processor object: */
 	p_proc_object = kzalloc(sizeof(struct proc_object), GFP_KERNEL);
 	/* Fill out the Processor Object: */
 	if (p_proc_object == NULL) {
diff --git a/drivers/staging/usbip/Kconfig b/drivers/staging/usbip/Kconfig
index dd13c0220681..199b1d4c0b85 100644
--- a/drivers/staging/usbip/Kconfig
+++ b/drivers/staging/usbip/Kconfig
@@ -1,6 +1,6 @@
 config USBIP_CORE
-	tristate "USB/IP support (EXPERIMENTAL)"
-	depends on USB && NET && EXPERIMENTAL
+	tristate "USB/IP support"
+	depends on USB && NET
 	default N
 	---help---
 	  This enables pushing USB packets over IP to allow remote
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index 92ced35e6b7f..c8d79a7f0e0e 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -187,10 +187,14 @@ static void stub_shutdown_connection(struct usbip_device *ud)
 	}
 
 	/* 1. stop threads */
-	if (ud->tcp_rx)
+	if (ud->tcp_rx) {
 		kthread_stop_put(ud->tcp_rx);
-	if (ud->tcp_tx)
+		ud->tcp_rx = NULL;
+	}
+	if (ud->tcp_tx) {
 		kthread_stop_put(ud->tcp_tx);
+		ud->tcp_tx = NULL;
+	}
 
 	/*
 	 * 2. close the socket
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/staging/usbip/stub_rx.c
index 1d5b3fc62160..694cfd7596f3 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/staging/usbip/stub_rx.c
@@ -155,7 +155,7 @@ static int tweak_set_configuration_cmd(struct urb *urb)
 	 * eventually reassigned to the device as far as driver matching
 	 * condition is kept.
 	 *
-	 * Unfortunatelly, an existing usbip connection will be dropped
+	 * Unfortunately, an existing usbip connection will be dropped
 	 * due to this driver unbinding. So, skip here.
 	 * A user may need to set a special configuration value before
 	 * exporting the device.
diff --git a/drivers/staging/usbip/usbip_common.c b/drivers/staging/usbip/usbip_common.c
index 95beb76497d6..57f11f9cd8a5 100644
--- a/drivers/staging/usbip/usbip_common.c
+++ b/drivers/staging/usbip/usbip_common.c
@@ -22,7 +22,9 @@
 #include <linux/fs.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
+#include <linux/stat.h>
 #include <linux/module.h>
+#include <linux/moduleparam.h>
 #include <net/sock.h>
 
 #include "usbip_common.h"
@@ -36,6 +38,8 @@ unsigned long usbip_debug_flag = 0xffffffff;
 unsigned long usbip_debug_flag;
 #endif
 EXPORT_SYMBOL_GPL(usbip_debug_flag);
+module_param(usbip_debug_flag, ulong, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(usbip_debug_flag, "debug flags (defined in usbip_common.h)");
 
 /* FIXME */
 struct device_attribute dev_attr_usbip_debug;
diff --git a/drivers/staging/usbip/userspace/configure.ac b/drivers/staging/usbip/userspace/configure.ac
index bf5cf49cb554..43e641e5ac06 100644
--- a/drivers/staging/usbip/userspace/configure.ac
+++ b/drivers/staging/usbip/userspace/configure.ac
@@ -56,11 +56,11 @@ AC_ARG_WITH([tcp-wrappers],
 	    [AS_HELP_STRING([--with-tcp-wrappers],
 			    [use the libwrap (TCP wrappers) library])],
 	    dnl [ACTION-IF-GIVEN]
-	    [saved_LIBS="$LIBS"
-	     if test "$withval" = "yes"; then
+	    [if test "$withval" = "yes"; then
 		     AC_MSG_RESULT([yes])
 		     AC_MSG_CHECKING([for hosts_access in -lwrap])
-		     LIBS="-lwrap $LIBS"
+		     saved_LIBS="$LIBS"
+		     LIBS="-lwrap $saved_LIBS"
 		     AC_TRY_LINK(
 		       [int hosts_access(); int allow_severity, deny_severity;],
 		       [hosts_access()],
@@ -69,9 +69,9 @@ AC_ARG_WITH([tcp-wrappers],
 				  [use tcp wrapper]) wrap_LIB="-lwrap"],
 		       [AC_MSG_RESULT([not found]); exit 1])
 	     else
-		     AC_MSG_RESULT([no])
-	     fi
-	     LIBS="$saved_LIBS"],
+		     AC_MSG_RESULT([no]);
+		     LIBS="$saved_LIBS"
+	     fi],
 	    dnl [ACTION-IF-NOT-GIVEN]
 	    [AC_MSG_RESULT([(default)])
 	     AC_MSG_CHECKING([for hosts_access in -lwrap])
diff --git a/drivers/staging/usbip/userspace/doc/usbip.8 b/drivers/staging/usbip/userspace/doc/usbip.8
index 1653bb2cd7d1..6e0d74503792 100644
--- a/drivers/staging/usbip/userspace/doc/usbip.8
+++ b/drivers/staging/usbip/userspace/doc/usbip.8
@@ -3,69 +3,87 @@
 usbip \- manage USB/IP devices
 .SH SYNOPSIS
 .B usbip
-[\fIoptions\fR]
+[\foptions\R] <\fIcommand\fR> <\fIargs\fR>
 
 .SH DESCRIPTION
-Devices exported by USB/IP servers can be listed, attached and
-detached using this program.
+On a USB/IP server, devices can be listed, bound, and unbound using
+this program.  On a USB/IP client, devices exported by USB/IP servers
+can be listed, attached and detached.
 
 .SH OPTIONS
 .HP
-\fB\-a\fR, \fB\-\-attach\fR <host> <bus_id>
+\fB\-\-debug\fR
 .IP
-Attach a remote USB device.
+Print debugging information.
+.PP
+
+.HP
+\fB\-\-log\fR
+.IP
+Log to syslog.
+.PP
+
+.SH COMMANDS
+.HP
+\fBversion\fR
+.IP
+Show version and exit.
 .PP
 
 .HP
-\fB\-x\fR, \fB\-\-attachall\fR <host>
+\fBhelp\fR [\fIcommand\fR]
 .IP
-Attach all remote USB devices on the specific host.
+Print the program help message, or help on a specific command, and
+then exit.
 .PP
 
 .HP
-\fB\-d\fR, \fB\-\-detach\fR <ports>
+\fBattach\fR \-\-host=<\fIhost\fR> \-\-busid=<\fIbus_id\fR>
+.IP
+Attach a remote USB device.
+.PP
+
+.HP
+\fBdetach\fR \-\-port=<\fIport\fR>
 .IP
 Detach an imported USB device.
 .PP
 
 .HP
-\fB\-l\fR, \fB\-\-list\fR <hosts>
+\fBbind\fR \-\-busid=<\fIbusid\fR>
 .IP
-List exported USB devices.
+Make a device exportable.
 .PP
 
 .HP
-\fB\-p\fR, \fB\-\-port\fR
+\fBunbind\fR \-\-busid=<\fIbusid\fR>
 .IP
-List virtual USB port status.
+Stop exporting a device so it can be used by a local driver.
 .PP
 
 .HP
-\fB\-D\fR, \fB\-\-debug\fR
+\fBlist\fR \-\-remote=<\fIhost\fR>
 .IP
-Print debugging information.
+List USB devices exported by a remote host.
 .PP
 
 .HP
-\fB\-v\fR, \fB\-\-version\fR
+\fBlist\fR \-\-local
 .IP
-Show version.
+List local USB devices.
 .PP
 
+
 .SH EXAMPLES
 
-    client:# usbip --list server
+    client:# usbip list --remote=server
         - List exportable usb devices on the server.
 
-    client:# usbip --attach server 1-2
+    client:# usbip attach --host=server --busid=1-2
         - Connect the remote USB device.
 
-    client:# usbip --port
-        - Show virtual port status.
-
-    client:# usbip --detach 0
+    client:# usbip detach --port=0
         - Detach the usb device.
 
 .SH "SEE ALSO"
-\fBusbipd\fP\fB(8)\fB\fP,
-\fBusbip_attach_driver\fP\fB(8)\fB\fP
+\fBusbipd\fP\fB(8)\fB\fP
diff --git a/drivers/staging/usbip/userspace/doc/usbip_bind_driver.8 b/drivers/staging/usbip/userspace/doc/usbip_bind_driver.8
deleted file mode 100644
index d43bbd6be934..000000000000
--- a/drivers/staging/usbip/userspace/doc/usbip_bind_driver.8
+++ /dev/null
@@ -1,42 +0,0 @@
-.TH USBIP_BIND_DRIVER "8" "February 2009" "usbip" "System Administration Utilities"
-.SH NAME
-usbip_bind_driver \- change driver binding for USB/IP
-
-.SH SYNOPSIS
-.B usbip_bind_driver
-[\fIoptions\fR]
-
-.SH DESCRIPTION
-Driver bindings for USB devices can be changed using
-this program. It is used to export and unexport USB
-devices over USB/IP.
-
-.SH OPTIONS
-.TP
-\fB\-u\fR, \fB\-\-usbip\fR <busid>
-Make a device exportable
-.TP
-\fB\-o\fR, \fB\-\-other\fR <busid>
-Use a device by a local driver
-.TP
-\fB\-l\fR, \fB\-\-list\fR
-Print usb devices and their drivers
-.TP
-\fB\-L\fR, \fB\-\-list2\fR
-Print usb devices and their drivers in parseable mode
-
-.SH EXAMPLES
-
-    server:# usbip_bind_driver --list
-        - List driver assignments for usb devices.
-
-    server:# usbip_bind_driver --usbip 1-2
-        - Bind usbip-host.ko to the device of busid 1-2.
-        - A usb device 1-2 is now exportable to other hosts!
-
-    server:# usbip_bind_driver --other 1-2
-        - Shutdown exporting and use the device locally.
-
-.SH "SEE ALSO"
-\fBusbip\fP\fB(8)\fB\fP,
-\fBusbipd\fP\fB(8)\fB\fP
diff --git a/drivers/staging/usbip/userspace/doc/usbipd.8 b/drivers/staging/usbip/userspace/doc/usbipd.8
index 006559f1df25..d896936f1780 100644
--- a/drivers/staging/usbip/userspace/doc/usbipd.8
+++ b/drivers/staging/usbip/userspace/doc/usbipd.8
@@ -10,7 +10,7 @@ usbipd \- USB/IP server daemon
 provides USB/IP clients access to exported USB devices.
 
 Devices have to explicitly be exported using
-.B usbip_bind_driver
+.B usbip bind
 before usbipd makes them available to other hosts.
 
 The daemon accepts connections from USB/IP clients
@@ -29,6 +29,11 @@ Run as a daemon process.
 Print debugging information.
 .PP
 
+\fB\-h\fR, \fB\-\-help\fR
+.IP
+Print the program help message and exit.
+.PP
+
 .HP
 \fB\-v\fR, \fB\-\-version\fR
 .IP
@@ -48,15 +53,14 @@ USB/IP client can connect and use exported devices.
     server:# usbipd -D
         - Start usbip daemon.
 
-    server:# usbip_bind_driver --list
+    server:# usbip list --local
         - List driver assignments for usb devices.
 
-    server:# usbip_bind_driver --usbip 1-2
+    server:# usbip bind --busid=1-2
         - Bind usbip-host.ko to the device of busid 1-2.
         - A usb device 1-2 is now exportable to other hosts!
-        - Use 'usbip_bind_driver --other 1-2' when you want to shutdown exporting and use the device locally.
+        - Use 'usbip unbind --busid=1-2' when you want to shutdown exporting and use the device locally.
 
 .SH "SEE ALSO"
-\fBusbip\fP\fB(8)\fB\fP,
-\fBusbip_attach_driver\fP\fB(8)\fB\fP
+\fBusbip\fP\fB(8)\fB\fP
 
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index 12a9a5fbc797..620d1beb4587 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -220,7 +220,7 @@ static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 
 	pr_info("changed %d\n", changed);
 
-	if (hcd->state == HC_STATE_SUSPENDED)
+	if ((hcd->state == HC_STATE_SUSPENDED) && (changed == 1))
 		usb_hcd_resume_root_hub(hcd);
 
 done:
@@ -749,6 +749,7 @@ static void vhci_device_unlink_cleanup(struct vhci_device *vdev)
 {
 	struct vhci_unlink *unlink, *tmp;
 
+	spin_lock(&the_controller->lock);
 	spin_lock(&vdev->priv_lock);
 
 	list_for_each_entry_safe(unlink, tmp, &vdev->unlink_tx, list) {
@@ -757,9 +758,12 @@ static void vhci_device_unlink_cleanup(struct vhci_device *vdev)
 		kfree(unlink);
 	}
 
-	list_for_each_entry_safe(unlink, tmp, &vdev->unlink_rx, list) {
+	while (!list_empty(&vdev->unlink_rx)) {
 		struct urb *urb;
 
+		unlink = list_first_entry(&vdev->unlink_rx, struct vhci_unlink,
+			list);
+
 		/* give back URB of unanswered unlink request */
 		pr_info("unlink cleanup rx %lu\n", unlink->unlink_seqnum);
 
@@ -774,18 +778,24 @@ static void vhci_device_unlink_cleanup(struct vhci_device *vdev)
 
 		urb->status = -ENODEV;
 
-		spin_lock(&the_controller->lock);
 		usb_hcd_unlink_urb_from_ep(vhci_to_hcd(the_controller), urb);
+
+		list_del(&unlink->list);
+
+		spin_unlock(&vdev->priv_lock);
 		spin_unlock(&the_controller->lock);
 
 		usb_hcd_giveback_urb(vhci_to_hcd(the_controller), urb,
 				     urb->status);
 
-		list_del(&unlink->list);
+		spin_lock(&the_controller->lock);
+		spin_lock(&vdev->priv_lock);
+
 		kfree(unlink);
 	}
 
 	spin_unlock(&vdev->priv_lock);
+	spin_unlock(&the_controller->lock);
 }
 
 /*
@@ -804,11 +814,14 @@ static void vhci_shutdown_connection(struct usbip_device *ud)
 	}
 
 	/* kill threads related to this sdev, if v.c. exists */
-	if (vdev->ud.tcp_rx)
+	if (vdev->ud.tcp_rx) {
 		kthread_stop_put(vdev->ud.tcp_rx);
-	if (vdev->ud.tcp_tx)
+		vdev->ud.tcp_rx = NULL;
+	}
+	if (vdev->ud.tcp_tx) {
 		kthread_stop_put(vdev->ud.tcp_tx);
-
+		vdev->ud.tcp_tx = NULL;
+	}
 	pr_info("stop threads\n");
 
 	/* active connection is closed */
@@ -828,11 +841,11 @@ static void vhci_shutdown_connection(struct usbip_device *ud)
 	 *	disable endpoints. pending urbs are unlinked(dequeued).
 	 *
 	 * NOTE: After calling rh_port_disconnect(), the USB device drivers of a
-	 * deteched device should release used urbs in a cleanup function(i.e.
+	 * detached device should release used urbs in a cleanup function (i.e.
 	 * xxx_disconnect()). Therefore, vhci_hcd does not need to release
 	 * pushed urbs and their private data in this function.
 	 *
-	 * NOTE: vhci_dequeue() must be considered carefully. When shutdowning
+	 * NOTE: vhci_dequeue() must be considered carefully. When shutting down
 	 * a connection, vhci_shutdown_connection() expects vhci_dequeue()
 	 * gives back pushed urbs and frees their private data by request of
 	 * the cleanup function of a USB driver. When unlinking a urb with an
diff --git a/drivers/staging/vme/devices/vme_pio2_core.c b/drivers/staging/vme/devices/vme_pio2_core.c
index 4bf8e05ac312..dad8281915bf 100644
--- a/drivers/staging/vme/devices/vme_pio2_core.c
+++ b/drivers/staging/vme/devices/vme_pio2_core.c
@@ -10,6 +10,8 @@
  * option) any later version.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/types.h>
@@ -163,15 +165,13 @@ static int __init pio2_init(void)
 	int retval = 0;
 
 	if (bus_num == 0) {
-		printk(KERN_ERR "%s: No cards, skipping registration\n",
-			driver_name);
+		pr_err("No cards, skipping registration\n");
 		goto err_nocard;
 	}
 
 	if (bus_num > PIO2_CARDS_MAX) {
-		printk(KERN_ERR
-			"%s: Driver only able to handle %d PIO2 Cards\n",
-			driver_name, PIO2_CARDS_MAX);
+		pr_err("Driver only able to handle %d PIO2 Cards\n",
+		       PIO2_CARDS_MAX);
 		bus_num = PIO2_CARDS_MAX;
 	}
 
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index e25645e226e3..c3f94f311ca7 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -64,13 +64,13 @@ static unsigned int bus_num;
  *
  * However the VME driver at http://www.vmelinux.org/ is rather old and doesn't
  * even support the tsi148 chipset (which has 8 master and 8 slave windows).
- * We'll run with this or now as far as possible, however it probably makes
+ * We'll run with this for now as far as possible, however it probably makes
  * sense to get rid of the old mappings and just do everything dynamically.
  *
  * So for now, we'll restrict the driver to providing 4 masters and 4 slaves as
  * defined above and try to support at least some of the interface from
- * http://www.vmelinux.org/ as an alternative drive can be written providing a
- * saner interface later.
+ * http://www.vmelinux.org/ as an alternative the driver can be written
+ * providing a saner interface later.
  *
  * The vmelinux.org driver never supported slave images, the devices reserved
  * for slaves were repurposed to support all 8 master images on the UniverseII!
@@ -242,7 +242,7 @@ static ssize_t resource_to_user(int minor, char __user *buf, size_t count,
 }
 
 /*
- * We are going ot alloc a page during init per window for small transfers.
+ * We are going to alloc a page during init per window for small transfers.
  * Small transfers will go user space -> buffer -> VME. Larger (more than a
  * page) transfers will lock the user space buffer into memory and then
  * transfer the data directly from the user space buffers out to VME.
@@ -396,7 +396,7 @@ static ssize_t vme_user_write(struct file *file, const char __user *buf,
 	default:
 		retval = -EINVAL;
 	}
-	
+
 	mutex_unlock(&image[minor].mutex);
 
 	if (retval > 0)
diff --git a/drivers/staging/vt6655/80211mgr.h b/drivers/staging/vt6655/80211mgr.h
index 3bdab3f56f1c..65780f28db41 100644
--- a/drivers/staging/vt6655/80211mgr.h
+++ b/drivers/staging/vt6655/80211mgr.h
@@ -186,7 +186,7 @@
 
 
 //
-// Cipher Suite Selectors defiened in 802.11i
+// Cipher Suite Selectors defined in 802.11i
 //
 #define WLAN_11i_CSS_USE_GROUP              0
 #define WLAN_11i_CSS_WEP40                  1
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index e7b93a21e3b2..8d2c6a789ab2 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -28,8 +28,8 @@
  * Functions:
  *      BBuGetFrameTime        - Calculate data frame transmitting time
  *      BBvCaculateParameter   - Caculate PhyLength, PhyService and Phy Signal parameter for baseband Tx
- *      BBbReadEmbeded         - Embeded read baseband register via MAC
- *      BBbWriteEmbeded        - Embeded write baseband register via MAC
+ *      BBbReadEmbedded         - Embedded read baseband register via MAC
+ *      BBbWriteEmbedded        - Embedded write baseband register via MAC
  *      BBbIsRegBitsOn         - Test if baseband register bits on
  *      BBbIsRegBitsOff        - Test if baseband register bits off
  *      BBbVT3253Init          - VIA VT3253 baseband chip init code
@@ -40,7 +40,7 @@
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
  *      08-07-2003 Bryan YC Fan:  Add MAXIM2827/2825 and RFMD2959 support.
- *      08-26-2003 Kyle Hsu    :  Modify BBuGetFrameTime() and BBvCaculateParameter().
+ *      08-26-2003 Kyle Hsu    :  Modify BBuGetFrameTime() and BBvCalculateParameter().
  *                                cancel the setting of MAC_REG_SOFTPWRCTL on BBbVT3253Init().
  *                                Add the comments.
  *      09-01-2003 Bryan YC Fan:  RF & BB tables updated.
@@ -1826,7 +1826,7 @@ BBuGetFrameTime (
 }
 
 /*
- * Description: Caculate Length, Service, and Signal fields of Phy for Tx
+ * Description: Calculate Length, Service, and Signal fields of Phy for Tx
  *
  * Parameters:
  *  In:
@@ -1842,7 +1842,7 @@ BBuGetFrameTime (
  *
  */
 void
-BBvCaculateParameter (
+BBvCalculateParameter (
     PSDevice pDevice,
     unsigned int cbFrameLength,
     unsigned short wRate,
@@ -2001,7 +2001,7 @@ BBvCaculateParameter (
 }
 
 /*
- * Description: Read a byte from BASEBAND, by embeded programming
+ * Description: Read a byte from BASEBAND, by embedded programming
  *
  * Parameters:
  *  In:
@@ -2013,7 +2013,7 @@ BBvCaculateParameter (
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbReadEmbeded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData)
+bool BBbReadEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData)
 {
     unsigned short ww;
     unsigned char byValue;
@@ -2043,7 +2043,7 @@ bool BBbReadEmbeded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned ch
 
 
 /*
- * Description: Write a Byte to BASEBAND, by embeded programming
+ * Description: Write a Byte to BASEBAND, by embedded programming
  *
  * Parameters:
  *  In:
@@ -2056,7 +2056,7 @@ bool BBbReadEmbeded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned ch
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbWriteEmbeded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData)
+bool BBbWriteEmbedded (unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData)
 {
     unsigned short ww;
     unsigned char byValue;
@@ -2102,7 +2102,7 @@ bool BBbIsRegBitsOn (unsigned long dwIoBase, unsigned char byBBAddr, unsigned ch
 {
     unsigned char byOrgData;
 
-    BBbReadEmbeded(dwIoBase, byBBAddr, &byOrgData);
+    BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
     return (byOrgData & byTestBits) == byTestBits;
 }
 
@@ -2125,7 +2125,7 @@ bool BBbIsRegBitsOff (unsigned long dwIoBase, unsigned char byBBAddr, unsigned c
 {
     unsigned char byOrgData;
 
-    BBbReadEmbeded(dwIoBase, byBBAddr, &byOrgData);
+    BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
     return (byOrgData & byTestBits) == 0;
 }
 
@@ -2155,14 +2155,14 @@ bool BBbVT3253Init (PSDevice pDevice)
     if (byRFType == RF_RFMD2959) {
         if (byLocalID <= REV_ID_VT3253_A1) {
             for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++) {
-                bResult &= BBbWriteEmbeded(dwIoBase,byVT3253InitTab_RFMD[ii][0],byVT3253InitTab_RFMD[ii][1]);
+                bResult &= BBbWriteEmbedded(dwIoBase,byVT3253InitTab_RFMD[ii][0],byVT3253InitTab_RFMD[ii][1]);
             }
         } else {
             for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++) {
-                bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_RFMD[ii][0],byVT3253B0_RFMD[ii][1]);
+                bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_RFMD[ii][0],byVT3253B0_RFMD[ii][1]);
             }
             for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++) {
-        	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC4_RFMD2959[ii][0],byVT3253B0_AGC4_RFMD2959[ii][1]);
+        	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC4_RFMD2959[ii][0],byVT3253B0_AGC4_RFMD2959[ii][1]);
             }
             VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
             MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
@@ -2177,10 +2177,10 @@ bool BBbVT3253Init (PSDevice pDevice)
         pDevice->ldBmThreshold[3] = 0;
     } else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S) ) {
         for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
     	}
         for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
     	}
         pDevice->abyBBVGA[0] = 0x1C;
         pDevice->abyBBVGA[1] = 0x10;
@@ -2192,10 +2192,10 @@ bool BBbVT3253Init (PSDevice pDevice)
         pDevice->ldBmThreshold[3] = 0;
     } else if (byRFType == RF_UW2451) {
         for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
-    	        bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
+    	        bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
     	}
         for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
     	}
         VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
         MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
@@ -2210,28 +2210,28 @@ bool BBbVT3253Init (PSDevice pDevice)
         pDevice->ldBmThreshold[3] = 0;
     } else if (byRFType == RF_UW2452) {
         for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
-            bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
+            bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_UW2451[ii][0],byVT3253B0_UW2451[ii][1]);
     	}
         // Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbeded(dwIoBase,0x09,0x41);
+        //bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
         // Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbeded(dwIoBase,0x0a,0x28);
+        //bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
         // Select VC1/VC2, CR215 = 0x02->0x06
-        bResult &= BBbWriteEmbeded(dwIoBase,0xd7,0x06);
+        bResult &= BBbWriteEmbedded(dwIoBase,0xd7,0x06);
 
         //{{RobertYu:20050125, request by Jack
-        bResult &= BBbWriteEmbeded(dwIoBase,0x90,0x20);
-        bResult &= BBbWriteEmbeded(dwIoBase,0x97,0xeb);
+        bResult &= BBbWriteEmbedded(dwIoBase,0x90,0x20);
+        bResult &= BBbWriteEmbedded(dwIoBase,0x97,0xeb);
         //}}
 
         //{{RobertYu:20050221, request by Jack
-        bResult &= BBbWriteEmbeded(dwIoBase,0xa6,0x00);
-        bResult &= BBbWriteEmbeded(dwIoBase,0xa8,0x30);
+        bResult &= BBbWriteEmbedded(dwIoBase,0xa6,0x00);
+        bResult &= BBbWriteEmbedded(dwIoBase,0xa8,0x30);
         //}}
-        bResult &= BBbWriteEmbeded(dwIoBase,0xb0,0x58);
+        bResult &= BBbWriteEmbedded(dwIoBase,0xb0,0x58);
 
         for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
     	}
         //VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23); // RobertYu: 20050104, 20050131 disable PA_Delay
         //MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0); // RobertYu: 20050104, 20050131 disable PA_Delay
@@ -2248,10 +2248,10 @@ bool BBbVT3253Init (PSDevice pDevice)
 
     } else if (byRFType == RF_VT3226) {
         for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
     	}
         for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
     	}
         pDevice->abyBBVGA[0] = 0x1C;
         pDevice->abyBBVGA[1] = 0x10;
@@ -2266,20 +2266,20 @@ bool BBbVT3253Init (PSDevice pDevice)
          //{{ RobertYu: 20050104
     } else if (byRFType == RF_AIROHA7230) {
         for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AIROHA2230[ii][0],byVT3253B0_AIROHA2230[ii][1]);
     	}
 
         //{{ RobertYu:20050223, request by JerryChung
         // Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbeded(dwIoBase,0x09,0x41);
+        //bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
         // Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
-        //bResult &= BBbWriteEmbeded(dwIoBase,0x0a,0x28);
+        //bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);
         // Select VC1/VC2, CR215 = 0x02->0x06
-        bResult &= BBbWriteEmbeded(dwIoBase,0xd7,0x06);
+        bResult &= BBbWriteEmbedded(dwIoBase,0xd7,0x06);
         //}}
 
         for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
-    	    bResult &= BBbWriteEmbeded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
+    	    bResult &= BBbWriteEmbedded(dwIoBase,byVT3253B0_AGC[ii][0],byVT3253B0_AGC[ii][1]);
     	}
         pDevice->abyBBVGA[0] = 0x1C;
         pDevice->abyBBVGA[1] = 0x10;
@@ -2297,8 +2297,8 @@ bool BBbVT3253Init (PSDevice pDevice)
     }
 
     if (byLocalID > REV_ID_VT3253_A1) {
-        BBbWriteEmbeded(dwIoBase, 0x04, 0x7F);
-        BBbWriteEmbeded(dwIoBase, 0x0D, 0x01);
+        BBbWriteEmbedded(dwIoBase, 0x04, 0x7F);
+        BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);
     }
 
     return bResult;
@@ -2324,7 +2324,7 @@ void BBvReadAllRegs (unsigned long dwIoBase, unsigned char *pbyBBRegs)
     int  ii;
     unsigned char byBase = 1;
     for (ii = 0; ii < BB_MAX_CONTEXT_SIZE; ii++) {
-        BBbReadEmbeded(dwIoBase, (unsigned char)(ii*byBase), pbyBBRegs);
+        BBbReadEmbedded(dwIoBase, (unsigned char)(ii*byBase), pbyBBRegs);
         pbyBBRegs += byBase;
     }
 }
@@ -2350,39 +2350,39 @@ void BBvLoopbackOn (PSDevice pDevice)
     unsigned long dwIoBase = pDevice->PortOffset;
 
     //CR C9 = 0x00
-    BBbReadEmbeded(dwIoBase, 0xC9, &pDevice->byBBCRc9);//CR201
-    BBbWriteEmbeded(dwIoBase, 0xC9, 0);
-    BBbReadEmbeded(dwIoBase, 0x4D, &pDevice->byBBCR4d);//CR77
-    BBbWriteEmbeded(dwIoBase, 0x4D, 0x90);
+    BBbReadEmbedded(dwIoBase, 0xC9, &pDevice->byBBCRc9);//CR201
+    BBbWriteEmbedded(dwIoBase, 0xC9, 0);
+    BBbReadEmbedded(dwIoBase, 0x4D, &pDevice->byBBCR4d);//CR77
+    BBbWriteEmbedded(dwIoBase, 0x4D, 0x90);
 
     //CR 88 = 0x02(CCK), 0x03(OFDM)
-    BBbReadEmbeded(dwIoBase, 0x88, &pDevice->byBBCR88);//CR136
+    BBbReadEmbedded(dwIoBase, 0x88, &pDevice->byBBCR88);//CR136
 
     if (pDevice->uConnectionRate <= RATE_11M) { //CCK
         // Enable internal digital loopback: CR33 |= 0000 0001
-        BBbReadEmbeded(dwIoBase, 0x21, &byData);//CR33
-        BBbWriteEmbeded(dwIoBase, 0x21, (unsigned char)(byData | 0x01));//CR33
+        BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
+        BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData | 0x01));//CR33
         // CR154 = 0x00
-        BBbWriteEmbeded(dwIoBase, 0x9A, 0);   //CR154
+        BBbWriteEmbedded(dwIoBase, 0x9A, 0);   //CR154
 
-        BBbWriteEmbeded(dwIoBase, 0x88, 0x02);//CR239
+        BBbWriteEmbedded(dwIoBase, 0x88, 0x02);//CR239
     }
     else { //OFDM
         // Enable internal digital loopback:CR154 |= 0000 0001
-        BBbReadEmbeded(dwIoBase, 0x9A, &byData);//CR154
-        BBbWriteEmbeded(dwIoBase, 0x9A, (unsigned char)(byData | 0x01));//CR154
+        BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
+        BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData | 0x01));//CR154
         // CR33 = 0x00
-        BBbWriteEmbeded(dwIoBase, 0x21, 0);   //CR33
+        BBbWriteEmbedded(dwIoBase, 0x21, 0);   //CR33
 
-        BBbWriteEmbeded(dwIoBase, 0x88, 0x03);//CR239
+        BBbWriteEmbedded(dwIoBase, 0x88, 0x03);//CR239
     }
 
     //CR14 = 0x00
-    BBbWriteEmbeded(dwIoBase, 0x0E, 0);//CR14
+    BBbWriteEmbedded(dwIoBase, 0x0E, 0);//CR14
 
     // Disable TX_IQUN
-    BBbReadEmbeded(pDevice->PortOffset, 0x09, &pDevice->byBBCR09);
-    BBbWriteEmbeded(pDevice->PortOffset, 0x09, (unsigned char)(pDevice->byBBCR09 & 0xDE));
+    BBbReadEmbedded(pDevice->PortOffset, 0x09, &pDevice->byBBCR09);
+    BBbWriteEmbedded(pDevice->PortOffset, 0x09, (unsigned char)(pDevice->byBBCR09 & 0xDE));
 }
 
 /*
@@ -2403,22 +2403,22 @@ void BBvLoopbackOff (PSDevice pDevice)
     unsigned char byData;
     unsigned long dwIoBase = pDevice->PortOffset;
 
-    BBbWriteEmbeded(dwIoBase, 0xC9, pDevice->byBBCRc9);//CR201
-    BBbWriteEmbeded(dwIoBase, 0x88, pDevice->byBBCR88);//CR136
-    BBbWriteEmbeded(dwIoBase, 0x09, pDevice->byBBCR09);//CR136
-    BBbWriteEmbeded(dwIoBase, 0x4D, pDevice->byBBCR4d);//CR77
+    BBbWriteEmbedded(dwIoBase, 0xC9, pDevice->byBBCRc9);//CR201
+    BBbWriteEmbedded(dwIoBase, 0x88, pDevice->byBBCR88);//CR136
+    BBbWriteEmbedded(dwIoBase, 0x09, pDevice->byBBCR09);//CR136
+    BBbWriteEmbedded(dwIoBase, 0x4D, pDevice->byBBCR4d);//CR77
 
     if (pDevice->uConnectionRate <= RATE_11M) { // CCK
         // Set the CR33 Bit2 to disable internal Loopback.
-        BBbReadEmbeded(dwIoBase, 0x21, &byData);//CR33
-        BBbWriteEmbeded(dwIoBase, 0x21, (unsigned char)(byData & 0xFE));//CR33
+        BBbReadEmbedded(dwIoBase, 0x21, &byData);//CR33
+        BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData & 0xFE));//CR33
     }
     else { // OFDM
-        BBbReadEmbeded(dwIoBase, 0x9A, &byData);//CR154
-        BBbWriteEmbeded(dwIoBase, 0x9A, (unsigned char)(byData & 0xFE));//CR154
+        BBbReadEmbedded(dwIoBase, 0x9A, &byData);//CR154
+        BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData & 0xFE));//CR154
     }
-    BBbReadEmbeded(dwIoBase, 0x0E, &byData);//CR14
-    BBbWriteEmbeded(dwIoBase, 0x0E, (unsigned char)(byData | 0x80));//CR14
+    BBbReadEmbedded(dwIoBase, 0x0E, &byData);//CR14
+    BBbWriteEmbedded(dwIoBase, 0x0E, (unsigned char)(byData | 0x80));//CR14
 
 }
 
@@ -2442,7 +2442,7 @@ BBvSetShortSlotTime (PSDevice pDevice)
     unsigned char byBBRxConf=0;
     unsigned char byBBVGA=0;
 
-    BBbReadEmbeded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
+    BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
 
     if (pDevice->bShortSlotTime) {
         byBBRxConf &= 0xDF;//1101 1111
@@ -2451,12 +2451,12 @@ BBvSetShortSlotTime (PSDevice pDevice)
     }
 
     // patch for 3253B0 Baseband with Cardbus module
-    BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byBBVGA);
+    BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
     if (byBBVGA == pDevice->abyBBVGA[0]) {
         byBBRxConf |= 0x20;//0010 0000
     }
 
-    BBbWriteEmbeded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
+    BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 
 }
 
@@ -2464,9 +2464,9 @@ void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
 {
     unsigned char byBBRxConf=0;
 
-    BBbWriteEmbeded(pDevice->PortOffset, 0xE7, byData);
+    BBbWriteEmbedded(pDevice->PortOffset, 0xE7, byData);
 
-    BBbReadEmbeded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
+    BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
     // patch for 3253B0 Baseband with Cardbus module
     if (byData == pDevice->abyBBVGA[0]) {
         byBBRxConf |= 0x20;//0010 0000
@@ -2476,7 +2476,7 @@ void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
         byBBRxConf |= 0x20;//0010 0000
     }
     pDevice->byBBVGACurrent = byData;
-    BBbWriteEmbeded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
+    BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 }
 
 
@@ -2495,10 +2495,10 @@ void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
 void
 BBvSoftwareReset (unsigned long dwIoBase)
 {
-    BBbWriteEmbeded(dwIoBase, 0x50, 0x40);
-    BBbWriteEmbeded(dwIoBase, 0x50, 0);
-    BBbWriteEmbeded(dwIoBase, 0x9C, 0x01);
-    BBbWriteEmbeded(dwIoBase, 0x9C, 0);
+    BBbWriteEmbedded(dwIoBase, 0x50, 0x40);
+    BBbWriteEmbedded(dwIoBase, 0x50, 0);
+    BBbWriteEmbedded(dwIoBase, 0x9C, 0x01);
+    BBbWriteEmbedded(dwIoBase, 0x9C, 0);
 }
 
 /*
@@ -2518,9 +2518,9 @@ BBvPowerSaveModeON (unsigned long dwIoBase)
 {
     unsigned char byOrgData;
 
-    BBbReadEmbeded(dwIoBase, 0x0D, &byOrgData);
+    BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
     byOrgData |= BIT0;
-    BBbWriteEmbeded(dwIoBase, 0x0D, byOrgData);
+    BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
 }
 
 /*
@@ -2540,9 +2540,9 @@ BBvPowerSaveModeOFF (unsigned long dwIoBase)
 {
     unsigned char byOrgData;
 
-    BBbReadEmbeded(dwIoBase, 0x0D, &byOrgData);
+    BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
     byOrgData &= ~(BIT0);
-    BBbWriteEmbeded(dwIoBase, 0x0D, byOrgData);
+    BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
 }
 
 /*
@@ -2567,7 +2567,7 @@ BBvSetTxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
 #ifdef	PLICE_DEBUG
 	//printk("Enter BBvSetTxAntennaMode\n");
 #endif
-    BBbReadEmbeded(dwIoBase, 0x09, &byBBTxConf);//CR09
+    BBbReadEmbedded(dwIoBase, 0x09, &byBBTxConf);//CR09
     if (byAntennaMode == ANT_DIVERSITY) {
         // bit 1 is diversity
         byBBTxConf |= 0x02;
@@ -2581,7 +2581,7 @@ BBvSetTxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
         byBBTxConf &= 0xFD; // 1111 1101
         byBBTxConf |= 0x04;
     }
-    BBbWriteEmbeded(dwIoBase, 0x09, byBBTxConf);//CR09
+    BBbWriteEmbedded(dwIoBase, 0x09, byBBTxConf);//CR09
 }
 
 
@@ -2606,7 +2606,7 @@ BBvSetRxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
 {
     unsigned char byBBRxConf;
 
-    BBbReadEmbeded(dwIoBase, 0x0A, &byBBRxConf);//CR10
+    BBbReadEmbedded(dwIoBase, 0x0A, &byBBRxConf);//CR10
     if (byAntennaMode == ANT_DIVERSITY) {
         byBBRxConf |= 0x01;
 
@@ -2616,7 +2616,7 @@ BBvSetRxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
         byBBRxConf &= 0xFE; // 1111 1110
         byBBRxConf |= 0x02;
     }
-    BBbWriteEmbeded(dwIoBase, 0x0A, byBBRxConf);//CR10
+    BBbWriteEmbedded(dwIoBase, 0x0A, byBBRxConf);//CR10
 }
 
 
@@ -2635,15 +2635,15 @@ BBvSetRxAntennaMode (unsigned long dwIoBase, unsigned char byAntennaMode)
 void
 BBvSetDeepSleep (unsigned long dwIoBase, unsigned char byLocalID)
 {
-    BBbWriteEmbeded(dwIoBase, 0x0C, 0x17);//CR12
-    BBbWriteEmbeded(dwIoBase, 0x0D, 0xB9);//CR13
+    BBbWriteEmbedded(dwIoBase, 0x0C, 0x17);//CR12
+    BBbWriteEmbedded(dwIoBase, 0x0D, 0xB9);//CR13
 }
 
 void
 BBvExitDeepSleep (unsigned long dwIoBase, unsigned char byLocalID)
 {
-    BBbWriteEmbeded(dwIoBase, 0x0C, 0x00);//CR12
-    BBbWriteEmbeded(dwIoBase, 0x0D, 0x01);//CR13
+    BBbWriteEmbedded(dwIoBase, 0x0C, 0x00);//CR12
+    BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);//CR13
 }
 
 
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index be2d68909490..9b5bc9c58d9f 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -73,12 +73,12 @@
 
 #define BBvClearFOE(dwIoBase)                               \
 {                                                           \
-    BBbWriteEmbeded(dwIoBase, 0xB1, 0);                     \
+    BBbWriteEmbedded(dwIoBase, 0xB1, 0);                     \
 }
 
 #define BBvSetFOE(dwIoBase)                                 \
 {                                                           \
-    BBbWriteEmbeded(dwIoBase, 0xB1, 0x0C);                  \
+    BBbWriteEmbedded(dwIoBase, 0xB1, 0x0C);                  \
 }
 
 
@@ -97,7 +97,7 @@ BBuGetFrameTime(
     );
 
 void
-BBvCaculateParameter (
+BBvCalculateParameter (
     PSDevice pDevice,
     unsigned int cbFrameLength,
     unsigned short wRate,
@@ -107,8 +107,8 @@ BBvCaculateParameter (
     unsigned char *pbyPhySgn
     );
 
-bool BBbReadEmbeded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData);
-bool BBbWriteEmbeded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData);
+bool BBbReadEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char *pbyData);
+bool BBbWriteEmbedded(unsigned long dwIoBase, unsigned char byBBAddr, unsigned char byData);
 
 void BBvReadAllRegs(unsigned long dwIoBase, unsigned char *pbyBBRegs);
 void BBvLoopbackOn(PSDevice pDevice);
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index fcffa4f0f4e3..fe57fb880a8f 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -784,8 +784,8 @@ BSSDBbIsSTAInNodeDB(void *pMgmtObject, unsigned char *abyDstAddr,
 /*+
  *
  * Routine Description:
- *    Find an empty node and allocated; if no empty found,
- *    instand used of most inactive one.
+ *    Find an empty node and allocat it; if there is no empty node,
+ *    then use the most inactive one.
  *
  * Return Value:
  *    None
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index 2721e0798496..319ca482f003 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -28,9 +28,9 @@
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
  *      CARDvSetLoopbackMode - Set Loopback mode
  *      CARDbSoftwareReset - Sortware reset NIC
- *      CARDqGetTSFOffset - Caculate TSFOffset
+ *      CARDqGetTSFOffset - Calculate TSFOffset
  *      CARDbGetCurrentTSF - Read Current NIC TSF counter
- *      CARDqGetNextTBTT - Caculate Next Beacon TSF counter
+ *      CARDqGetNextTBTT - Calculate Next Beacon TSF counter
  *      CARDvSetFirstNextTBTT - Set NIC Beacon time
  *      CARDvUpdateNextTBTT - Sync. NIC Beacon time
  *      CARDbRadioPowerOff - Turn Off NIC Radio Power
@@ -100,7 +100,7 @@ const unsigned short cwRXBCNTSFOff[MAX_RATE] =
 
 static
 void
-s_vCaculateOFDMRParameter(
+s_vCalculateOFDMRParameter(
     unsigned char byRate,
     CARD_PHY_TYPE ePHYType,
     unsigned char *pbyTxRate,
@@ -111,7 +111,7 @@ s_vCaculateOFDMRParameter(
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Caculate TxRate and RsvTime fields for RSPINF in OFDM mode.
+ * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
  *
  * Parameters:
  *  In:
@@ -126,7 +126,7 @@ s_vCaculateOFDMRParameter(
  */
 static
 void
-s_vCaculateOFDMRParameter (
+s_vCalculateOFDMRParameter (
     unsigned char byRate,
     CARD_PHY_TYPE ePHYType,
     unsigned char *pbyTxRate,
@@ -251,7 +251,7 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
     MACvSelectPage1(pDevice->PortOffset);
 
     //RSPINF_b_1
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
                          PK_TYPE_11B,
@@ -262,7 +262,7 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     ///RSPINF_b_2
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
                          PK_TYPE_11B,
@@ -273,7 +273,7 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_b_5
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
                          PK_TYPE_11B,
@@ -284,7 +284,7 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_b_11
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
                          PK_TYPE_11B,
@@ -295,51 +295,51 @@ s_vSetRSPINF (PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs,
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_a_6
-    s_vCaculateOFDMRParameter(RATE_6M,
+    s_vCalculateOFDMRParameter(RATE_6M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_9
-    s_vCaculateOFDMRParameter(RATE_9M,
+    s_vCalculateOFDMRParameter(RATE_9M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_12
-    s_vCaculateOFDMRParameter(RATE_12M,
+    s_vCalculateOFDMRParameter(RATE_12M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_18
-    s_vCaculateOFDMRParameter(RATE_18M,
+    s_vCalculateOFDMRParameter(RATE_18M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_24
-    s_vCaculateOFDMRParameter(RATE_24M,
+    s_vCalculateOFDMRParameter(RATE_24M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_36
-    s_vCaculateOFDMRParameter(
+    s_vCalculateOFDMRParameter(
                               VNTWIFIbyGetACKTxRate(RATE_36M, pvSupportRateIEs, pvExtSupportRateIEs),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_48
-    s_vCaculateOFDMRParameter(
+    s_vCalculateOFDMRParameter(
                               VNTWIFIbyGetACKTxRate(RATE_48M, pvSupportRateIEs, pvExtSupportRateIEs),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_54
-    s_vCaculateOFDMRParameter(
+    s_vCalculateOFDMRParameter(
                               VNTWIFIbyGetACKTxRate(RATE_54M, pvSupportRateIEs, pvExtSupportRateIEs),
                               ePHYType,
                               &byTxRate,
@@ -461,22 +461,22 @@ bool CARDbSetPhyParameter (void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigne
             pDevice->abyBBVGA[0] = 0x20;
             pDevice->abyBBVGA[2] = 0x10;
             pDevice->abyBBVGA[3] = 0x10;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x1C) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
             }
         } else if (pDevice->byRFType == RF_UW2452) {
             MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
             pDevice->abyBBVGA[0] = 0x18;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x14) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE1, 0x57);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0x57);
             }
         } else {
             MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
         }
-        BBbWriteEmbeded(pDevice->PortOffset, 0x88, 0x03);
+        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x03);
         bySlot = C_SLOT_SHORT;
         bySIFS = C_SIFS_A;
         byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
@@ -490,19 +490,19 @@ bool CARDbSetPhyParameter (void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigne
             pDevice->abyBBVGA[0] = 0x1C;
             pDevice->abyBBVGA[2] = 0x00;
             pDevice->abyBBVGA[3] = 0x00;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x20) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
             }
         } else if (pDevice->byRFType == RF_UW2452) {
             pDevice->abyBBVGA[0] = 0x14;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x18) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE1, 0xD3);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
             }
         }
-        BBbWriteEmbeded(pDevice->PortOffset, 0x88, 0x02);
+        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x02);
         bySlot = C_SLOT_LONG;
         bySIFS = C_SIFS_BG;
         byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
@@ -517,19 +517,19 @@ bool CARDbSetPhyParameter (void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigne
             pDevice->abyBBVGA[0] = 0x1C;
             pDevice->abyBBVGA[2] = 0x00;
             pDevice->abyBBVGA[3] = 0x00;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x20) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
             }
         } else if (pDevice->byRFType == RF_UW2452) {
             pDevice->abyBBVGA[0] = 0x14;
-            BBbReadEmbeded(pDevice->PortOffset, 0xE7, &byData);
+            BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
             if (byData == 0x18) {
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-                BBbWriteEmbeded(pDevice->PortOffset, 0xE1, 0xD3);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+                BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
             }
         }
-        BBbWriteEmbeded(pDevice->PortOffset, 0x88, 0x08);
+        BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x08);
         bySIFS = C_SIFS_BG;
         if(VNTWIFIbIsShortSlotTime(wCapInfo)) {
             bySlot = C_SLOT_SHORT;
@@ -1354,7 +1354,8 @@ CARDbSetQuiet (
 /*
  *
  * Description:
- *    Do Quiet, It will called by either ISR (after start) or VNTWIFI (before start) so do not need SPINLOCK
+ *    Do Quiet, It will be called by either ISR(after start) 
+ *    or VNTWIFI(before start) so we do not need a SPINLOCK
  *
  * Parameters:
  *  In:
@@ -1738,7 +1739,7 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
     MACvSelectPage1(pDevice->PortOffset);
 
     //RSPINF_b_1
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          CARDwGetCCKControlRate((void *)pDevice, RATE_1M),
                          PK_TYPE_11B,
@@ -1749,7 +1750,7 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     ///RSPINF_b_2
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          CARDwGetCCKControlRate((void *)pDevice, RATE_2M),
                          PK_TYPE_11B,
@@ -1760,7 +1761,7 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_b_5
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          CARDwGetCCKControlRate((void *)pDevice, RATE_5M),
                          PK_TYPE_11B,
@@ -1771,7 +1772,7 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_b_11
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          CARDwGetCCKControlRate((void *)pDevice, RATE_11M),
                          PK_TYPE_11B,
@@ -1782,56 +1783,56 @@ void CARDvSetRSPINF (void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 
     VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen,MAKEWORD(bySignal,byServ)));
     //RSPINF_a_6
-    s_vCaculateOFDMRParameter(RATE_6M,
+    s_vCalculateOFDMRParameter(RATE_6M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_9
-    s_vCaculateOFDMRParameter(RATE_9M,
+    s_vCalculateOFDMRParameter(RATE_9M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_12
-    s_vCaculateOFDMRParameter(RATE_12M,
+    s_vCalculateOFDMRParameter(RATE_12M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_18
-    s_vCaculateOFDMRParameter(RATE_18M,
+    s_vCalculateOFDMRParameter(RATE_18M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
    VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_24
-    s_vCaculateOFDMRParameter(RATE_24M,
+    s_vCalculateOFDMRParameter(RATE_24M,
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_36
-    s_vCaculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_36M),
+    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_36M),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_48
-    s_vCaculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_48M),
+    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_48M),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate,byRsvTime));
     //RSPINF_a_54
-    s_vCaculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
+    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
     VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate,byRsvTime));
 
     //RSPINF_a_72
-    s_vCaculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
+    s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
                               ePHYType,
                               &byTxRate,
                               &byRsvTime);
@@ -2041,7 +2042,7 @@ bool CARDbSoftwareReset (void *pDeviceHandler)
 
 
 /*
- * Description: Caculate TSF offset of two TSF input
+ * Description: Calculate TSF offset of two TSF input
  *              Get TSF Offset from RxBCN's TSF and local TSF
  *
  * Parameters:
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
index efbb8f45f728..b86ec1b6d187 100644
--- a/drivers/staging/vt6655/datarate.c
+++ b/drivers/staging/vt6655/datarate.c
@@ -126,7 +126,7 @@ DATARATEbyGetRateIdx (
 /*+
  *
  * Routine Description:
- *      Rate fallback Algorithm Implementaion
+ *      Rate fallback Algorithm Implementation
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index c5e6b98d3e4e..e54e00bc5665 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -327,7 +327,7 @@ typedef struct tagSDeFragControlBlock
 //flags for driver status
 #define     DEVICE_FLAGS_OPENED          0x00010000UL
 #define     DEVICE_FLAGS_WOL_ENABLED     0x00080000UL
-//flags for capbilities
+//flags for capabilities
 #define     DEVICE_FLAGS_TX_ALIGN        0x01000000UL
 #define     DEVICE_FLAGS_HAVE_CAM        0x02000000UL
 #define     DEVICE_FLAGS_FLOW_CTRL       0x04000000UL
@@ -567,7 +567,7 @@ typedef struct __device_info {
     bool bPrvActive4RadioOFF;
     bool bGPIOBlockRead;
 
-    // Beacon releated
+    // Beacon related
     unsigned short wSeqCounter;
     unsigned short wBCNBufLen;
     bool bBeaconBufReady;
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 89d1c22695a0..9e3b3f2bbe53 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -290,7 +290,7 @@ DEFINE_PCI_DEVICE_TABLE(vt6655_pci_id_table) = {
 
 
 static int  vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent);
-static bool vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice, PCHIP_INFO);
+static void vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice, PCHIP_INFO);
 static void device_free_info(PSDevice pDevice);
 static bool device_get_pci_info(PSDevice, struct pci_dev* pcid);
 static void device_print_info(PSDevice pDevice);
@@ -347,21 +347,22 @@ static int Config_FileGetParameter(unsigned char *string,
 
 
 
-static char* get_chip_name(int chip_id) {
-    int i;
-    for (i=0;chip_info_table[i].name!=NULL;i++)
-        if (chip_info_table[i].chip_id==chip_id)
-            break;
-    return chip_info_table[i].name;
+static char* get_chip_name(int chip_id)
+{
+	int i;
+	for (i = 0; chip_info_table[i].name != NULL; i++)
+		if (chip_info_table[i].chip_id == chip_id)
+			break;
+	return chip_info_table[i].name;
 }
 
 static void __devexit vt6655_remove(struct pci_dev *pcid)
 {
-    PSDevice pDevice=pci_get_drvdata(pcid);
+	PSDevice pDevice = pci_get_drvdata(pcid);
 
-    if (pDevice==NULL)
-        return;
-    device_free_info(pDevice);
+	if (pDevice == NULL)
+		return;
+	device_free_info(pDevice);
 
 }
 
@@ -397,31 +398,29 @@ device_set_bool_opt(unsigned int *opt, int val,bool def,u32 flag, char* name,cha
     }
 }
 */
-static void
-device_get_options(PSDevice pDevice, int index, char* devname) {
-
-    POPTIONS pOpts = &(pDevice->sOpts);
-  pOpts->nRxDescs0=RX_DESC_DEF0;
-  pOpts->nRxDescs1=RX_DESC_DEF1;
-  pOpts->nTxDescs[0]=TX_DESC_DEF0;
-  pOpts->nTxDescs[1]=TX_DESC_DEF1;
-pOpts->flags|=DEVICE_FLAGS_IP_ALIGN;
-  pOpts->int_works=INT_WORKS_DEF;
-  pOpts->rts_thresh=RTS_THRESH_DEF;
-  pOpts->frag_thresh=FRAG_THRESH_DEF;
-  pOpts->data_rate=DATA_RATE_DEF;
-  pOpts->channel_num=CHANNEL_DEF;
-
-pOpts->flags|=DEVICE_FLAGS_PREAMBLE_TYPE;
-pOpts->flags|=DEVICE_FLAGS_OP_MODE;
-//pOpts->flags|=DEVICE_FLAGS_PS_MODE;
-  pOpts->short_retry=SHORT_RETRY_DEF;
-  pOpts->long_retry=LONG_RETRY_DEF;
-  pOpts->bbp_type=BBP_TYPE_DEF;
-pOpts->flags|=DEVICE_FLAGS_80211h_MODE;
-pOpts->flags|=DEVICE_FLAGS_DiversityANT;
-
-
+static void device_get_options(PSDevice pDevice, int index, char* devname)
+{
+	POPTIONS pOpts = &(pDevice->sOpts);
+
+	pOpts->nRxDescs0 = RX_DESC_DEF0;
+	pOpts->nRxDescs1 = RX_DESC_DEF1;
+	pOpts->nTxDescs[0] = TX_DESC_DEF0;
+	pOpts->nTxDescs[1] = TX_DESC_DEF1;
+	pOpts->flags |= DEVICE_FLAGS_IP_ALIGN;
+	pOpts->int_works = INT_WORKS_DEF;
+	pOpts->rts_thresh = RTS_THRESH_DEF;
+	pOpts->frag_thresh = FRAG_THRESH_DEF;
+	pOpts->data_rate = DATA_RATE_DEF;
+	pOpts->channel_num = CHANNEL_DEF;
+
+	pOpts->flags |= DEVICE_FLAGS_PREAMBLE_TYPE;
+	pOpts->flags |= DEVICE_FLAGS_OP_MODE;
+	//pOpts->flags|=DEVICE_FLAGS_PS_MODE;
+	pOpts->short_retry = SHORT_RETRY_DEF;
+	pOpts->long_retry = LONG_RETRY_DEF;
+	pOpts->bbp_type = BBP_TYPE_DEF;
+	pOpts->flags |= DEVICE_FLAGS_80211h_MODE;
+	pOpts->flags |= DEVICE_FLAGS_DiversityANT;
 }
 
 static void
@@ -518,7 +517,7 @@ static void s_vCompleteCurrentMeasure (PSDevice pDevice, unsigned char byResult)
 
 
 //
-// Initialiation of MAC & BBP registers
+// Initialisation of MAC & BBP registers
 //
 
 static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
@@ -894,18 +893,15 @@ static bool device_release_WPADEV(PSDevice pDevice)
     return true;
 }
 
-
 static const struct net_device_ops device_netdev_ops = {
-    .ndo_open               = device_open,
-    .ndo_stop               = device_close,
-    .ndo_do_ioctl           = device_ioctl,
-    .ndo_get_stats          = device_get_stats,
-    .ndo_start_xmit         = device_xmit,
-    .ndo_set_rx_mode	    = device_set_multi,
+	.ndo_open               = device_open,
+	.ndo_stop               = device_close,
+	.ndo_do_ioctl           = device_ioctl,
+	.ndo_get_stats          = device_get_stats,
+	.ndo_start_xmit         = device_xmit,
+	.ndo_set_rx_mode	= device_set_multi,
 };
 
-
-
 static int __devinit
 vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 {
@@ -926,7 +922,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
     if (dev == NULL) {
         printk(KERN_ERR DEVICE_NAME ": allocate net device failed \n");
-        return -ENODEV;
+        return -ENOMEM;
     }
 
     // Chain it all together
@@ -939,9 +935,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
         bFirst=false;
     }
 
-    if (!vt6655_init_info(pcid, &pDevice, pChip_info)) {
-        return -ENOMEM;
-    }
+    vt6655_init_info(pcid, &pDevice, pChip_info);
     pDevice->dev = dev;
     pDevice->next_module = root_device_dev;
     root_device_dev = dev;
@@ -1064,7 +1058,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
     //Mask out the options cannot be set to the chip
     pDevice->sOpts.flags &= pChip_info->flags;
 
-    //Enable the chip specified capbilities
+    //Enable the chip specified capabilities
     pDevice->flags = pDevice->sOpts.flags | (pChip_info->flags & 0xFF000000UL);
     pDevice->tx_80211 = device_dma0_tx_80211;
     pDevice->sMgmtObj.pAdapter = (void *)pDevice;
@@ -1105,7 +1099,7 @@ static void device_print_info(PSDevice pDevice)
 
 }
 
-static bool __devinit vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice,
+static void __devinit vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice,
     PCHIP_INFO pChip_info) {
 
     PSDevice p;
@@ -1129,8 +1123,6 @@ static bool __devinit vt6655_init_info(struct pci_dev* pcid, PSDevice* ppDevice,
     (*ppDevice)->multicast_limit =32;
 
     spin_lock_init(&((*ppDevice)->lock));
-
-    return true;
 }
 
 static bool device_get_pci_info(PSDevice pDevice, struct pci_dev* pcid) {
@@ -1678,7 +1670,7 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
                 uFrameSize = pTD->pTDInfo->dwReqCount - uFIFOHeaderSize;
                 pTxBufHead = (PSTxBufHead) (pTD->pTDInfo->buf);
                 // Update the statistics based on the Transmit status
-                // now, we DO'NT check TSR0_CDH
+                // now, we DONT check TSR0_CDH
 
                 STAvUpdateTDStatCounter(&pDevice->scStatistic,
                         byTsr0, byTsr1,
@@ -2660,7 +2652,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
         (pDevice->byLocalID != REV_ID_VT3253_B0) &&
         (pDevice->bBSSIDFilter == true)) {
         // update RSSI
-        //BBbReadEmbeded(pDevice->PortOffset, 0x3E, &byRSSI);
+        //BBbReadEmbedded(pDevice->PortOffset, 0x3E, &byRSSI);
         //pDevice->uCurrRSSI = byRSSI;
     }
     */
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index e8a71ba4b92c..373e9e4fc87d 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -718,7 +718,7 @@ device_receive_frame (
     if ((*pbyRSSI != 0) &&
         (pMgmt->pCurrBSS!=NULL)) {
         RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-        // Moniter if RSSI is too strong.
+        // Monitor if RSSI is too strong.
         pMgmt->pCurrBSS->byRSSIStatCnt++;
         pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
         pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index 6ac6f452b261..67b1b88b1b89 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -495,9 +495,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		return -EINVAL;
 	}
 
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= MAX_GROUP_KEY)
 			return -EINVAL;
         iNodeIndex = 0;
@@ -716,9 +714,7 @@ static int hostap_get_encryption(PSDevice pDevice,
 
 	param->u.crypt.err = 0;
 
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
         iNodeIndex = 0;
 	} else {
 	    if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index ef197efab049..afed6e33dfc7 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -111,7 +111,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	case WLAN_CMD_ZONETYPE_SET:
-		/* mike add :cann't support. */
+		/* mike add :can't support. */
 		result = -EOPNOTSUPP;
 		break;
 
@@ -539,11 +539,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			pMgmt->abyIBSSSuppRates[3] |= BIT7;
 		}
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %x %x %x %x\n",
-			pMgmt->abyIBSSSuppRates[2],
-			pMgmt->abyIBSSSuppRates[3],
-			pMgmt->abyIBSSSuppRates[4],
-			pMgmt->abyIBSSSuppRates[5]);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %*ph\n",
+				4, pMgmt->abyIBSSSuppRates + 2);
 
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index 77aad7f5ae76..5cdda8dab854 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -358,7 +358,7 @@ int iwctl_giwscan(struct net_device *dev,
 
 
 /*
- * Wireless Handler : set frequence or channel
+ * Wireless Handler : set frequency or channel
  */
 
 int iwctl_siwfreq(struct net_device *dev,
@@ -404,7 +404,7 @@ int iwctl_siwfreq(struct net_device *dev,
 }
 
 /*
- * Wireless Handler : get frequence or channel
+ * Wireless Handler : get frequency or channel
  */
 
 int iwctl_giwfreq(struct net_device *dev,
@@ -1346,7 +1346,7 @@ if((wrq->flags & IW_ENCODE_DISABLED)==0){
 
 		}else if(index>0){
 	//when the length is 0 the request only changes the default transmit key index
-	//check the new key has a non zero lenget
+	//check the new key if it has a non zero length
 	if(pDevice->bEncryptionEnable==false)
 	{
 		rc = -EINVAL;
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 774b0d4a7e06..194fedc715fa 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -372,7 +372,7 @@ bool KeybRemoveKey (
     int  i;
 
     if (is_broadcast_ether_addr(pbyBSSID)) {
-        // dealte all key
+        // delete all keys
         if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
             for (i=0;i<MAX_KEY_TABLE;i++) {
                 pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index f8d1651341f8..30c261579412 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -56,9 +56,9 @@
  *      MACbSafeStop - Stop MAC function
  *      MACbShutdown - Shut down MAC
  *      MACvInitialize - Initialize MAC
- *      MACvSetCurrRxDescAddr - Set Rx Descriptos Address
- *      MACvSetCurrTx0DescAddr - Set Tx0 Descriptos Address
- *      MACvSetCurrTx1DescAddr - Set Tx1 Descriptos Address
+ *      MACvSetCurrRxDescAddr - Set Rx Descriptors Address
+ *      MACvSetCurrTx0DescAddr - Set Tx0 Descriptors Address
+ *      MACvSetCurrTx1DescAddr - Set Tx1 Descriptors Address
  *      MACvTimer0MicroSDelay - Micro Second Delay Loop by MAC
  *
  * Revision History:
@@ -1498,7 +1498,7 @@ int     ii;
 
     wOffset += (uKeyIdx * 4);
     for (ii=0;ii<4;ii++) {
-        // alway push 128 bits
+        // always push 128 bits
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"3.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
         VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
         VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
@@ -1567,7 +1567,7 @@ int     ii;
     wOffset++;
     wOffset++;
     wOffset += (uKeyIdx * 4);
-    // alway push 128 bits
+    // always push 128 bits
     for (ii=0; ii<3; ii++) {
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
         VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
@@ -1696,7 +1696,7 @@ int     ii;
 
     wOffset += (uKeyIdx * 4);
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %lX, idx:%d\n", wOffset, *pdwKey, uKeyIdx);
-    // alway push 128 bits
+    // always push 128 bits
     for (ii=0; ii<4; ii++) {
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
         VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index e3ccfee90268..adfb366f4901 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -586,7 +586,7 @@
 #define PKT_TYPE_NONE           0x00    // turn off receiver
 #define PKT_TYPE_ALL_MULTICAST  0x80
 #define PKT_TYPE_PROMISCUOUS    0x40
-#define PKT_TYPE_DIRECTED       0x20    // obselete, directed address is always accepted
+#define PKT_TYPE_DIRECTED       0x20    // obsolete, directed address is always accepted
 #define PKT_TYPE_BROADCAST      0x10
 #define PKT_TYPE_MULTICAST      0x08
 #define PKT_TYPE_ERROR_WPA      0x04
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 1b91a8370954..63ae4adddf2f 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -191,7 +191,7 @@ void STAvUpdateRDStatCounter (PSStatCounter pStatistic,
         pStatistic->ullRsrOK++;
 
         if (cbFrameLength >= ETH_ALEN) {
-            // update counters in case that successful transmit
+            // update counters in case of successful transmit
             if (byRSR & RSR_ADDRBROAD) {
                 pStatistic->ullRxBroadcastFrames++;
                 pStatistic->ullRxBroadcastBytes += (unsigned long long) cbFrameLength;
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 4c0b02e8f0b5..661d534304cc 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -207,7 +207,7 @@ PSbConsiderPowerDown(
     if (pDevice->bCmdRunning)
         return false;
 
-    // Froce PSEN on
+    // Force PSEN on
     MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
 
     // check if all TD are empty,
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index aa696650b86c..aaa231aaf4e9 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -26,7 +26,7 @@
  * Date: Feb. 19, 2004
  *
  * Functions:
- *      IFRFbWriteEmbeded      - Embeded write RF register via MAC
+ *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
  *
  * Revision History:
  *
@@ -453,18 +453,18 @@ bool s_bAL7230Init (unsigned long dwIoBase)
     BBvPowerSaveModeOFF(dwIoBase); //RobertYu:20050106, have DC value for Calibration
 
     for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[ii]);
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[ii]);
 
     // PLL On
     MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
     //Calibration
     MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:active, RCK:diable
+    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:active, RCK:disable
     MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:diable, RCK:active
+    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:disable, RCK:active
     MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]); //TXDCOC:diable, RCK:diable
+    bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]); //TXDCOC:disable, RCK:disable
 
     MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
                                                      SOFTPWRCTL_SWPE2    |
@@ -490,9 +490,9 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
     // PLLON Off
     MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-    bResult &= IFRFbWriteEmbeded (dwIoBase, dwAL7230ChannelTable0[byChannel-1]); //Reg0
-    bResult &= IFRFbWriteEmbeded (dwIoBase, dwAL7230ChannelTable1[byChannel-1]); //Reg1
-    bResult &= IFRFbWriteEmbeded (dwIoBase, dwAL7230ChannelTable2[byChannel-1]); //Reg4
+    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable0[byChannel-1]); //Reg0
+    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable1[byChannel-1]); //Reg1
+    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL7230ChannelTable2[byChannel-1]); //Reg4
 
     // PLLOn On
     MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
@@ -574,7 +574,7 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Write to IF/RF, by embeded programming
+ * Description: Write to IF/RF, by embedded programming
  *
  * Parameters:
  *  In:
@@ -586,7 +586,7 @@ bool s_bAL7230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool IFRFbWriteEmbeded (unsigned long dwIoBase, unsigned long dwData)
+bool IFRFbWriteEmbedded (unsigned long dwIoBase, unsigned long dwData)
 {
     unsigned short ww;
     unsigned long dwValue;
@@ -669,11 +669,11 @@ bool RFbAL2230Init (unsigned long dwIoBase)
 
     //patch abnormal AL2230 frequency output
 //2008-8-21 chester <add>
-    IFRFbWriteEmbeded(dwIoBase, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+    IFRFbWriteEmbedded(dwIoBase, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
 
 
     for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL2230InitTable[ii]);
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[ii]);
 //2008-8-21 chester <add>
 MACvTimer0MicroSDelay(dwIoBase, 30); //delay 30 us
 
@@ -681,11 +681,11 @@ MACvTimer0MicroSDelay(dwIoBase, 30); //delay 30 us
     MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
     MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
     MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+    bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
     MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-    bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
+    bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
 
     MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
                                                      SOFTPWRCTL_SWPE2    |
@@ -704,8 +704,8 @@ bool RFbAL2230SelectChannel (unsigned long dwIoBase, unsigned char byChannel)
 
     bResult = true;
 
-    bResult &= IFRFbWriteEmbeded (dwIoBase, dwAL2230ChannelTable0[byChannel-1]);
-    bResult &= IFRFbWriteEmbeded (dwIoBase, dwAL2230ChannelTable1[byChannel-1]);
+    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL2230ChannelTable0[byChannel-1]);
+    bResult &= IFRFbWriteEmbedded (dwIoBase, dwAL2230ChannelTable1[byChannel-1]);
 
     // Set Channel[7] = 0 to tell H/W channel is changing now.
     VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
@@ -817,7 +817,7 @@ bool bResult = true;
 
     switch (pDevice->byRFType) {
         case RF_AIROHA7230 :
-            bResult = IFRFbWriteEmbeded (pDevice->PortOffset, 0x1ABAEF00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
+            bResult = IFRFbWriteEmbedded (pDevice->PortOffset, 0x1ABAEF00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
             break;
         default :
             bResult = true;
@@ -1072,23 +1072,23 @@ unsigned long dwMax7230Pwr = 0;
     switch (pDevice->byRFType) {
 
         case RF_AIROHA :
-            bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
             if (uRATE <= RATE_11M) {
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             } else {
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             }
             break;
 
 
         case RF_AL2230S :
-            bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
             if (uRATE <= RATE_11M) {
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             }else {
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             }
 
             break;
@@ -1098,7 +1098,7 @@ unsigned long dwMax7230Pwr = 0;
             dwMax7230Pwr = 0x080C0B00 | ( (byPwr) << 12 ) |
                            (BY_AL7230_REG_LEN << 3 )  | IFREGCTL_REGW;
 
-            bResult &= IFRFbWriteEmbeded(pDevice->PortOffset, dwMax7230Pwr);
+            bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwMax7230Pwr);
             break;
 
 
@@ -1166,24 +1166,24 @@ bool RFbAL7230SelectChannelPostProcess (unsigned long dwIoBase, unsigned char by
     if( (byOldChannel <= CB_MAX_CHANNEL_24G) && (byNewChannel > CB_MAX_CHANNEL_24G) )
     {
         // Change from 2.4G to 5G
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[2]); //Reg2
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[3]); //Reg3
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[5]); //Reg5
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[7]); //Reg7
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[10]);//Reg10
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[12]);//Reg12
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTableAMode[15]);//Reg15
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[2]); //Reg2
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[3]); //Reg3
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[5]); //Reg5
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[7]); //Reg7
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[10]);//Reg10
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[12]);//Reg12
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[15]);//Reg15
     }
     else if( (byOldChannel > CB_MAX_CHANNEL_24G) && (byNewChannel <= CB_MAX_CHANNEL_24G) )
     {
         // change from 5G to 2.4G
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[2]); //Reg2
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[3]); //Reg3
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[5]); //Reg5
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[7]); //Reg7
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[10]);//Reg10
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[12]);//Reg12
-        bResult &= IFRFbWriteEmbeded(dwIoBase, dwAL7230InitTable[15]);//Reg15
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[2]); //Reg2
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[3]); //Reg3
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[5]); //Reg5
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[7]); //Reg7
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[10]);//Reg10
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[12]);//Reg12
+        bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[15]);//Reg15
     }
 
     return bResult;
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index 73f09693ee7a..1da0fdeb2e1c 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -75,7 +75,7 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool IFRFbWriteEmbeded(unsigned long dwIoBase, unsigned long dwData);
+bool IFRFbWriteEmbedded(unsigned long dwIoBase, unsigned long dwData);
 bool RFbSelectChannel(unsigned long dwIoBase, unsigned char byRFType, unsigned char byChannel);
 bool RFbInit (
     PSDevice  pDevice
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 6935b37d5444..4972e57845c2 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -27,7 +27,7 @@
  * Functions:
  *      s_vGenerateTxParameter - Generate tx dma required parameter.
  *      vGenerateMACHeader - Translate 802.3 to 802.11 header
- *      cbGetFragCount - Caculate fragment number count
+ *      cbGetFragCount - Calculate fragment number count
  *      csBeacon_xmit - beacon tx function
  *      csMgmt_xmit - management tx function
  *      s_cbFillTxBufHead - fulfill tx dma buffer header
@@ -733,11 +733,11 @@ s_uFillDataHead (
         if (byFBOption == AUTO_FB_NONE) {
             PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
@@ -759,11 +759,11 @@ s_uFillDataHead (
             // Auto Fallback
             PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
@@ -788,7 +788,7 @@ s_uFillDataHead (
             // Auto Fallback
             PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -805,7 +805,7 @@ s_uFillDataHead (
         } else {
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -823,7 +823,7 @@ s_uFillDataHead (
     else {
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -871,11 +871,11 @@ s_vFillRTSHead (
         if (byFBOption == AUTO_FB_NONE) {
             PSRTS_g pBuf = (PSRTS_g)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
@@ -904,11 +904,11 @@ s_vFillRTSHead (
         else {
            PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
@@ -946,7 +946,7 @@ s_vFillRTSHead (
         if (byFBOption == AUTO_FB_NONE) {
             PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -975,7 +975,7 @@ s_vFillRTSHead (
         else {
             PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -1005,7 +1005,7 @@ s_vFillRTSHead (
     else if (byPktType == PK_TYPE_11B) {
         PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
         //Get SignalField,ServiceField,Length
-        BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+        BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
             (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
         );
         pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -1065,7 +1065,7 @@ s_vFillCTSHead (
             // Auto Fall back
             PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
 
@@ -1092,7 +1092,7 @@ s_vFillCTSHead (
         } else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
             PSCTS pBuf = (PSCTS)pvCTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
@@ -2568,7 +2568,7 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
     if (bIsPSPOLL) {
         // The MAC will automatically replace the Duration-field of MAC header by Duration-field
         // of  FIFO control header.
-        // This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
+        // This will cause AID-field of PS-POLL packet to be incorrect (Because PS-POLL's AID field is
         // in the same place of other packet's Duration-field).
         // And it will cause Cisco-AP to issue Disassociation-packet
         if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
@@ -2664,7 +2664,7 @@ CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
                                                           wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
     }
 
-    BBvCaculateParameter(pDevice, cbFrameSize, wCurrentRate, byPktType,
+    BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, byPktType,
         (unsigned short *)&(wLen), (unsigned char *)&(pTxDataHead->byServiceField), (unsigned char *)&(pTxDataHead->bySignalField)
     );
     pTxDataHead->wTransmitLength = cpu_to_le16(wLen);
@@ -2860,7 +2860,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
     // SetPower will cause error power TX state for OFDM Date packet in TX buffer.
     // 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-    //                    And cmd timer will wait data pkt TX finish before scanning so it's OK
+    //                    And cmd timer will wait data pkt TX to finish before scanning so it's OK
     //                    to set power here.
     if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
         RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
@@ -2957,7 +2957,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
     pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)cbMacHdLen << 10);
 
     // Notes:
-    // Although spec says MMPDU can be fragmented; In most case,
+    // Although spec says MMPDU can be fragmented; In most casses,
     // no one will send a MMPDU under fragmentation. With RTS may occur.
     pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
 
diff --git a/drivers/staging/vt6655/tcrc.c b/drivers/staging/vt6655/tcrc.c
index f9c28bf8a6af..1313c4cd0860 100644
--- a/drivers/staging/vt6655/tcrc.c
+++ b/drivers/staging/vt6655/tcrc.c
@@ -18,7 +18,7 @@
  *
  * File: tcrc.c
  *
- * Purpose: Implement functions to caculate CRC
+ * Purpose: Implement functions to calculate CRC
  *
  * Author: Tevin Chen
  *
diff --git a/drivers/staging/vt6655/tcrc.h b/drivers/staging/vt6655/tcrc.h
index d0449855beb1..a2044212d1c9 100644
--- a/drivers/staging/vt6655/tcrc.h
+++ b/drivers/staging/vt6655/tcrc.h
@@ -18,7 +18,7 @@
  *
  * File: tcrc.h
  *
- * Purpose: Implement functions to caculate CRC
+ * Purpose: Implement functions to calculate CRC
  *
  * Author: Tevin Chen
  *
diff --git a/drivers/staging/vt6655/tether.c b/drivers/staging/vt6655/tether.c
index 1cf8508e407d..28554bf75549 100644
--- a/drivers/staging/vt6655/tether.c
+++ b/drivers/staging/vt6655/tether.c
@@ -25,7 +25,7 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      ETHbyGetHashIndexByCrc32 - Caculate multicast hash value by CRC32
+ *      ETHbyGetHashIndexByCrc32 - Calculate multicast hash value by CRC32
  *      ETHbIsBufferCrc32Ok - Check CRC value of the buffer if Ok or not
  *
  * Revision History:
@@ -50,7 +50,7 @@
 
 
 /*
- * Description: Caculate multicast hash value by CRC32
+ * Description: Calculate multicast hash value by CRC32
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6655/tkip.c b/drivers/staging/vt6655/tkip.c
index ed3eac17ae8d..f141ba1cbb9b 100644
--- a/drivers/staging/vt6655/tkip.c
+++ b/drivers/staging/vt6655/tkip.c
@@ -170,7 +170,7 @@ unsigned int rotr1(unsigned int a)
 
 
 /*
- * Description: Caculate RC4Key fom TK, TA, and TSC
+ * Description: Calculate RC4Key fom TK, TA, and TSC
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index d645ecd89417..62c44b87d310 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -60,7 +60,7 @@
  * Parameters:
  *  In:
  *      pMgmtHandle - pointer to management object
- *      eOPMode     - Opreation Mode
+ *      eOPMode     - Operation Mode
  *  Out:
  *      none
  *
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
index 7b5b99c8cf14..94bd1fc42c93 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -121,7 +121,7 @@ vAdHocBeaconStop(PSDevice  pDevice)
 
     /*
      * temporarily stop Beacon packet for AdHoc Server
-     * if all of the following coditions are met:
+     * if all of the following conditions are met:
      *  (1) STA is in AdHoc mode
      *  (2) VT3253 is programmed as automatic Beacon Transmitting
      *  (3) One of the following conditions is met
@@ -812,8 +812,8 @@ printk("chester-abyDesireSSID=%s\n",((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySS
                             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail \n");
                         }
                         pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
-                        // check if sta ps enable, wait next pspoll
-                        // if sta ps disable, send all pending buffers.
+                        // check if sta ps enabled, and wait next pspoll.
+                        // if sta ps disable, then send all pending buffers.
                         if (pMgmt->sNodeDBTable[ii].bPSEnable)
                             break;
                     }
diff --git a/drivers/staging/vt6655/wmgr.c b/drivers/staging/vt6655/wmgr.c
index c46d51908ac0..b6f99ecbbeb5 100644
--- a/drivers/staging/vt6655/wmgr.c
+++ b/drivers/staging/vt6655/wmgr.c
@@ -26,8 +26,8 @@
  * Date: May 8, 2002
  *
  * Functions:
- *      nsMgrObjectInitial - Initialize Management Objet data structure
- *      vMgrObjectReset - Reset Management Objet data structure
+ *      nsMgrObjectInitial - Initialize Management Object data structure
+ *      vMgrObjectReset - Reset Management Object data structure
  *      vMgrAssocBeginSta - Start associate function
  *      vMgrReAssocBeginSta - Start reassociate function
  *      vMgrDisassocBeginSta - Start disassociate function
@@ -54,7 +54,7 @@
  *      bMgrPrepareBeaconToSend - Prepare Beacon frame
  *      s_vMgrLogStatus - Log 802.11 Status
  *      vMgrRxManagePacket - Rcv management frame dispatch function
- *      s_vMgrFormatTIM- Assember TIM field of beacon
+ *      s_vMgrFormatTIM- Assembler TIM field of beacon
  *      vMgrTimerInit- Initial 1-sec and command call back funtions
  *
  * Revision History:
@@ -425,7 +425,7 @@ vMgrTimerInit(
 /*+
  *
  * Routine Description:
- *    Reset the management object  structure.
+ *    Reset the management object structure.
  *
  * Return Value:
  *    None.
@@ -1287,14 +1287,14 @@ s_vMgrRxAuthentication(
     vMgrDecodeAuthen(&sFrame);
     switch (cpu_to_le16((*(sFrame.pwAuthSequence )))){
         case 1:
-            //AP funciton
+            //AP function
             s_vMgrRxAuthenSequence_1(pDevice,pMgmt, &sFrame);
             break;
         case 2:
             s_vMgrRxAuthenSequence_2(pDevice, pMgmt, &sFrame);
             break;
         case 3:
-            //AP funciton
+            //AP function
             s_vMgrRxAuthenSequence_3(pDevice, pMgmt, &sFrame);
             break;
         case 4:
@@ -1923,7 +1923,7 @@ s_vMgrRxBeacon(
             byIEChannel = sFrame.pDSParms->byCurrChannel;
         }
         if (byCurrChannel != byIEChannel) {
-            // adjust channel info. bcs we rcv adjcent channel pakckets
+            // adjust channel info. bcs we rcv adjacent channel packets
             bChannelHit = false;
             byCurrChannel = byIEChannel;
         }
@@ -2081,7 +2081,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
                 }
             }
             //
-            // Basic Rate Set may change dynamiclly
+            // Basic Rate Set may change dynamically
             //
             if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
                 uRateLen = WLAN_RATES_MAXLEN_11B;
@@ -2134,7 +2134,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
     }
 
 //    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Beacon 2 \n");
-    // check if CF field exisit
+    // check if CF field exists
     if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)) {
         if (sFrame.pCFParms->wCFPDurRemaining > 0) {
             // TODO: deal with CFP period to set NAV
@@ -2244,7 +2244,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
 		    if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
 		 	    pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 
-            // adhoc mode:TSF updated only when beacon larger then local TSF
+            // adhoc mode:TSF updated only when beacon larger than local TSF
             if (bTSFLargeDiff && bTSFOffsetPostive &&
                 (pMgmt->eCurrState == WMAC_STATE_JOINTED))
                 bUpdateTSF = true;
@@ -2252,7 +2252,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
             // During dpc, already in spinlocked.
             if (BSSDBbIsSTAInNodeDB(pMgmt, sFrame.pHdr->sA3.abyAddr2, &uNodeIndex)) {
 
-                // Update the STA, (Techically the Beacons of all the IBSS nodes
+                // Update the STA, (Technically the Beacons of all the IBSS nodes
 		        // should be identical, but that's not happening in practice.
                 pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
                                                         (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
@@ -2305,7 +2305,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
 */
             }
 
-            // if other stations jointed, indicate connect to upper layer..
+            // if other stations joined, indicate connection to upper layer..
             if (pMgmt->eCurrState == WMAC_STATE_STARTED) {
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
                 pMgmt->eCurrState = WMAC_STATE_JOINTED;
@@ -3081,8 +3081,8 @@ s_vMgrSynchBSS (
                //   }
            // }
   //   if( uSameBssidNum>=2) {	 //we only check AP in hidden sssid  mode
-        if ((pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||           //networkmanager 0.7.0 does not give the pairwise-key selsection,
-             (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {         // so we need re-selsect it according to real pairwise-key info.
+        if ((pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||           //networkmanager 0.7.0 does not give the pairwise-key selection,
+             (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {         // so we need re-select it according to real pairwise-key info.
                if(pCurr->bWPAValid == true)  {   //WPA-PSK
                           pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
 		    if(pCurr->abyPKType[0] == WPA_TKIP) {
@@ -3193,7 +3193,7 @@ s_vMgrFormatTIM(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -3310,7 +3310,7 @@ s_MgrMakeBeacon(
             *((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
             sFrame.pRSNWPA->len +=2;
 
-            // RSN Capabilites
+            // RSN Capabilities
             *((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len))=0;
             sFrame.pRSNWPA->len +=2;
             sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
@@ -3420,7 +3420,7 @@ s_MgrMakeBeacon(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -3611,7 +3611,7 @@ s_MgrMakeProbeResponse(
  *
  *
  * Return Value:
- *    A ptr to frame or NULL on allocation failue
+ *    A ptr to frame or NULL on allocation failure
  *
 -*/
 
@@ -3652,7 +3652,7 @@ s_MgrMakeAssocRequest(
     memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
     memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
-    // Set the capibility and listen interval
+    // Set the capability and listen interval
     *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
     *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
 
@@ -3762,7 +3762,7 @@ s_MgrMakeAssocRequest(
 
         sFrame.pRSNWPA->len +=6;
 
-        // RSN Capabilites
+        // RSN Capabilities
 
         *pbyRSN++=0x00;
         *pbyRSN++=0x00;
@@ -3831,7 +3831,7 @@ s_MgrMakeAssocRequest(
         }
         sFrame.pRSN->len +=6;
 
-        // RSN Capabilites
+        // RSN Capabilities
         if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
             memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
         } else {
@@ -3886,7 +3886,7 @@ s_MgrMakeAssocRequest(
  *
  *
  * Return Value:
- *    A ptr to frame or NULL on allocation failue
+ *    A ptr to frame or NULL on allocation failure
  *
 -*/
 
@@ -3929,7 +3929,7 @@ s_MgrMakeReAssocRequest(
     memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
     memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
-    /* Set the capibility and listen interval */
+    /* Set the capability and listen interval */
     *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
     *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
 
@@ -4019,7 +4019,7 @@ s_MgrMakeReAssocRequest(
 
         sFrame.pRSNWPA->len +=6;
 
-        // RSN Capabilites
+        // RSN Capabilities
         *pbyRSN++=0x00;
         *pbyRSN++=0x00;
         sFrame.pRSNWPA->len +=2;
@@ -4087,7 +4087,7 @@ s_MgrMakeReAssocRequest(
         }
         sFrame.pRSN->len +=6;
 
-        // RSN Capabilites
+        // RSN Capabilities
         if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
             memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
         } else {
@@ -4138,7 +4138,7 @@ s_MgrMakeReAssocRequest(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -4212,7 +4212,7 @@ s_MgrMakeAssocResponse(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -4333,7 +4333,7 @@ s_vMgrRxProbeResponse(
             byIEChannel = sFrame.pDSParms->byCurrChannel;
         }
         if (byCurrChannel != byIEChannel) {
-            // adjust channel info. bcs we rcv adjcent channel pakckets
+            // adjust channel info. bcs we rcv adjacent channel packets
             bChannelHit = false;
             byCurrChannel = byIEChannel;
         }
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
index e3ae562f521a..bfa67ae5d40a 100644
--- a/drivers/staging/vt6655/wmgr.h
+++ b/drivers/staging/vt6655/wmgr.h
@@ -286,7 +286,7 @@ typedef struct tagSMgmtObject
     CMD_STATE               eCommandState;
     unsigned int	uScanChannel;
 
-    // Desire joinning BSS vars
+    // Desire joining BSS vars
     unsigned char abyDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
     unsigned char abyDesireBSSID[WLAN_BSSID_LEN];
 
@@ -310,7 +310,7 @@ typedef struct tagSMgmtObject
     unsigned int	uScanEndCh;
     unsigned short wScanSteps;
     unsigned int	uScanBSSType;
-    // Desire scannig vars
+    // Desire scanning vars
     unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
     unsigned char abyScanBSSID[WLAN_BSSID_LEN];
 
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 0afb9fe0379a..4412fe9396a4 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -229,7 +229,7 @@ WPA_ParseRSN (
  * Parameters:
  *  In:
  *      byCmd    - Search type
- *      byEncrypt- Encrcypt Type
+ *      byEncrypt- Encrypt Type
  *      pBSSList - BSS list
  *  Out:
  *      none
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
index 744799cfe832..884db1abe123 100644
--- a/drivers/staging/vt6655/wpa2.c
+++ b/drivers/staging/vt6655/wpa2.c
@@ -175,16 +175,16 @@ WPA2vParseRSN (
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_USE_GROUP;
                         bUseGK = true;
                     } else if ( !memcmp(pbyOUI, abyOUIWEP40, 4)) {
-                        // Invialid CSS, continue to parsing
+                        // Invalid CSS, continue to parsing
                     } else if ( !memcmp(pbyOUI, abyOUITKIP, 4)) {
                         if (pBSSNode->byCSSGK != WLAN_11i_CSS_CCMP)
                             pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_TKIP;
                         else
-                            ; // Invialid CSS, continue to parsing
+                            ; // Invalid CSS, continue to parsing
                     } else if ( !memcmp(pbyOUI, abyOUICCMP, 4)) {
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_CCMP;
                     } else if ( !memcmp(pbyOUI, abyOUIWEP104, 4)) {
-                        // Invialid CSS, continue to parsing
+                        // Invalid CSS, continue to parsing
                     } else {
                         // any vendor checks here
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
@@ -329,7 +329,7 @@ WPA2uSetIEs(
         }
         pRSNIEs->len +=6;
 
-        // RSN Capabilites
+        // RSN Capabilities
         if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
             memcpy(&pRSNIEs->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
         } else {
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
index 732ba88dc796..2b6ae1e403bf 100644
--- a/drivers/staging/vt6655/wpactl.c
+++ b/drivers/staging/vt6655/wpactl.c
@@ -77,7 +77,7 @@ static void wpadev_setup(struct net_device *dev)
 
 /*
  * Description:
- *      register netdev for wpa supplicant deamon
+ *      register netdev for wpa supplicant daemon
  *
  * Parameters:
  *  In:
@@ -164,7 +164,7 @@ static int wpa_release_wpadev(PSDevice pDevice)
 
 /*
  * Description:
- *      Set enable/disable dev for wpa supplicant deamon
+ *      Set enable/disable dev for wpa supplicant daemon
  *
  * Parameters:
  *  In:
@@ -847,7 +847,7 @@ else
           if(!bWepEnabled)  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 	else pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
             //pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-            //pMgmt->bShareKeyAlgorithm = false; //20080717-06,<Modify> by chester//Fix Open mode, WEP encrytion
+            //pMgmt->bShareKeyAlgorithm = false; //20080717-06,<Modify> by chester//Fix Open mode, WEP encryption
            }
 //mike save old encryption status
 	pDevice->eOldEncryptionStatus = pDevice->eEncryptionStatus;
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
index 66e2eeae628b..82e93cb82053 100644
--- a/drivers/staging/vt6655/wroute.c
+++ b/drivers/staging/vt6655/wroute.c
@@ -18,7 +18,7 @@
  *
  * File: wroute.c
  *
- * Purpose: handle WMAC frame relay & filterring
+ * Purpose: handle WMAC frame relay & filtering
  *
  * Author: Lyndon Chen
  *
diff --git a/drivers/staging/vt6656/80211mgr.h b/drivers/staging/vt6656/80211mgr.h
index 515b9c1d4d10..e5db73be0e71 100644
--- a/drivers/staging/vt6656/80211mgr.h
+++ b/drivers/staging/vt6656/80211mgr.h
@@ -191,7 +191,7 @@
 
 
 //
-// Cipher Suite Selectors defiened in 802.11i
+// Cipher Suite Selectors defined in 802.11i
 //
 #define WLAN_11i_CSS_USE_GROUP              0
 #define WLAN_11i_CSS_WEP40                  1
@@ -720,7 +720,7 @@ typedef struct tagWLAN_FR_AUTHEN {
 
 } WLAN_FR_AUTHEN, *PWLAN_FR_AUTHEN;
 
-// Deauthenication
+// Deauthentication
 typedef struct tagWLAN_FR_DEAUTHEN {
 
     unsigned int                    uType;
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index 06f27f624db4..385501595b4d 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -27,7 +27,7 @@
  *
  * Functions:
  *      BBuGetFrameTime        - Calculate data frame transmitting time
- *      BBvCaculateParameter   - Caculate PhyLength, PhyService and Phy Signal parameter for baseband Tx
+ *      BBvCalculateParameter   - Calculate PhyLength, PhyService and Phy Signal parameter for baseband Tx
  *      BBbVT3184Init          - VIA VT3184 baseband chip init code
  *      BBvLoopbackOn          - Turn on BaseBand Loopback mode
  *      BBvLoopbackOff         - Turn off BaseBand Loopback mode
@@ -741,7 +741,7 @@ BBuGetFrameTime (
 }
 
 /*
- * Description: Caculate Length, Service, and Signal fields of Phy for Tx
+ * Description: Calculate Length, Service, and Signal fields of Phy for Tx
  *
  * Parameters:
  *  In:
@@ -757,7 +757,7 @@ BBuGetFrameTime (
  *
  */
 void
-BBvCaculateParameter (
+BBvCalculateParameter (
       PSDevice pDevice,
       unsigned int cbFrameLength,
       WORD wRate,
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index 8db8cd07d5f5..844d5a8b13e5 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -104,7 +104,7 @@ BBuGetFrameTime(
      WORD wRate
     );
 
-void BBvCaculateParameter(PSDevice pDevice,
+void BBvCalculateParameter(PSDevice pDevice,
 			  unsigned int cbFrameLength,
 			  WORD wRate,
 			  BYTE byPacketType,
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index 099936771e69..2ac066df8340 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -226,7 +226,7 @@ PKnownBSS BSSpSearchBSSList(void *hDeviceContext,
                 if (pSelect == NULL) {
                     pSelect = pCurrBSS;
                 } else {
-                    // compare RSSI, select signal strong one
+                    // compare RSSI, select the strongest signal 
                     if (pCurrBSS->uRSSI < pSelect->uRSSI) {
                         pSelect = pCurrBSS;
                     }
@@ -274,9 +274,9 @@ void BSSvClearBSSList(void *hDeviceContext, BOOL bKeepCurrBSSID)
             if (pMgmt->sBSSList[ii].bActive &&
 		!compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID,
 				    pMgmt->abyCurrBSSID)) {
- //mike mark: there are two same BSSID in list if that AP is in hidden ssid mode,one 's SSID is null,
- //                 but other's is obvious, so if it acssociate with your STA  exactly,you must keep two
- //                 of them!!!!!!!!!
+ //mike mark: there are two BSSID's in list. If that AP is in hidden ssid mode, one SSID is null,
+ //                 but other's might not be obvious, so if it associate's with your STA,
+ //                 you must keep the two of them!!
                // bKeepCurrBSSID = FALSE;
                 continue;
             }
@@ -489,7 +489,7 @@ BOOL BSSbInsertToBSSList(void *hDeviceContext,
     }
 
     if (pDevice->bUpdateBBVGA) {
-        // Moniter if RSSI is too strong.
+        // Monitor if RSSI is too strong.
         pBSSList->byRSSIStatCnt = 0;
         RFvRSSITodBm(pDevice, (BYTE)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
         pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
@@ -621,7 +621,7 @@ BOOL BSSbUpdateToBSSList(void *hDeviceContext,
 
     if (pRxPacket->uRSSI != 0) {
         RFvRSSITodBm(pDevice, (BYTE)(pRxPacket->uRSSI), &ldBm);
-        // Moniter if RSSI is too strong.
+        // Monitor if RSSI is too strong.
         pBSSList->byRSSIStatCnt++;
         pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
         pBSSList->ldBmAverage[pBSSList->byRSSIStatCnt] = ldBm;
@@ -687,8 +687,8 @@ BOOL BSSbIsSTAInNodeDB(void *hDeviceContext,
 /*+
  *
  * Routine Description:
- *    Find an empty node and allocated; if no empty found,
- *    instand used of most inactive one.
+ *    Find an empty node and allocate it; if no empty node
+ *    is found, then use the most inactive one.
  *
  * Return Value:
  *    None
@@ -718,7 +718,7 @@ void BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
         }
     }
 
-    // if not found replace uInActiveCount is largest one.
+    // if not found replace uInActiveCount with the largest one.
     if ( ii == (MAX_NODE_NUM + 1)) {
         *puNodeIndex = SelectIndex;
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Replace inactive node = %d\n", SelectIndex);
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index e3ddc0b55317..826520b03383 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -28,9 +28,9 @@
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
  *      CARDvSetLoopbackMode - Set Loopback mode
  *      CARDbSoftwareReset - Sortware reset NIC
- *      CARDqGetTSFOffset - Caculate TSFOffset
+ *      CARDqGetTSFOffset - Calculate TSFOffset
  *      CARDbGetCurrentTSF - Read Current NIC TSF counter
- *      CARDqGetNextTBTT - Caculate Next Beacon TSF counter
+ *      CARDqGetNextTBTT - Calculate Next Beacon TSF counter
  *      CARDvSetFirstNextTBTT - Set NIC Beacon time
  *      CARDvUpdateNextTBTT - Sync. NIC Beacon time
  *      CARDbRadioPowerOff - Turn Off NIC Radio Power
@@ -40,7 +40,7 @@
  *
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
- *      08-26-2003 Kyle Hsu:      Modify the defination type of dwIoBase.
+ *      08-26-2003 Kyle Hsu:      Modify the definition type of dwIoBase.
  *      09-01-2003 Bryan YC Fan:  Add vUpdateIFS().
  *
  */
@@ -200,7 +200,7 @@ static WORD swGetOFDMControlRate(void *pDeviceHandler, WORD wRateIdx)
 }
 
 /*
- * Description: Caculate TxRate and RsvTime fields for RSPINF in OFDM mode.
+ * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
  *
  * Parameters:
  *  In:
@@ -214,7 +214,7 @@ static WORD swGetOFDMControlRate(void *pDeviceHandler, WORD wRateIdx)
  *
  */
 void
-CARDvCaculateOFDMRParameter (
+CARDvCalculateOFDMRParameter (
       WORD wRate,
       BYTE byBBType,
      PBYTE pbyTxRate,
@@ -337,7 +337,7 @@ void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType)
     int     i;
 
     //RSPINF_b_1
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          swGetCCKControlRate(pDevice, RATE_1M),
                          PK_TYPE_11B,
@@ -347,7 +347,7 @@ void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType)
     );
 
     ///RSPINF_b_2
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          swGetCCKControlRate(pDevice, RATE_2M),
                          PK_TYPE_11B,
@@ -357,7 +357,7 @@ void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType)
     );
 
     //RSPINF_b_5
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          swGetCCKControlRate(pDevice, RATE_5M),
                          PK_TYPE_11B,
@@ -367,7 +367,7 @@ void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType)
     );
 
     //RSPINF_b_11
-    BBvCaculateParameter(pDevice,
+    BBvCalculateParameter(pDevice,
                          14,
                          swGetCCKControlRate(pDevice, RATE_11M),
                          PK_TYPE_11B,
@@ -377,55 +377,55 @@ void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType)
     );
 
     //RSPINF_a_6
-    CARDvCaculateOFDMRParameter (RATE_6M,
+    CARDvCalculateOFDMRParameter (RATE_6M,
                                  byBBType,
                                  &abyTxRate[0],
                                  &abyRsvTime[0]);
 
     //RSPINF_a_9
-    CARDvCaculateOFDMRParameter (RATE_9M,
+    CARDvCalculateOFDMRParameter (RATE_9M,
                                  byBBType,
                                  &abyTxRate[1],
                                  &abyRsvTime[1]);
 
     //RSPINF_a_12
-    CARDvCaculateOFDMRParameter (RATE_12M,
+    CARDvCalculateOFDMRParameter (RATE_12M,
                                  byBBType,
                                  &abyTxRate[2],
                                  &abyRsvTime[2]);
 
     //RSPINF_a_18
-    CARDvCaculateOFDMRParameter (RATE_18M,
+    CARDvCalculateOFDMRParameter (RATE_18M,
                                  byBBType,
                                  &abyTxRate[3],
                                  &abyRsvTime[3]);
 
     //RSPINF_a_24
-    CARDvCaculateOFDMRParameter (RATE_24M,
+    CARDvCalculateOFDMRParameter (RATE_24M,
                                  byBBType,
                                  &abyTxRate[4],
                                  &abyRsvTime[4]);
 
     //RSPINF_a_36
-    CARDvCaculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_36M),
+    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_36M),
                                  byBBType,
                                  &abyTxRate[5],
                                  &abyRsvTime[5]);
 
     //RSPINF_a_48
-    CARDvCaculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_48M),
+    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_48M),
                                  byBBType,
                                  &abyTxRate[6],
                                  &abyRsvTime[6]);
 
     //RSPINF_a_54
-    CARDvCaculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
+    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
                                  byBBType,
                                  &abyTxRate[7],
                                  &abyRsvTime[7]);
 
     //RSPINF_a_72
-    CARDvCaculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
+    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
                                  byBBType,
                                  &abyTxRate[8],
                                  &abyRsvTime[8]);
@@ -640,7 +640,7 @@ BYTE CARDbyGetPktType(void *pDeviceHandler)
 
 
 /*
- * Description: Caculate TSF offset of two TSF input
+ * Description: Calculate TSF offset of two TSF input
  *              Get TSF Offset from RxBCN's TSF and local TSF
  *
  * Parameters:
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index 171dd68cf5b2..6370d1039103 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -478,7 +478,7 @@ typedef struct __device_info {
     unsigned int                        cbTD;
 
     //
-    //  Variables to track resources for the Interript In Pipe
+    //  Variables to track resources for the Interrupt In Pipe
     //
     INT_BUFFER                  intBuf;
     BOOL                        fKillEventPollingThread;
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index 3aa895ec6507..28edf9e7efcb 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -748,7 +748,7 @@ RXbBulkInProcessData (
     if ((*pbyRSSI != 0) &&
         (pMgmt->pCurrBSS!=NULL)) {
         RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-        // Moniter if RSSI is too strong.
+        // Monitor if RSSI is too strong.
         pMgmt->pCurrBSS->byRSSIStatCnt++;
         pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
         pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
diff --git a/drivers/staging/vt6656/hostap.c b/drivers/staging/vt6656/hostap.c
index 682002a5b8d7..0a73d4060ee1 100644
--- a/drivers/staging/vt6656/hostap.c
+++ b/drivers/staging/vt6656/hostap.c
@@ -18,7 +18,7 @@
  *
  * File: hostap.c
  *
- * Purpose: handle hostap deamon ioctl input/out functions
+ * Purpose: handle hostap daemon ioctl input/out functions
  *
  * Author: Lyndon Chen
  *
@@ -48,7 +48,7 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 /*
  * Description:
- *      register net_device (AP) for hostap deamon
+ *      register net_device (AP) for hostap daemon
  *
  * Parameters:
  *  In:
@@ -176,7 +176,7 @@ int vt6656_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked)
 
 /*
  * Description:
- *      remove station function supported for hostap deamon
+ *      remove station function supported for hostap daemon
  *
  * Parameters:
  *  In:
@@ -204,7 +204,7 @@ static int hostap_remove_sta(PSDevice pDevice,
 
 /*
  * Description:
- *      add a station from hostap deamon
+ *      add a station from hostap daemon
  *
  * Parameters:
  *  In:
@@ -439,9 +439,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		return -EINVAL;
 	}
 
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
 		if (param->u.crypt.idx >= MAX_GROUP_KEY)
 			return -EINVAL;
         iNodeIndex = 0;
@@ -663,9 +661,7 @@ static int hostap_get_encryption(PSDevice pDevice,
 
 	param->u.crypt.err = 0;
 
-	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
-	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
-	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+	if (is_broadcast_ether_addr(param->sta_addr)) {
         iNodeIndex = 0;
 	} else {
 	    if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &iNodeIndex) == FALSE) {
@@ -686,7 +682,7 @@ static int hostap_get_encryption(PSDevice pDevice,
 
 /*
  * Description:
- *      vt6656_hostap_ioctl main function supported for hostap deamon.
+ *      vt6656_hostap_ioctl main function supported for hostap daemon.
  *
  * Parameters:
  *  In:
@@ -732,8 +728,8 @@ int vt6656_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 		break;
 	case VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR:
 	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR \n");
-		return -EOPNOTSUPP;
-		break;
+		ret = -EOPNOTSUPP;
+		goto out;
 	case VIAWGET_HOSTAPD_FLUSH:
 	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_FLUSH \n");
         spin_lock_irq(&pDevice->lock);
@@ -777,13 +773,13 @@ int vt6656_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 
 	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
 	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS \n");
-	    return -EOPNOTSUPP;
-
+	    ret = -EOPNOTSUPP;
+	    goto out;
 	default:
 	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6656_hostap_ioctl: unknown cmd=%d\n",
 		       (int)param->cmd);
-		return -EOPNOTSUPP;
-		break;
+		ret = -EOPNOTSUPP;
+		goto out;
 	}
 
 
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index eba4b5061cf7..bba31caae036 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -149,7 +149,7 @@ void INTnsProcessData(PSDevice pDevice)
 					pMgmt->sNodeDBTable[0].bRxPSPoll =
 						FALSE;
 				} else if (pMgmt->byDTIMCount == 0) {
-					/* check if mutltcast tx bufferring */
+					/* check if multicast tx buffering */
 					pMgmt->byDTIMCount =
 						pMgmt->byDTIMPeriod-1;
 					pMgmt->sNodeDBTable[0].bRxPSPoll = TRUE;
diff --git a/drivers/staging/vt6656/ioctl.c b/drivers/staging/vt6656/ioctl.c
index 5b9a84f95185..b6af5f691128 100644
--- a/drivers/staging/vt6656/ioctl.c
+++ b/drivers/staging/vt6656/ioctl.c
@@ -526,11 +526,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			pMgmt->abyIBSSSuppRates[3] |= BIT7;
 		}
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %x %x %x %x\n",
-			pMgmt->abyIBSSSuppRates[2],
-			pMgmt->abyIBSSSuppRates[3],
-			pMgmt->abyIBSSSuppRates[4],
-			pMgmt->abyIBSSSuppRates[5]);
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %*ph\n",
+			4, pMgmt->abyIBSSSuppRates + 2);
 
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
@@ -620,7 +617,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EFAULT;
 			break;
 		}
-		/* for some AP maybe good authenticate */
+		/* for some AP's maybe a good authentication */
 		if (wpa_Result.key_mgmt == 0x20)
 			pMgmt->Cisco_cckm = 1;
 		else
@@ -644,7 +641,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Private command not support..\n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Private command not supported..\n");
 	}
 
 	return result;
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index 8b9894b1146a..8f198749ca51 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -674,7 +674,7 @@ int iwctl_giwaplist(struct net_device *dev, struct iw_request_info *info,
 			jj++;
 		}
 
-		wrq->flags = 1; // Should be define'd
+		wrq->flags = 1; // Should be defined
 		wrq->length = jj;
 		memcpy(extra, sock, sizeof(struct sockaddr) * jj);
 		memcpy(extra + sizeof(struct sockaddr) * jj, qual, sizeof(struct iw_quality) * jj);
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index ee62a06a75f4..a61fcb9591aa 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -403,7 +403,7 @@ BOOL KeybRemoveKey(
     BOOL    bReturnValue = FALSE;
 
     if (is_broadcast_ether_addr(pbyBSSID)) {
-        // dealte all key
+        // delete all keys
         if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
             for (i=0;i<MAX_KEY_TABLE;i++) {
                 pTable->KeyTable[i].PairwiseKey.bKeyValid = FALSE;
@@ -618,7 +618,7 @@ BOOL KeybGetTransmitKey(PSKeyManagement pTable, PBYTE pbyBSSID, DWORD dwKeyType,
 
 
 /*
- * Description: Check Pairewise Key
+ * Description: Check Pairwise Key
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index d536756549e6..ad422dea702b 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -29,14 +29,14 @@
  *   vt6656_probe - module initial (insmod) driver entry
  *   device_remove1 - module remove entry
  *   device_open - allocate dma/descripter resource & initial mac/bbp function
- *   device_xmit - asynchrous data tx function
+ *   device_xmit - asynchronous data tx function
  *   device_set_multi - set mac filter
  *   device_ioctl - ioctl entry
- *   device_close - shutdown mac/bbp & free dma/descripter resource
+ *   device_close - shutdown mac/bbp & free dma/descriptor resource
  *   device_alloc_frag_buf - rx fragement pre-allocated function
  *   device_free_tx_bufs - free tx buffer function
  *   device_dma0_tx_80211- tx 802.11 frame via dma0
- *   device_dma0_xmit- tx PS bufferred frame via dma0
+ *   device_dma0_xmit- tx PS buffered frame via dma0
  *   device_init_registers- initial MAC & BBP & RF internal registers.
  *   device_init_rings- initial tx/rx ring buffer
  *   device_init_defrag_cb- initial & allocate de-fragement buffer.
@@ -316,7 +316,7 @@ static void device_init_diversity_timer(PSDevice pDevice)
 
 
 //
-// Initialiation of MAC & BBP registers
+// Initialization of MAC & BBP registers
 //
 
 static BOOL device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
@@ -639,7 +639,7 @@ static BOOL device_release_WPADEV(PSDevice pDevice)
   viawget_wpa_header *wpahdr;
   int ii=0;
  // wait_queue_head_t	Set_wait;
-  //send device close to wpa_supplicnat layer
+  //send device close to wpa_supplicant layer
     if (pDevice->bWPADEVUp==TRUE) {
                  wpahdr = (viawget_wpa_header *)pDevice->skb->data;
                  wpahdr->type = VIAWGET_DEVICECLOSE_MSG;
@@ -1010,7 +1010,7 @@ static int  device_open(struct net_device *dev) {
     }
 
     if (device_init_defrag_cb(pDevice)== FALSE) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Initial defragement cb fail \n");
+        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Initial defragment cb fail \n");
         goto free_rx_tx;
     }
 
@@ -1296,7 +1296,7 @@ static inline u32 ether_crc(int length, unsigned char *data)
     return crc;
 }
 
-//find out  the start  position of str2 from str1
+//find out the start  position of str2 from str1
 static unsigned char *kstrstr(const unsigned char *str1,
 			      const unsigned char *str2) {
   int str1_len = strlen(str1);
@@ -1345,7 +1345,7 @@ static int Config_FileGetParameter(unsigned char *string,
        }
 
    memset(buf2,0,100);
-   memcpy(buf2,start_p,end_p-start_p);    //get the tartget line
+   memcpy(buf2,start_p,end_p-start_p);    //get the target line
    buf2[end_p-start_p]='\0';
 
    //find value
@@ -1396,7 +1396,7 @@ static unsigned char *Config_FileOperation(PSDevice pDevice)
 	  }
 
      if(!(filp->f_op) || !(filp->f_op->read) ||!(filp->f_op->write)) {
-           printk("file %s cann't readable or writable?\n",config_path);
+           printk("file %s is not read or writeable?\n",config_path);
 	  result = -1;
 	  goto error1;
      	}
@@ -1969,7 +1969,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 
 	default:
 		rc = -EOPNOTSUPP;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Ioctl command not support..%x\n", cmd);
+        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Ioctl command not supported..%x\n", cmd);
 
 
     }
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index b3136773b5da..ab3a55462056 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -19,7 +19,7 @@
  *
  * File: power.c
  *
- * Purpose: Handles 802.11 power management  functions
+ * Purpose: Handles 802.11 power management functions
  *
  * Author: Lyndon Chen
  *
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index 3fd0478a9a54..593cdc713b0e 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -26,7 +26,7 @@
  * Date: Feb. 19, 2004
  *
  * Functions:
- *      IFRFbWriteEmbeded      - Embeded write RF register via MAC
+ *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
  *
  * Revision History:
  *
@@ -711,7 +711,7 @@ const BYTE RFaby11aChannelIndex[200] = {
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Write to IF/RF, by embeded programming
+ * Description: Write to IF/RF, by embedded programming
  *
  * Parameters:
  *  In:
@@ -722,7 +722,7 @@ const BYTE RFaby11aChannelIndex[200] = {
  * Return Value: TRUE if succeeded; FALSE if failed.
  *
  */
-BOOL IFRFbWriteEmbeded (PSDevice pDevice, DWORD dwData)
+BOOL IFRFbWriteEmbedded (PSDevice pDevice, DWORD dwData)
 {
     BYTE        pbyData[4];
 
@@ -828,23 +828,23 @@ BOOL        bResult = TRUE;
         case RF_AL2230 :
             if (pDevice->byCurPwr >= AL2230_PWR_IDX_LEN)
                 return FALSE;
-            bResult &= IFRFbWriteEmbeded(pDevice, dwAL2230PowerTable[pDevice->byCurPwr]);
+            bResult &= IFRFbWriteEmbedded(pDevice, dwAL2230PowerTable[pDevice->byCurPwr]);
             if (uRATE <= RATE_11M)
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             else
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             break;
 
         case RF_AL2230S :
             if (pDevice->byCurPwr >= AL2230_PWR_IDX_LEN)
                 return FALSE;
-            bResult &= IFRFbWriteEmbeded(pDevice, dwAL2230PowerTable[pDevice->byCurPwr]);
+            bResult &= IFRFbWriteEmbedded(pDevice, dwAL2230PowerTable[pDevice->byCurPwr]);
             if (uRATE <= RATE_11M) {
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             }else {
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
             }
             break;
 
@@ -854,10 +854,10 @@ BOOL        bResult = TRUE;
                 DWORD       dwMax7230Pwr;
 
                 if (uRATE <= RATE_11M) { //RobertYu:20060426, for better 11b mask
-                    bResult &= IFRFbWriteEmbeded(pDevice, 0x111BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
+                    bResult &= IFRFbWriteEmbedded(pDevice, 0x111BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
                 }
                 else {
-                    bResult &= IFRFbWriteEmbeded(pDevice, 0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
+                    bResult &= IFRFbWriteEmbedded(pDevice, 0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW);
                 }
 
                 if (pDevice->byCurPwr > AL7230_PWR_IDX_LEN) return FALSE;
@@ -866,7 +866,7 @@ BOOL        bResult = TRUE;
                 dwMax7230Pwr = 0x080C0B00 | ( (pDevice->byCurPwr) << 12 ) |
                                  (BY_AL7230_REG_LEN << 3 )  | IFREGCTL_REGW;
 
-                bResult &= IFRFbWriteEmbeded(pDevice, dwMax7230Pwr);
+                bResult &= IFRFbWriteEmbedded(pDevice, dwMax7230Pwr);
                 break;
             }
             break;
@@ -879,7 +879,7 @@ BOOL        bResult = TRUE;
                 return FALSE;
             dwVT3226Pwr = ((0x3F-pDevice->byCurPwr) << 20 ) | ( 0x17 << 8 ) /* Reg7 */ |
                            (BY_VT3226_REG_LEN << 3 )  | IFREGCTL_REGW;
-            bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226Pwr);
+            bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226Pwr);
             break;
         }
 
@@ -894,27 +894,27 @@ BOOL        bResult = TRUE;
 
                 dwVT3226Pwr = ((0x3F-pDevice->byCurPwr) << 20 ) | ( 0xE07 << 8 ) /* Reg7 */ |   //RobertYu:20060420, TWIF 1.10
                                (BY_VT3226_REG_LEN << 3 )  | IFREGCTL_REGW;
-                bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226Pwr);
+                bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226Pwr);
 
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x03C6A200+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x03C6A200+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
                 if (pDevice->sMgmtObj.eScanState != WMAC_NO_SCANNING) {
                     // scanning, the channel number is pDevice->uScanChannel
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"@@@@ RFbRawSetPower> 11B mode uCurrChannel[%d]\n", pDevice->sMgmtObj.uScanChannel);
-                    bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226D0LoCurrentTable[pDevice->sMgmtObj.uScanChannel-1]); //RobertYu:20060420, sometimes didn't change channel just set power with different rate
+                    bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226D0LoCurrentTable[pDevice->sMgmtObj.uScanChannel-1]); //RobertYu:20060420, sometimes didn't change channel just set power with different rate
                 } else {
                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"@@@@ RFbRawSetPower> 11B mode uCurrChannel[%d]\n", pDevice->sMgmtObj.uCurrChannel);
-                    bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226D0LoCurrentTable[pDevice->sMgmtObj.uCurrChannel-1]); //RobertYu:20060420, sometimes didn't change channel just set power with different rate
+                    bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226D0LoCurrentTable[pDevice->sMgmtObj.uCurrChannel-1]); //RobertYu:20060420, sometimes didn't change channel just set power with different rate
                 }
 
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x015C0800+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060420, ok now, new switching power (mini-pci can have bigger power consumption)
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x015C0800+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060420, ok now, new switching power (mini-pci can have bigger power consumption)
             } else {
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"@@@@ RFbRawSetPower> 11G mode\n");
                 dwVT3226Pwr = ((0x3F-pDevice->byCurPwr) << 20 ) | ( 0x7 << 8 ) /* Reg7 */ |   //RobertYu:20060420, TWIF 1.10
                                (BY_VT3226_REG_LEN << 3 )  | IFREGCTL_REGW;
-                bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226Pwr);
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x00C6A200+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060327
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x016BC600+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060111
-                bResult &= IFRFbWriteEmbeded(pDevice, 0x00900800+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060111
+                bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226Pwr);
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x00C6A200+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060327
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x016BC600+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060111
+                bResult &= IFRFbWriteEmbedded(pDevice, 0x00900800+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060111
             }
             break;
         }
@@ -929,7 +929,7 @@ BOOL        bResult = TRUE;
 
             dwVT3342Pwr =  ((0x3F-pDevice->byCurPwr) << 20 ) | ( 0x27 << 8 ) /* Reg7 */ |
                             (BY_VT3342_REG_LEN << 3 )  | IFREGCTL_REGW;
-            bResult &= IFRFbWriteEmbeded(pDevice, dwVT3342Pwr);
+            bResult &= IFRFbWriteEmbedded(pDevice, dwVT3342Pwr);
             break;
         }
 
@@ -1048,7 +1048,7 @@ BYTE    abyArray[256];
                     wLength1,
                     abyArray
                     );
-    //Channle Table 0
+    //Channel Table 0
     wValue = 0;
     while ( wLength2 > 0 ) {
 
@@ -1106,7 +1106,7 @@ BYTE    abyArray[256];
                     wLength1,
                     abyArray);
 
-        //Channle Table 0
+        //Channel Table 0
         wValue = 0;
         while ( wLength2 > 0 ) {
 
@@ -1141,9 +1141,9 @@ BOOL s_bVT3226D0_11bLoCurrentAdjust(
 
     bResult = TRUE;
     if( b11bMode )
-        bResult &= IFRFbWriteEmbeded(pDevice, dwVT3226D0LoCurrentTable[byChannel-1]);
+        bResult &= IFRFbWriteEmbedded(pDevice, dwVT3226D0LoCurrentTable[byChannel-1]);
     else
-        bResult &= IFRFbWriteEmbeded(pDevice, 0x016BC600+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060412
+        bResult &= IFRFbWriteEmbedded(pDevice, 0x016BC600+(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW); //RobertYu:20060412
 
     return bResult;
 }
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index f5ba8fd7f816..72eb27ac436b 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -63,7 +63,7 @@
 extern const BYTE RFaby11aChannelIndex[200];
 /*---------------------  Export Functions  --------------------------*/
 
-BOOL IFRFbWriteEmbeded(PSDevice pDevice, DWORD dwData);
+BOOL IFRFbWriteEmbedded(PSDevice pDevice, DWORD dwData);
 BOOL RFbSetPower(PSDevice pDevice, unsigned int uRATE, unsigned int uCH);
 
 BOOL RFbRawSetPower(
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index b6e04e7b629b..339083879883 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -841,7 +841,7 @@ s_uFillDataHead (
 	if ((uDMAIdx == TYPE_ATIMDMA) || (uDMAIdx == TYPE_BEACONDMA)) {
 		PSTxDataHead_ab pBuf = (PSTxDataHead_ab) pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
@@ -858,10 +858,10 @@ s_uFillDataHead (
             if (byFBOption == AUTO_FB_NONE) {
                 PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
                 //Get SignalField,ServiceField,Length
-                BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+                BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                     (PWORD)&(pBuf->wTransmitLength_a), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
                 );
-                BBvCaculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+                BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                     (PWORD)&(pBuf->wTransmitLength_b), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
                 );
                 //Get Duration and TimeStamp
@@ -881,10 +881,10 @@ s_uFillDataHead (
                 // Auto Fallback
                 PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
                 //Get SignalField,ServiceField,Length
-                BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+                BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                     (PWORD)&(pBuf->wTransmitLength_a), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
                 );
-                BBvCaculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+                BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                     (PWORD)&(pBuf->wTransmitLength_b), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
                 );
                 //Get Duration and TimeStamp
@@ -907,7 +907,7 @@ s_uFillDataHead (
             // Auto Fallback
             PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
@@ -924,7 +924,7 @@ s_uFillDataHead (
         } else {
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
@@ -942,7 +942,7 @@ s_uFillDataHead (
     else if (byPktType == PK_TYPE_11B) {
             PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
+            BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
                 (PWORD)&(pBuf->wTransmitLength), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             //Get Duration and TimeStampOff
@@ -987,17 +987,17 @@ s_vFillRTSHead (
         uRTSFrameLen -= 4;
     }
 
-    // Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
+    // Note: So far RTSHead doesn't appear in ATIM & Beacom DMA, so we don't need to take them into account.
     //       Otherwise, we need to modified codes for them.
     if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
         if (byFBOption == AUTO_FB_NONE) {
             PSRTS_g pBuf = (PSRTS_g)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
@@ -1035,11 +1035,11 @@ s_vFillRTSHead (
         else {
            PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_a), (PBYTE)&(pBuf->bySignalField_a)
             );
             pBuf->wTransmitLength_a = cpu_to_le16(wLen);
@@ -1084,7 +1084,7 @@ s_vFillRTSHead (
         if (byFBOption == AUTO_FB_NONE) {
             PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -1119,7 +1119,7 @@ s_vFillRTSHead (
         else {
             PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
+            BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
             );
             pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -1155,7 +1155,7 @@ s_vFillRTSHead (
     else if (byPktType == PK_TYPE_11B) {
         PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
         //Get SignalField,ServiceField,Length
-        BBvCaculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+        BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
             (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField), (PBYTE)&(pBuf->bySignalField)
         );
         pBuf->wTransmitLength = cpu_to_le16(wLen);
@@ -1221,7 +1221,7 @@ s_vFillCTSHead (
             // Auto Fall back
             PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
@@ -1246,7 +1246,7 @@ s_vFillCTSHead (
         } else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
             PSCTS pBuf = (PSCTS)pvCTS;
             //Get SignalField,ServiceField,Length
-            BBvCaculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
+            BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
                 (PWORD)&(wLen), (PBYTE)&(pBuf->byServiceField_b), (PBYTE)&(pBuf->bySignalField_b)
             );
             pBuf->wTransmitLength_b = cpu_to_le16(wLen);
@@ -1827,7 +1827,7 @@ s_bPacketToWirelessUsb(
  *
  * Parameters:
  *  In:
- *      pDevice         - Pointer to adpater
+ *      pDevice         - Pointer to adapter
  *      dwTxBufferAddr  - Transmit Buffer
  *      pPacket         - Packet from upper layer
  *      cbPacketSize    - Transmit Data Length
@@ -2198,7 +2198,7 @@ CMD_STATUS csMgmt_xmit(
 
     if (bIsPSPOLL) {
         // The MAC will automatically replace the Duration-field of MAC header by Duration-field
-        // of  FIFO control header.
+        // of FIFO control header.
         // This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
         // in the same place of other packet's Duration-field).
         // And it will cause Cisco-AP to issue Disassociation-packet
@@ -2272,7 +2272,7 @@ csBeacon_xmit(
         wCurrentRate = RATE_6M;
         pTxDataHead = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize);
         //Get SignalField,ServiceField,Length
-        BBvCaculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11A,
+        BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11A,
             (PWORD)&(pTxDataHead->wTransmitLength), (PBYTE)&(pTxDataHead->byServiceField), (PBYTE)&(pTxDataHead->bySignalField)
         );
         //Get Duration and TimeStampOff
@@ -2285,7 +2285,7 @@ csBeacon_xmit(
         pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
         pTxDataHead = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize);
         //Get SignalField,ServiceField,Length
-        BBvCaculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11B,
+        BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, PK_TYPE_11B,
             (PWORD)&(pTxDataHead->wTransmitLength), (PBYTE)&(pTxDataHead->byServiceField), (PBYTE)&(pTxDataHead->bySignalField)
         );
         //Get Duration and TimeStampOff
@@ -2473,7 +2473,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb) {
         cbMacHdLen = WLAN_HDR_ADDR3_LEN;
     }
 
-    // hostapd deamon ext support rate patch
+    // hostapd daemon ext support rate patch
     if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
 
         if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0) {
@@ -2591,7 +2591,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb) {
     pMACHeader->wFrameCtl &= cpu_to_le16(0xfffc);
     memcpy(pbyPayloadHead, (skb->data + cbMacHdLen), cbFrameBodySize);
 
-    // replace support rate, patch for hostapd deamon( only support 11M)
+    // replace support rate, patch for hostapd daemon( only support 11M)
     if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
         if (cbExtSuppRate != 0) {
             if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
@@ -2770,7 +2770,7 @@ int nsDMA_tx_packet(PSDevice pDevice, unsigned int uDMAIdx, struct sk_buff *skb)
                 pMgmt->abyPSTxMap[0] |= byMask[0];
                 return 0;
             }
-            // muticast/broadcast data rate
+            // multicast/broadcast data rate
 
             if (pDevice->byBBType != BB_TYPE_11A)
                 pDevice->wCurrentRate = RATE_2M;
@@ -2855,7 +2855,7 @@ int nsDMA_tx_packet(PSDevice pDevice, unsigned int uDMAIdx, struct sk_buff *skb)
 			  }
 			  PRINT_K("Authentication completed!!\n");
                         }
-		    else if((Key_info & BIT3) && (Descriptor_type==2) &&  //RSN pairse-key challenge
+		    else if((Key_info & BIT3) && (Descriptor_type==2) &&  //RSN pairwise-key challenge
 			       (Key_info & BIT8) && (Key_info & BIT9)) {
 			  pDevice->fWPA_Authened = TRUE;
                             PRINT_K("WPA2 Authentication completed!!\n");
diff --git a/drivers/staging/vt6656/tcrc.c b/drivers/staging/vt6656/tcrc.c
index e25021e850a0..2237eeb5ec5b 100644
--- a/drivers/staging/vt6656/tcrc.c
+++ b/drivers/staging/vt6656/tcrc.c
@@ -18,7 +18,7 @@
  *
  * File: tcrc.c
  *
- * Purpose: Implement functions to caculate CRC
+ * Purpose: Implement functions to calculate CRC
  *
  * Author: Tevin Chen
  *
diff --git a/drivers/staging/vt6656/tcrc.h b/drivers/staging/vt6656/tcrc.h
index 4dfd01e477a4..dc54bd8fc4fc 100644
--- a/drivers/staging/vt6656/tcrc.h
+++ b/drivers/staging/vt6656/tcrc.h
@@ -18,7 +18,7 @@
  *
  * File: tcrc.h
  *
- * Purpose: Implement functions to caculate CRC
+ * Purpose: Implement functions to calculate CRC
  *
  * Author: Tevin Chen
  *
diff --git a/drivers/staging/vt6656/tether.c b/drivers/staging/vt6656/tether.c
index 4f368f174b21..083b2153a271 100644
--- a/drivers/staging/vt6656/tether.c
+++ b/drivers/staging/vt6656/tether.c
@@ -25,7 +25,7 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      ETHbyGetHashIndexByCrc32 - Caculate multicast hash value by CRC32
+ *      ETHbyGetHashIndexByCrc32 - Calculate multicast hash value by CRC32
  *      ETHbIsBufferCrc32Ok - Check CRC value of the buffer if Ok or not
  *
  * Revision History:
@@ -50,7 +50,7 @@
 
 
 /*
- * Description: Caculate multicast hash value by CRC32
+ * Description: Calculate multicast hash value by CRC32
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6656/tkip.c b/drivers/staging/vt6656/tkip.c
index 0715636cb9cb..003123e550f6 100644
--- a/drivers/staging/vt6656/tkip.c
+++ b/drivers/staging/vt6656/tkip.c
@@ -168,7 +168,7 @@ static unsigned int rotr1(unsigned int a)
 
 
 /*
- * Description: Caculate RC4Key fom TK, TA, and TSC
+ * Description: Calculate RC4Key fom TK, TA, and TSC
  *
  * Parameters:
  *  In:
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 9d2caa819f47..586fbe1627f7 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -263,7 +263,7 @@ s_vProbeChannel(
  *
  *
  * Return Value:
- *    A ptr to Tx frame or NULL on allocation failue
+ *    A ptr to Tx frame or NULL on allocation failure
  *
 -*/
 
@@ -751,7 +751,7 @@ void vRunCommand(void *hDeviceContext)
                       pDevice->nTxDataTimeCout = 0;
 		 }
 		 else {
-		   // printk("mike:-->First time triger TimerTxData InSleep\n");
+		   // printk("mike:-->First time trigger TimerTxData InSleep\n");
 		 }
 		pDevice->IsTxDataTrigger = TRUE;
                 add_timer(&pDevice->sTimerTxData);
@@ -794,7 +794,7 @@ void vRunCommand(void *hDeviceContext)
 			DBG_PRT(MSG_LEVEL_DEBUG,
 				KERN_INFO "vMgrCreateOwnIBSS fail!\n");
                 }
-                // alway turn off unicast bit
+                // always turn off unicast bit
                 MACvRegBitsOff(pDevice, MAC_REG_RCR, RCR_UNICAST);
                 pDevice->byRxMode &= ~RCR_UNICAST;
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode );
@@ -946,7 +946,7 @@ void vRunCommand(void *hDeviceContext)
                   pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
                   pItemSSID->len = 0;
                   memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
-                //clear dessire SSID
+                //clear desired SSID
                 pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
                 pItemSSID->len = 0;
                 memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
diff --git a/drivers/staging/vt6656/wctl.c b/drivers/staging/vt6656/wctl.c
index c231ae7176f5..9249263b2da8 100644
--- a/drivers/staging/vt6656/wctl.c
+++ b/drivers/staging/vt6656/wctl.c
@@ -89,7 +89,7 @@ BOOL WCTLbIsDuplicate (PSCache pCache, PS802_11Header pMACHeader)
             ADD_ONE_WITH_WRAP_AROUND(uIndex, DUPLICATE_RX_CACHE_LENGTH);
         }
     }
-    /* Not fount in cache - insert */
+    /* Not found in cache - insert */
     pCacheEntry = &pCache->asCacheEntry[pCache->uInPtr];
     pCacheEntry->wFmSequence = pMACHeader->wSeqCtl;
     memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
diff --git a/drivers/staging/vt6656/wmgr.c b/drivers/staging/vt6656/wmgr.c
index f08e2d15c7b3..7db6a8d3508b 100644
--- a/drivers/staging/vt6656/wmgr.c
+++ b/drivers/staging/vt6656/wmgr.c
@@ -27,7 +27,7 @@
  *
  * Functions:
  *      nsMgrObjectInitial - Initialize Management Objet data structure
- *      vMgrObjectReset - Reset Management Objet data structure
+ *      vMgrObjectReset - Reset Management Object data structure
  *      vMgrAssocBeginSta - Start associate function
  *      vMgrReAssocBeginSta - Start reassociate function
  *      vMgrDisassocBeginSta - Start disassociate function
@@ -54,7 +54,7 @@
  *      bMgrPrepareBeaconToSend - Prepare Beacon frame
  *      s_vMgrLogStatus - Log 802.11 Status
  *      vMgrRxManagePacket - Rcv management frame dispatch function
- *      s_vMgrFormatTIM- Assember TIM field of beacon
+ *      s_vMgrFormatTIM- Assembler TIM field of beacon
  *      vMgrTimerInit- Initial 1-sec and command call back funtions
  *
  * Revision History:
@@ -2032,7 +2032,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==TRUE)
                 }
 
                 //
-                // Preamble may change dynamiclly
+                // Preamble may change dynamically
                 //
                 byOldPreambleType = pDevice->byPreambleType;
                 if (WLAN_GET_CAP_INFO_SHORTPREAMBLE(pBSSList->wCapInfo)) {
@@ -2044,7 +2044,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==TRUE)
                 if (pDevice->byPreambleType != byOldPreambleType)
                     CARDvSetRSPINF(pDevice, (BYTE)pDevice->byBBType);
             //
-            // Basic Rate Set may change dynamiclly
+            // Basic Rate Set may change dynamically
             //
             if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
                 uRateLen = WLAN_RATES_MAXLEN_11B;
@@ -2188,7 +2188,7 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==TRUE)
             // During dpc, already in spinlocked.
             if (BSSbIsSTAInNodeDB(pDevice, sFrame.pHdr->sA3.abyAddr2, &uNodeIndex)) {
 
-                // Update the STA, (Techically the Beacons of all the IBSS nodes
+                // Update the STA, (Technically the Beacons of all the IBSS nodes
 		        // should be identical, but that's not happening in practice.
                 pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
                                                         (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
@@ -2722,7 +2722,7 @@ void vMgrJoinBSSBegin(void *hDeviceContext, PCMD_STATUS pStatus)
             memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
             // Add current BSS to Candidate list
-            // This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
+            // This should only work for WPA2 BSS, and WPA2 BSS check must be done before.
             if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
 		BOOL bResult = bAdd_PMKID_Candidate((void *) pDevice,
 						    pMgmt->abyCurrBSSID,
@@ -3181,7 +3181,7 @@ s_vMgrFormatTIM(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -3353,7 +3353,7 @@ s_MgrMakeBeacon(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -3528,7 +3528,7 @@ s_MgrMakeAssocRequest(
     memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
     memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
-    // Set the capibility and listen interval
+    // Set the capability and listen interval
     *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
     *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
 
@@ -3749,7 +3749,7 @@ s_MgrMakeAssocRequest(
  *
  *
  * Return Value:
- *    A ptr to frame or NULL on allocation failue
+ *    A ptr to frame or NULL on allocation failure
  *
 -*/
 
@@ -3792,7 +3792,7 @@ s_MgrMakeReAssocRequest(
     memcpy( sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
     memcpy( sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 
-    /* Set the capibility and listen interval */
+    /* Set the capability and listen interval */
     *(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
     *(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
 
@@ -4004,7 +4004,7 @@ s_MgrMakeReAssocRequest(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
@@ -4077,7 +4077,7 @@ s_MgrMakeAssocResponse(
  *
  *
  * Return Value:
- *    PTR to frame; or NULL on allocation failue
+ *    PTR to frame; or NULL on allocation failure
  *
 -*/
 
diff --git a/drivers/staging/vt6656/wpa.c b/drivers/staging/vt6656/wpa.c
index b16d4ddc117b..f6429a26ae0f 100644
--- a/drivers/staging/vt6656/wpa.c
+++ b/drivers/staging/vt6656/wpa.c
@@ -231,7 +231,7 @@ WPA_ParseRSN(
  * Parameters:
  *  In:
  *      byCmd    - Search type
- *      byEncrypt- Encrcypt Type
+ *      byEncrypt- Encrypt Type
  *      pBSSList - BSS list
  *  Out:
  *      none
diff --git a/drivers/staging/vt6656/wpa2.c b/drivers/staging/vt6656/wpa2.c
index d4f3f7530ee4..c0926976627d 100644
--- a/drivers/staging/vt6656/wpa2.c
+++ b/drivers/staging/vt6656/wpa2.c
@@ -174,16 +174,16 @@ WPA2vParseRSN (
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_USE_GROUP;
                         bUseGK = TRUE;
                     } else if ( !memcmp(pbyOUI, abyOUIWEP40, 4)) {
-                        // Invialid CSS, continue to parsing
+                        // Invalid CSS, continue parsing
                     } else if ( !memcmp(pbyOUI, abyOUITKIP, 4)) {
                         if (pBSSNode->byCSSGK != WLAN_11i_CSS_CCMP)
                             pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_TKIP;
                         else
-                            ; // Invialid CSS, continue to parsing
+                            ; // Invalid CSS, continue parsing
                     } else if ( !memcmp(pbyOUI, abyOUICCMP, 4)) {
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_CCMP;
                     } else if ( !memcmp(pbyOUI, abyOUIWEP104, 4)) {
-                        // Invialid CSS, continue to parsing
+                        // Invalid CSS, continue parsing
                     } else {
                         // any vendor checks here
                         pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
diff --git a/drivers/staging/vt6656/wpactl.c b/drivers/staging/vt6656/wpactl.c
index 5435e8205b2c..3e65aa132011 100644
--- a/drivers/staging/vt6656/wpactl.c
+++ b/drivers/staging/vt6656/wpactl.c
@@ -74,7 +74,7 @@ static void wpadev_setup(struct net_device *dev)
 
 /*
  * Description:
- *      register netdev for wpa supplicant deamon
+ *      register netdev for wpa supplicant daemon
  *
  * Parameters:
  *  In:
@@ -154,7 +154,7 @@ static int wpa_release_wpadev(PSDevice pDevice)
 
 /*
  * Description:
- *      Set enable/disable dev for wpa supplicant deamon
+ *      Set enable/disable dev for wpa supplicant daemon
  *
  * Parameters:
  *  In:
@@ -326,7 +326,7 @@ int wpa_set_wpadev(PSDevice pDevice, int val)
 	if ((byKeyDecMode == KEY_CTL_TKIP) &&
 		(param->u.wpa_key.key_len != MAX_KEY_LEN)) {
 		// TKIP Key must be 256 bits
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return- TKIP Key must be 256 bits!\n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return - TKIP Key must be 256 bits!\n");
 		return -EINVAL;
     }
 	// Check AES key length
diff --git a/drivers/staging/winbond/Kconfig b/drivers/staging/winbond/Kconfig
index 132671d96d0d..a29f60836b77 100644
--- a/drivers/staging/winbond/Kconfig
+++ b/drivers/staging/winbond/Kconfig
@@ -1,6 +1,6 @@
 config W35UND
 	tristate "IS89C35 WLAN USB driver"
-	depends on MAC80211 && WLAN && USB && EXPERIMENTAL
+	depends on MAC80211 && WLAN && USB
 	default n
 	---help---
 	  This is highly experimental driver for Winbond WIFI card.
diff --git a/drivers/staging/winbond/localpara.h b/drivers/staging/winbond/localpara.h
index d7980575bed1..84effc47d79d 100644
--- a/drivers/staging/winbond/localpara.h
+++ b/drivers/staging/winbond/localpara.h
@@ -137,7 +137,7 @@ struct wb_local_para {
 	u8	iPowerSaveMode; /* 0 indicates on, 1 indicates off */
 	u8	ATIMmode;
 	u8	ExcludeUnencrypted;
-	/* Unit ime count for the decision to enter PS mode */
+	/* Unit time count for the decision to enter PS mode */
 	u16	CheckCountForPS;
 	u8	boHasTxActivity;/* tx activity has occurred */
 	u8	boMacPsValid;	/* Power save mode obtained from H/W is valid or not */
@@ -187,7 +187,7 @@ struct wb_local_para {
 	u8	reserved7[3];
 
 	struct	chan_info CurrentChan;	/* Current channel no. and channel band. It may be changed by scanning. */
-	u8	boHandover;		/* Roaming, Hnadover to other AP. */
+	u8	boHandover;		/* Roaming, Handover to other AP. */
 	u8	boCCAbusy;
 
 	u16	CWMax;			/* It may not be the real value that H/W used */
diff --git a/drivers/staging/winbond/mds.c b/drivers/staging/winbond/mds.c
index c9f0e8f856a0..1b8b8ace39e0 100644
--- a/drivers/staging/winbond/mds.c
+++ b/drivers/staging/winbond/mds.c
@@ -569,7 +569,7 @@ Mds_SendComplete(struct wbsoft_priv *adapter, struct T02_descriptor *pT02)
 	unsigned char	SendOK = true;
 	u8	RetryCount, TxRate;
 
-	if (pT02->T02_IgnoreResult) /* Don't care the result */
+	if (pT02->T02_IgnoreResult) /* Don't care about the result */
 		return;
 	if (pT02->T02_IsLastMpdu) {
 		/* TODO: DTO -- get the retry count and fragment count */
diff --git a/drivers/staging/winbond/mto.c b/drivers/staging/winbond/mto.c
index 1b52ebd4b011..560c0ab617d1 100644
--- a/drivers/staging/winbond/mto.c
+++ b/drivers/staging/winbond/mto.c
@@ -23,7 +23,7 @@
 #include "core.h"
 
 /* Declare SQ3 to rate and fragmentation threshold table */
-/* Declare fragmentation thresholds table */
+/* Declare fragmentation threshold table */
 #define MTO_MAX_FRAG_TH_LEVELS		5
 #define MTO_MAX_DATA_RATE_LEVELS	12
 
diff --git a/drivers/staging/winbond/phy_calibration.c b/drivers/staging/winbond/phy_calibration.c
index 77a3fff708c6..cabae3466704 100644
--- a/drivers/staging/winbond/phy_calibration.c
+++ b/drivers/staging/winbond/phy_calibration.c
@@ -399,7 +399,7 @@ void _rxadc_dc_offset_cancellation_winbond(struct hw_data *phw_data, u32 frequen
 	val |= MASK_ADC_DC_CAL_STR;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, val);
 
-	/* e. The result are shown in "adc_dc_cal_i[8:0] and adc_dc_cal_q[8:0]" */
+	/* e. The results are shown in "adc_dc_cal_i[8:0] and adc_dc_cal_q[8:0]" */
 #ifdef _DEBUG
 	hw_get_dxx_reg(phw_data, REG_OFFSET_READ, &val);
 	PHY_DEBUG(("[CAL]    REG_OFFSET_READ = 0x%08X\n", val));
@@ -720,7 +720,7 @@ u8 _tx_iq_calibration_loop_winbond(struct hw_data *phw_data,
 		for (capture_time = 0; capture_time < 10; capture_time++) {
 			/*
 			 * a. Set iqcal_mode[1:0] to 0x2 and set "calib_start" to 0x1 to
-			 *    enable "IQ alibration Mode II"
+			 *    enable "IQ calibration Mode II"
 			 */
 			reg_mode_ctrl &= ~(MASK_IQCAL_TONE_SEL|MASK_IQCAL_MODE);
 			reg_mode_ctrl &= ~MASK_IQCAL_MODE;
@@ -750,7 +750,7 @@ u8 _tx_iq_calibration_loop_winbond(struct hw_data *phw_data,
 
 			/*
 			 * d. Set iqcal_mode[1:0] to 0x3 and set "calib_start" to 0x1 to
-			 *    enable "IQ alibration Mode II"
+			 *    enable "IQ calibration Mode II"
 			 */
 			/* hw_get_dxx_reg(phw_data, REG_MODE_CTRL, &val); */
 			hw_get_dxx_reg(phw_data, REG_MODE_CTRL, &reg_mode_ctrl);
@@ -980,7 +980,7 @@ void _tx_iq_calibration_winbond(struct hw_data *phw_data)
 	phy_set_rf_data(phw_data, 0, (0<<24)|0xFDF1C0);
 	/* ; [BB-chip]: Calibration (6f).Send test pattern */
 	/* ; [BB-chip]: Calibration (6g). Search RXGCL optimal value */
-	/* ; [BB-chip]: Calibration (6h). Caculate TX-path IQ imbalance and setting TX path IQ compensation table */
+	/* ; [BB-chip]: Calibration (6h). Calculate TX-path IQ imbalance and setting TX path IQ compensation table */
 	/* phy_set_rf_data(phw_data, 3, (3<<24)|0x025586); */
 
 	msleep(30); /* 20060612.1.a 30ms delay. Add the follow 2 lines */
@@ -1373,7 +1373,7 @@ u8 _rx_iq_calibration_loop_winbond(struct hw_data *phw_data, u16 factor, u32 fre
 /***************************************************************/
 void _rx_iq_calibration_winbond(struct hw_data *phw_data, u32 frequency)
 {
-/* figo 20050523 marked this flag for can't compile for relesase */
+/* figo 20050523 marked this flag for can't compile for release */
 #ifdef _DEBUG
 	s32     rx_cal_reg[4];
 	u32     val;
@@ -1397,7 +1397,7 @@ void _rx_iq_calibration_winbond(struct hw_data *phw_data, u32 frequency)
 
 	/*  ; [BB-chip]: Calibration (7f). Send test pattern */
 	/*	; [BB-chip]: Calibration (7g). Search RXGCL optimal value */
-	/*	; [BB-chip]: Calibration (7h). Caculate RX-path IQ imbalance and setting RX path IQ compensation table */
+	/*	; [BB-chip]: Calibration (7h). Calculate RX-path IQ imbalance and setting RX path IQ compensation table */
 
 	result = _rx_iq_calibration_loop_winbond(phw_data, 12589, frequency);
 
@@ -1454,7 +1454,7 @@ void phy_calibration_winbond(struct hw_data *phw_data, u32 frequency)
 
 	_rxadc_dc_offset_cancellation_winbond(phw_data, frequency);
 	/* _txidac_dc_offset_cancellation_winbond(phw_data); */
-	/* _txqdac_dc_offset_cacellation_winbond(phw_data); */
+	/* _txqdac_dc_offset_cancellation_winbond(phw_data); */
 
 	_tx_iq_calibration_winbond(phw_data);
 	_rx_iq_calibration_winbond(phw_data, frequency);
diff --git a/drivers/staging/winbond/reg.c b/drivers/staging/winbond/reg.c
index 1b38d6d225c9..5ecf9a121e78 100644
--- a/drivers/staging/winbond/reg.c
+++ b/drivers/staging/winbond/reg.c
@@ -693,7 +693,7 @@ u32 w89rf242_rf_data[] = {
 	(0x0E << 24) | 0x5557DC, /* 1555F ; IBSC  (0x0E) -- IRLNA & IRLNB (PTAT & Const current)=01/01; FA5976B_1.3F */
 	(0x10 << 24) | 0x000C20, /* 00030 ; TMODA (0x10) -- LNA_gain_step=0011 ; LNA=15/16dB */
 	(0x11 << 24) | 0x0C0022, /* 03000 ; TMODB (0x11) -- Turn ON RX-Q path Test Switch; To improve IQ path group delay (FA5976A_1.3C) */
-	(0x12 << 24) | 0x000024  /* TMODC (0x12) -- Turn OFF Tempearure sensor */
+	(0x12 << 24) | 0x000024  /* TMODC (0x12) -- Turn OFF Temperature sensor */
 };
 
 u32 w89rf242_channel_data_24[][2] = {
diff --git a/drivers/staging/winbond/sme_api.h b/drivers/staging/winbond/sme_api.h
index 8f4596c9e9b3..652ae7085a5f 100644
--- a/drivers/staging/winbond/sme_api.h
+++ b/drivers/staging/winbond/sme_api.h
@@ -12,7 +12,6 @@
 #include "localpara.h"
 
 /****************** CONSTANT AND MACRO SECTION ******************************/
-#define _INLINE				__inline
 
 #define MEDIA_STATE_DISCONNECTED	0
 #define MEDIA_STATE_CONNECTED		1
@@ -26,17 +25,17 @@
 
 /* OID_802_11_BSSID */
 s8 sme_get_bssid(void *pcore_data, u8 *pbssid);
-s8 sme_get_desired_bssid(void *pcore_data, u8 *pbssid); /* Not use */
+s8 sme_get_desired_bssid(void *pcore_data, u8 *pbssid); /* Unused */
 s8 sme_set_desired_bssid(void *pcore_data, u8 *pbssid);
 
 /* OID_802_11_SSID */
 s8 sme_get_ssid(void *pcore_data, u8 *pssid, u8 *pssid_len);
-s8 sme_get_desired_ssid(void *pcore_data, u8 *pssid, u8 *pssid_len);/* Not use */
+s8 sme_get_desired_ssid(void *pcore_data, u8 *pssid, u8 *pssid_len);/* Unused */
 s8 sme_set_desired_ssid(void *pcore_data, u8 *pssid, u8 ssid_len);
 
 /* OID_802_11_INFRASTRUCTURE_MODE */
 s8 sme_get_bss_type(void *pcore_data, u8 *pbss_type);
-s8 sme_get_desired_bss_type(void *pcore_data, u8 *pbss_type); /* Not use */
+s8 sme_get_desired_bss_type(void *pcore_data, u8 *pbss_type); /* Unused */
 s8 sme_set_desired_bss_type(void *pcore_data, u8 bss_type);
 
 /* OID_802_11_FRAGMENTATION_THRESHOLD */
@@ -107,7 +106,7 @@ s8 sme_set_bssid_list_scan(void *pcore_data, void *pscan_para);
 s8 sme_set_reload_defaults(void *pcore_data, u8 reload_type);
 
 
-/*------------------------- none-standard ----------------------------------*/
+/*------------------------- non-standard ----------------------------------*/
 s8 sme_get_connect_status(void *pcore_data, u8 *pstatus);
 /*--------------------------------------------------------------------------*/
 
@@ -138,7 +137,7 @@ s8 sme_set_txrate_policy(void *pcore_data, u8 policy);
 s8 sme_get_txrate_policy(void *pcore_data, u8 *policy);
 s8 sme_get_cwmin_value(void *pcore_data, u8 *cwmin);
 s8 sme_get_cwmax_value(void *pcore_data, u16 *cwmax);
-s8 sme_get_ms_radio_mode(void *pcore_data, u8 * pMsRadioOff);
+s8 sme_get_ms_radio_mode(void *pcore_data, u8 *pMsRadioOff);
 s8 sme_set_ms_radio_mode(void *pcore_data, u8 boMsRadioOff);
 
 void sme_get_tx_power_level(void *pcore_data, u32 *TxPower);
diff --git a/drivers/staging/winbond/wb35reg.c b/drivers/staging/winbond/wb35reg.c
index da595f16f634..1bff7d1c9a77 100644
--- a/drivers/staging/winbond/wb35reg.c
+++ b/drivers/staging/winbond/wb35reg.c
@@ -217,7 +217,7 @@ unsigned char Wb35Reg_Write(struct hw_data *pHwData, u16 RegisterNo, u32 Registe
  * This command will be executed with a user defined value. When it completes,
  * this value is useful. For example, hal_set_current_channel will use it.
  * true  : read command process successfully
- * false : register not support
+ * false : register not supported
  */
 unsigned char Wb35Reg_WriteWithCallbackValue(struct hw_data *pHwData,
 						u16 RegisterNo,
@@ -631,7 +631,7 @@ unsigned char Wb35Reg_initial(struct hw_data *pHwData)
  *  CardComputeCrc --
  *
  *  Description:
- *    Runs the AUTODIN II CRC algorithm on buffer Buffer of length, Length.
+ *    Runs the AUTODIN II CRC algorithm on the buffers Buffer length.
  *
  *  Arguments:
  *    Buffer - the input buffer
diff --git a/drivers/staging/winbond/wb35tx.c b/drivers/staging/winbond/wb35tx.c
index 5df39d46cda4..30a77ccfe480 100644
--- a/drivers/staging/winbond/wb35tx.c
+++ b/drivers/staging/winbond/wb35tx.c
@@ -1,13 +1,13 @@
-//============================================================================
-//  Copyright (c) 1996-2002 Winbond Electronic Corporation
-//
-//  Module Name:
-//    Wb35Tx.c
-//
-//  Abstract:
-//    Processing the Tx message and put into down layer
-//
-//============================================================================
+/*
+ *  Copyright (c) 1996-2002 Winbond Electronic Corporation
+ *
+ *  Module Name:
+ *    Wb35Tx.c
+ *
+ *  Abstract:
+ *    Processing the Tx message and put into down layer
+ *
+ */
 #include <linux/usb.h>
 #include <linux/gfp.h>
 
@@ -15,7 +15,7 @@
 #include "mds_f.h"
 
 unsigned char
-Wb35Tx_get_tx_buffer(struct hw_data * pHwData, u8 **pBuffer)
+Wb35Tx_get_tx_buffer(struct hw_data *pHwData, u8 **pBuffer)
 {
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
@@ -25,28 +25,29 @@ Wb35Tx_get_tx_buffer(struct hw_data * pHwData, u8 **pBuffer)
 
 static void Wb35Tx(struct wbsoft_priv *adapter);
 
-static void Wb35Tx_complete(struct urb * pUrb)
+static void Wb35Tx_complete(struct urb *pUrb)
 {
 	struct wbsoft_priv *adapter = pUrb->context;
-	struct hw_data *	pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 	struct wb35_mds *pMds = &adapter->Mds;
 
 	printk("wb35: tx complete\n");
-	// Variable setting
+	/* Variable setting */
 	pWb35Tx->EP4vm_state = VM_COMPLETED;
-	pWb35Tx->EP4VM_status = pUrb->status; //Store the last result of Irp
-	pMds->TxOwner[ pWb35Tx->TxSendIndex ] = 0;// Set the owner. Free the owner bit always.
+	pWb35Tx->EP4VM_status = pUrb->status; /* Store the last result of Irp */
+	/* Set the owner. Free the owner bit always. */
+	pMds->TxOwner[pWb35Tx->TxSendIndex] = 0;
 	pWb35Tx->TxSendIndex++;
 	pWb35Tx->TxSendIndex %= MAX_USB_TX_BUFFER_NUMBER;
 
-	if (pHwData->SurpriseRemove) // Let WbWlanHalt to handle surprise remove
+	if (pHwData->SurpriseRemove) /* Let WbWlanHalt handle surprise remove */
 		goto error;
 
 	if (pWb35Tx->tx_halt)
 		goto error;
 
-	// The URB is completed, check the result
+	/* The URB is completed, check the result */
 	if (pWb35Tx->EP4VM_status != 0) {
 		printk("URB submission failed\n");
 		pWb35Tx->EP4vm_state = VM_STOP;
@@ -64,43 +65,42 @@ error:
 
 static void Wb35Tx(struct wbsoft_priv *adapter)
 {
-	struct hw_data *	pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 	u8		*pTxBufferAddress;
 	struct wb35_mds *pMds = &adapter->Mds;
-	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx4Urb;
-	int         	retv;
+	struct urb *pUrb = (struct urb *)pWb35Tx->Tx4Urb;
+	int		retv;
 	u32		SendIndex;
 
-
 	if (pHwData->SurpriseRemove)
 		goto cleanup;
 
 	if (pWb35Tx->tx_halt)
 		goto cleanup;
 
-	// Ownership checking
+	/* Ownership checking */
 	SendIndex = pWb35Tx->TxSendIndex;
-	if (!pMds->TxOwner[SendIndex]) //No more data need to be sent, return immediately
+	/* No more data need to be sent, return immediately */
+	if (!pMds->TxOwner[SendIndex])
 		goto cleanup;
 
 	pTxBufferAddress = pWb35Tx->TxBuffer[SendIndex];
-	//
-	// Issuing URB
-	//
+
+	/* Issuing URB */
 	usb_fill_bulk_urb(pUrb, pHwData->udev,
 			  usb_sndbulkpipe(pHwData->udev, 4),
-			  pTxBufferAddress, pMds->TxBufferSize[ SendIndex ],
+			  pTxBufferAddress, pMds->TxBufferSize[SendIndex],
 			  Wb35Tx_complete, adapter);
 
 	pWb35Tx->EP4vm_state = VM_RUNNING;
 	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
-	if (retv<0) {
+	if (retv < 0) {
 		printk("EP4 Tx Irp sending error\n");
 		goto cleanup;
 	}
 
-	// Check if driver needs issue Irp for EP2
+	/* Check if driver needs issue Irp for EP2 */
 	pWb35Tx->TxFillCount += pMds->TxCountInBuffer[SendIndex];
 	if (pWb35Tx->TxFillCount > 12)
 		Wb35Tx_EP2VM_start(adapter);
@@ -115,10 +115,10 @@ static void Wb35Tx(struct wbsoft_priv *adapter)
 
 void Wb35Tx_start(struct wbsoft_priv *adapter)
 {
-	struct hw_data * pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
-	// Allow only one thread to run into function
+	/* Allow only one thread to run into function */
 	if (atomic_inc_return(&pWb35Tx->TxFireCounter) == 1) {
 		pWb35Tx->EP4vm_state = VM_RUNNING;
 		Wb35Tx(adapter);
@@ -126,7 +126,7 @@ void Wb35Tx_start(struct wbsoft_priv *adapter)
 		atomic_dec(&pWb35Tx->TxFireCounter);
 }
 
-unsigned char Wb35Tx_initial(struct hw_data * pHwData)
+unsigned char Wb35Tx_initial(struct hw_data *pHwData)
 {
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
@@ -135,54 +135,50 @@ unsigned char Wb35Tx_initial(struct hw_data * pHwData)
 		return false;
 
 	pWb35Tx->Tx2Urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!pWb35Tx->Tx2Urb)
-	{
-		usb_free_urb( pWb35Tx->Tx4Urb );
+	if (!pWb35Tx->Tx2Urb) {
+		usb_free_urb(pWb35Tx->Tx4Urb);
 		return false;
 	}
 
 	return true;
 }
 
-//======================================================
-void Wb35Tx_stop(struct hw_data * pHwData)
+void Wb35Tx_stop(struct hw_data *pHwData)
 {
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
-	// Trying to canceling the Trp of EP2
+	/* Try to cancel the Trp of EP2 */
 	if (pWb35Tx->EP2vm_state == VM_RUNNING)
-		usb_unlink_urb( pWb35Tx->Tx2Urb ); // Only use unlink, let Wb35Tx_destrot to free them
+		/* Only use unlink, let Wb35Tx_destroy free them */
+		usb_unlink_urb(pWb35Tx->Tx2Urb);
 	pr_debug("EP2 Tx stop\n");
 
-	// Trying to canceling the Irp of EP4
+	/* Try to cancel the Irp of EP4 */
 	if (pWb35Tx->EP4vm_state == VM_RUNNING)
-		usb_unlink_urb( pWb35Tx->Tx4Urb ); // Only use unlink, let Wb35Tx_destrot to free them
+		/* Only use unlink, let Wb35Tx_destroy free them */
+		usb_unlink_urb(pWb35Tx->Tx4Urb);
 	pr_debug("EP4 Tx stop\n");
 }
 
-//======================================================
-void Wb35Tx_destroy(struct hw_data * pHwData)
+void Wb35Tx_destroy(struct hw_data *pHwData)
 {
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
-	// Wait for VM stop
+	/* Wait for VM stop */
 	do {
-		msleep(10);  // Delay for waiting function enter 940623.1.a
-	} while( (pWb35Tx->EP2vm_state != VM_STOP) && (pWb35Tx->EP4vm_state != VM_STOP) );
-	msleep(10);  // Delay for waiting function enter 940623.1.b
-
-	if (pWb35Tx->Tx4Urb)
-		usb_free_urb( pWb35Tx->Tx4Urb );
+		msleep(10); /* Delay for waiting function enter 940623.1.a */
+	} while ((pWb35Tx->EP2vm_state != VM_STOP) && (pWb35Tx->EP4vm_state != VM_STOP));
+	msleep(10); /* Delay for waiting function enter 940623.1.b */
 
-	if (pWb35Tx->Tx2Urb)
-		usb_free_urb( pWb35Tx->Tx2Urb );
+	usb_free_urb(pWb35Tx->Tx4Urb);
+	usb_free_urb(pWb35Tx->Tx2Urb);
 
 	pr_debug("Wb35Tx_destroy OK\n");
 }
 
 void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter, u32 TimeCount)
 {
-	struct hw_data * pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 	unsigned char Trigger = false;
 
@@ -199,46 +195,45 @@ void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter, u32 TimeCount)
 
 static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter);
 
-static void Wb35Tx_EP2VM_complete(struct urb * pUrb)
+static void Wb35Tx_EP2VM_complete(struct urb *pUrb)
 {
 	struct wbsoft_priv *adapter = pUrb->context;
-	struct hw_data *	pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct T02_descriptor	T02, TSTATUS;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
-	u32 *		pltmp = (u32 *)pWb35Tx->EP2_buf;
+	u32 *pltmp = (u32 *)pWb35Tx->EP2_buf;
 	u32		i;
 	u16		InterruptInLength;
 
-
-	// Variable setting
+	/* Variable setting */
 	pWb35Tx->EP2vm_state = VM_COMPLETED;
 	pWb35Tx->EP2VM_status = pUrb->status;
 
-	// For Linux 2.4. Interrupt will always trigger
-	if (pHwData->SurpriseRemove) // Let WbWlanHalt to handle surprise remove
+	/* For Linux 2.4. Interrupt will always trigger */
+	if (pHwData->SurpriseRemove) /* Let WbWlanHalt handle surprise remove */
 		goto error;
 
 	if (pWb35Tx->tx_halt)
 		goto error;
 
-	//The Urb is completed, check the result
+	/* The Urb is completed, check the result */
 	if (pWb35Tx->EP2VM_status != 0) {
 		printk("EP2 IoCompleteRoutine return error\n");
-		pWb35Tx->EP2vm_state= VM_STOP;
+		pWb35Tx->EP2vm_state = VM_STOP;
 		goto error;
 	}
 
-	// Update the Tx result
+	/* Update the Tx result */
 	InterruptInLength = pUrb->actual_length;
-	// Modify for minimum memory access and DWORD alignment.
-	T02.value = cpu_to_le32(pltmp[0]) >> 8; // [31:8] -> [24:0]
-	InterruptInLength -= 1;// 20051221.1.c Modify the follow for more stable
-	InterruptInLength >>= 2; // InterruptInLength/4
+	/* Modify for minimum memory access and DWORD alignment. */
+	T02.value = cpu_to_le32(pltmp[0]) >> 8; /* [31:8] -> [24:0] */
+	InterruptInLength -= 1; /* 20051221.1.c Modify the follow for more stable */
+	InterruptInLength >>= 2; /* InterruptInLength/4 */
 	for (i = 1; i <= InterruptInLength; i++) {
 		T02.value |= ((cpu_to_le32(pltmp[i]) & 0xff) << 24);
 
-		TSTATUS.value = T02.value;  //20061009 anson's endian
-		Mds_SendComplete( adapter, &TSTATUS );
+		TSTATUS.value = T02.value;  /* 20061009 anson's endian */
+		Mds_SendComplete(adapter, &TSTATUS);
 		T02.value = cpu_to_le32(pltmp[i]) >> 8;
 	}
 
@@ -250,10 +245,10 @@ error:
 
 static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter)
 {
-	struct hw_data *	pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
-	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx2Urb;
-	u32 *	pltmp = (u32 *)pWb35Tx->EP2_buf;
+	struct urb *pUrb = (struct urb *)pWb35Tx->Tx2Urb;
+	u32 *pltmp = (u32 *)pWb35Tx->EP2_buf;
 	int		retv;
 
 	if (pHwData->SurpriseRemove)
@@ -262,11 +257,10 @@ static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter)
 	if (pWb35Tx->tx_halt)
 		goto error;
 
-	//
-	// Issuing URB
-	//
-	usb_fill_int_urb( pUrb, pHwData->udev, usb_rcvintpipe(pHwData->udev,2),
-			  pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete, adapter, 32);
+	/* Issuing URB */
+	usb_fill_int_urb(pUrb, pHwData->udev, usb_rcvintpipe(pHwData->udev, 2),
+			 pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete,
+			 adapter, 32);
 
 	pWb35Tx->EP2vm_state = VM_RUNNING;
 	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
@@ -284,10 +278,10 @@ error:
 
 void Wb35Tx_EP2VM_start(struct wbsoft_priv *adapter)
 {
-	struct hw_data * pHwData = &adapter->sHwData;
+	struct hw_data *pHwData = &adapter->sHwData;
 	struct wb35_tx *pWb35Tx = &pHwData->Wb35Tx;
 
-	// Allow only one thread to run into function
+	/* Allow only one thread to run into function */
 	if (atomic_inc_return(&pWb35Tx->TxResultCount) == 1) {
 		pWb35Tx->EP2vm_state = VM_RUNNING;
 		Wb35Tx_EP2VM(adapter);
diff --git a/drivers/staging/winbond/wb35tx_s.h b/drivers/staging/winbond/wb35tx_s.h
index f70f43395591..715f87d6ac5b 100644
--- a/drivers/staging/winbond/wb35tx_s.h
+++ b/drivers/staging/winbond/wb35tx_s.h
@@ -3,45 +3,37 @@
 
 #include "mds_s.h"
 
-//====================================
-// IS89C35 Tx related definition
-//====================================
-#define TX_INTERFACE			0	// Interface 1
-#define TX_PIPE					3	// endpoint 4
-#define TX_INTERRUPT			1	// endpoint 2
-#define MAX_INTERRUPT_LENGTH	64	// It must be 64 for EP2 hardware
-
-
-
-//====================================
-// Internal variable for module
-//====================================
-
+/* IS89C35 Tx related definition */
+#define TX_INTERFACE		0	/* Interface 1 */
+#define TX_PIPE			3	/* Endpoint 4 */
+#define TX_INTERRUPT		1	/* Endpoint 2 */
+#define MAX_INTERRUPT_LENGTH	64	/* It must be 64 for EP2 hardware */
 
+/* Internal variable for module */
 struct wb35_tx {
-	// For Tx buffer
+	/* For Tx buffer */
 	u8	TxBuffer[ MAX_USB_TX_BUFFER_NUMBER ][ MAX_USB_TX_BUFFER ];
 
-	// For Interrupt pipe
+	/* For Interrupt pipe */
 	u8	EP2_buf[MAX_INTERRUPT_LENGTH];
 
-	atomic_t	TxResultCount;// For thread control of EP2 931130.4.m
-	atomic_t	TxFireCounter;// For thread control of EP4 931130.4.n
-	u32			ByteTransfer;
+	atomic_t	TxResultCount; /* For thread control of EP2 931130.4.m */
+	atomic_t	TxFireCounter; /* For thread control of EP4 931130.4.n */
+	u32		ByteTransfer;
 
-	u32	    TxSendIndex;// The next index of Mds array to be sent
-	u32	    EP2vm_state; // for EP2vm state
-	u32	    EP4vm_state; // for EP4vm state
-	u32	    tx_halt; // Stopping VM
+	u32	TxSendIndex; /* The next index of Mds array to be sent */
+	u32	EP2vm_state; /* for EP2vm state */
+	u32	EP4vm_state; /* for EP4vm state */
+	u32	tx_halt; /* Stopping VM */
 
-	struct urb *				Tx4Urb;
-	struct urb *				Tx2Urb;
+	struct urb	*Tx4Urb;
+	struct urb	*Tx2Urb;
 
 	int		EP2VM_status;
 	int		EP4VM_status;
 
-	u32	TxFillCount; // 20060928
-	u32	TxTimer; // 20060928 Add if sending packet not great than 13
+	u32	TxFillCount; /* 20060928 */
+	u32	TxTimer; /* 20060928 Add if sending packet is greater than 13 */
 };
 
 #endif
diff --git a/drivers/staging/wlags49_h2/README.ubuntu b/drivers/staging/wlags49_h2/README.ubuntu
index 5f1cfb8fd427..bfad7dc7725f 100644
--- a/drivers/staging/wlags49_h2/README.ubuntu
+++ b/drivers/staging/wlags49_h2/README.ubuntu
@@ -46,12 +46,12 @@ If you have a card using the HERMES II.5 chip you have to make
 changes to the Makefile and uncomment -DHERMES25. This will build
 driver wlags49_h25_cs.
 
-Note: You can detemine the type with command "pccardctrl info"
+Note: You can determine the type with command "pccardctrl info"
         MANIFID: 0156,0002 = HERMES - not supported by this driver
         MANIFID: 0156,0003 = HERMES II   (Wireless B)
         MANIFID: 0156,0004 = HERMES II.5 (Wireless B/G)
 
-After succesfull compile type command
+After successful compile type command
 
 sudo make install
 
@@ -81,7 +81,7 @@ The linux driver files (wl_xxxx.c) are changed in the following ways:
 - Addaptations of Andrey Borzenkov applied to 7.22 source
 - Alterations to avoid most HCF_ASSERTs
 -- Switching interrupts off and on in the HCF
--- Bugfixes, things that were aparently wrong like reporting link status
+-- Bugfixes, things that were apparently wrong like reporting link status
    change which checked a variable that was not changed in HCF anymore.
 -- Used on WEP but setting keys via SIOCSIWENCODEEXT was not supported
 -- Recovery actions added
@@ -93,7 +93,7 @@ have to "open" the device first to get a handle and after "close" no
 changed; the former ioctl functions are now called before "open" and
 after "close", which was not expected. One of the problems was enable/
 disable of interrupts in the HCF. Interrupt handling starts at "open"
-so if a former "ioctl" routinge is called before "open" or after "close"
+so if a former "ioctl" routine is called before "open" or after "close"
 then nothing should be done with interrupt switching in the HCF. Once
 this was solved most HCF_ASSERTS went away.
 
@@ -120,8 +120,8 @@ include the man page. Even though setting parameters on the module
 does not work anymore but it provides some information about all the
 settings.
 
-I have not have personal contact with Agere, but others have. Agere
-agreed to make their software available under the BSD licence.
+I have no personal contact with Agere, but others have. Agere
+agreed to make their software available under the BSD license.
 This driver is based on the 7.22 version.
 
 The following was mailed by Agere to Andrey Borzenkov about this:
diff --git a/drivers/staging/wlags49_h2/TODO b/drivers/staging/wlags49_h2/TODO
index 94032b6ac2b5..f1a45611b236 100644
--- a/drivers/staging/wlags49_h2/TODO
+++ b/drivers/staging/wlags49_h2/TODO
@@ -1,4 +1,4 @@
-First of all, the best thing would be that this driver becomes obsolte by
+First of all, the best thing would be that this driver becomes obsolete by
 adding support for Hermes II and Hermes II.5 cards to the existing orinoco
 driver. The orinoco driver currently only supports Hermes I based cards.
 Since this will not happen by magic and has not happened until now this
@@ -10,11 +10,11 @@ list.
 
 TODO:
 	- verify against a Hermes II.5 card
-	- verify with WPA encription (both with H2 and H2.5 cards)
+	- verify with WPA encryption (both with H2 and H2.5 cards)
 	- sometimes the card does not initialize correctly, retry mechanisms
-	  are build in to catch most cases but not all
+	  are built in to catch most cases but not all
 	- once the driver runs it is very stable, but I have the impression
-	  some the crittical sections take to long
+	  that some of the critical sections take some time.
 	- the driver is split into a Hermes II and a Hermes II.5 part, it
 	  would be nice to handle both with one module instead of two
 	- review by the wireless developer community
@@ -25,7 +25,7 @@ TODO:
 DONE:
 	- verified against a Hermes II card (Thomson Speedtouch 110 PCMCIA
 	  card)
-	- verified with WEP encription
+	- verified with WEP encryption
 
 Please send any patches or complaints about this driver to Greg
 Kroah-Hartman <greg@kroah.com> and Cc: Henk de Groot <pe1dnn@amsat.org>
diff --git a/drivers/staging/wlags49_h2/hcf.c b/drivers/staging/wlags49_h2/hcf.c
index 423544634ae5..4aac26d486f1 100644
--- a/drivers/staging/wlags49_h2/hcf.c
+++ b/drivers/staging/wlags49_h2/hcf.c
@@ -508,7 +508,7 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  *    - HCF_ACT_INT_FORCE_ON enable interrupt generation by WaveLAN NIC
  *    - HCF_ACT_INT_OFF      disable interrupt generation by WaveLAN NIC
  *    - HCF_ACT_INT_ON       compensate 1 HCF_ACT_INT_OFF, enable interrupt generation if balance reached
- *    - HCF_ACT_PRS_SCAN     Hermes Probe Respons Scan (F102) command
+ *    - HCF_ACT_PRS_SCAN     Hermes Probe Response Scan (F102) command
  *    - HCF_ACT_RX_ACK       acknowledge non-DMA receiver to Hermes
  *    - HCF_ACT_SCAN         Hermes Inquire Scan (F101) command (non-WARP only)
  *    - HCF_ACT_SLEEP        DDS Sleep request
@@ -571,7 +571,7 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  * The F/W is wokenup by the HCF when the NIC Interrupts mode are disabled, i.e. at the first HCF_ACT_INT_OFF
  * after going into sleep.
  *
- * The following Miscellanuous actions are defined:
+ * The following Miscellaneous actions are defined:
  *
  * o HCF_ACT_RX_ACK: Receiver Acknowledgement (non-DMA, non-USB mode only)
  * Acking the receiver, frees the NIC memory used to hold the Rx frame and allows the F/W to
@@ -579,7 +579,7 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  * If the MSF does not need access (any longer) to the current frame, e.g. because it is rejected based on the
  * look ahead or copied to another buffer, the receiver may be acked. Acking earlier is assumed to have the
  * potential of improving the performance.
- * If the MSF does not explitly ack te receiver, the acking is done implicitly if:
+ * If the MSF does not explicitly ack the receiver, the acking is done implicitly if:
  * - the received frame fits in the look ahead buffer, by the hcf_service_nic call that reported the Rx frame
  * - if not in the above step, by hcf_rcv_msg (assuming hcf_rcv_msg is called)
  * - if neither of the above implicit acks nor an explicit ack by the MSF, by the first hcf_service_nic after
@@ -591,9 +591,9 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  * The Inquire Tallies command requests the F/W to provide its current set of tallies.
  * See also hcf_get_info with CFG_TALLIES as parameter.
  *
- * o HCF_ACT_PRS_SCAN: Inquire Probe Respons Scan command
+ * o HCF_ACT_PRS_SCAN: Inquire Probe Response Scan command
  * This command is only operational if the F/W is enabled.
- * The Probe Respons Scan command starts a scan sequence.
+ * The Probe Response Scan command starts a scan sequence.
  * The HCF puts the result of this action in an MSF defined buffer (see CFG_RID_LOG_STRCT).
  *
  * o HCF_ACT_SCAN: Inquire Scan command
@@ -606,7 +606,7 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  * - NIC interrupts are not disabled while required by parameter action.
  * - an invalid code is specified in parameter action.
  * - HCF_ACT_INT_ON commands outnumber the HCF_ACT_INT_OFF commands.
- * - reentrancy, may be  caused by calling hcf_functions without adequate protection against NIC interrupts or
+ * - reentrancy, may be caused by calling hcf_functions without adequate protection against NIC interrupts or
  *   multi-threading
  *
  * - Since the HCF does not maintain status information relative to the F/W enabled state, it is not asserted
@@ -625,7 +625,7 @@ HCF_STATIC hcf_16* BASED xxxx[ ] = {
  *   change in HREG_EV_STAT matching a bit in HREG_INT_EN, i.e. not if invoked as result of another device
  *   generating an interrupt on the shared interrupt line.
  *   Note 1: it has been observed that under certain adverse conditions on certain platforms the writing of
- *   HREG_INT_EN can apparently fail, therefor it is paramount that HREG_INT_EN is written again with 0 for
+ *   HREG_INT_EN can apparently fail, therefore it is paramount that HREG_INT_EN is written again with 0 for
  *   each and every call to HCF_ACT_INT_OFF.
  *   Note 2: it has been observed that under certain H/W & S/W architectures this logic is called when there is
  *   no NIC at all. To cater for this, the value of HREG_INT_EN is validated. If the unused bit 0x0100 is set,
@@ -902,7 +902,7 @@ hcf_action( IFBP ifbp, hcf_16 action )
  * - A command other than Continue, Enable, Disable, Connect or Disconnect is given.
  * - An invalid combination of the subfields is given or a bit outside the subfields is given.
  * - any return code besides HCF_SUCCESS.
- * - reentrancy, may be  caused by calling a hcf_function without adequate protection against NIC interrupts or
+ * - reentrancy, may be caused by calling a hcf_function without adequate protection against NIC interrupts or
  *   multi-threading
  *
  *.DIAGRAM
@@ -1030,7 +1030,7 @@ hcf_cntl( IFBP ifbp, hcf_16 cmd )
  * hcf_connect passes the MSF-defined location of the IFB to the HCF and grants or revokes access right for the
  * HCF to the IFB. Revoking is done by specifying HCF_DISCONNECT rather than an I/O address for the parameter
  * io_base.  Every call of hcf_connect in "connect" mode, must eventually be followed by a call of hcf_connect
- * in "disconnect" mode. Clalling hcf_connect in "connect"/"disconnect" mode can not be nested.
+ * in "disconnect" mode. Calling hcf_connect in "connect"/"disconnect" mode can not be nested.
  * The IFB address must be used as a handle with all subsequent HCF-function calls and the HCF uses the IFB
  * address as a handle when it performs a call(back) of an MSF-function (i.e. msf_assert).
  *
@@ -1058,7 +1058,7 @@ hcf_cntl( IFBP ifbp, hcf_16 cmd )
  *   specification for S/W reset
  *   Note 2: it turns out that on some H/W constellations, the clock to access the EEProm is not lowered
  *   to an appropriate frequency by HREG_IO_SRESET. By giving an HCMD_INI first, this problem is worked around.
- *2b: Experimentally it is determined over a wide range of F/W versions that waiting for the for Cmd bit in
+ *2b: Experimentally it is determined over a wide range of F/W versions that are waiting for the for Cmd bit in
  *   Ev register gives a workable strategy. The available documentation does not give much clues.
  *4: clear and initialize the IFB
  *   The HCF house keeping info is designed such that zero is the appropriate initial value for as much as
diff --git a/drivers/staging/wlags49_h2/hcfcfg.h b/drivers/staging/wlags49_h2/hcfcfg.h
index 39fb4d326f65..869b5c343a86 100644
--- a/drivers/staging/wlags49_h2/hcfcfg.h
+++ b/drivers/staging/wlags49_h2/hcfcfg.h
@@ -21,7 +21,7 @@
 * hcfcfg.tpl list all #defines which must be specified to:
 *   adjust the HCF functions defined in HCF.C to the characteristics of a specific environment
 *		o maximum sizes for messages
-*		o Endianess
+*		o Endianness
 *	Compiler specific macros
 *		o port I/O macros
 *		o type definitions
diff --git a/drivers/staging/wlags49_h2/hcfdef.h b/drivers/staging/wlags49_h2/hcfdef.h
index 30744e194a23..74c0f713c57e 100644
--- a/drivers/staging/wlags49_h2/hcfdef.h
+++ b/drivers/staging/wlags49_h2/hcfdef.h
@@ -652,15 +652,15 @@ err: you used an invalid bitmask;
 #if 0 //get compiler going
 #if HCF_EX_INT_TICK != HREG_EV_TICK
 ;? out dated checking
-err: someone redefined these macros while the implemenation assumes they are equal;
+err: someone redefined these macros while the implementation assumes they are equal;
 #endif
 #if HCF_EX_INT_TX_OK != HFS_TX_CNTL_TX_OK || HFS_TX_CNTL_TX_OK != HREG_EV_TX_OK
 ;? out dated checking
-err: someone redefined these macros while the implemenation assumes they are equal;
+err: someone redefined these macros while the implementation assumes they are equal;
 #endif
 #if HCF_EX_INT_TX_EX != HFS_TX_CNTL_TX_EX || HFS_TX_CNTL_TX_EX != HREG_EV_TX_EX
 ;? out dated checking
-err: someone redefined these macros while the implemenation assumes they are equal;
+err: someone redefined these macros while the implementation assumes they are equal;
 #endif
 #endif // 0 get compiler going
 
diff --git a/drivers/staging/wlags49_h2/mdd.h b/drivers/staging/wlags49_h2/mdd.h
index 5c3515f31a16..0c914971c1ef 100644
--- a/drivers/staging/wlags49_h2/mdd.h
+++ b/drivers/staging/wlags49_h2/mdd.h
@@ -652,7 +652,7 @@ XX1( CFG_SCAN,					SCAN_RS_STRCT, scan_result[32]		 ) /*Scan results											*
 #define CFG_CNF_WDS_ADDR6				0xFC16		//[AP] Port 6 MAC Adrs of corresponding WDS Link node
 #define CFG_CNF_PM_MCAST_BUF			0xFC17		//[AP] Switch for PM buffereing of Multicast Messages
 #define CFG_CNF_MCAST_PM_BUF			CFG_CNF_PM_MCAST_BUF	//name does not match H-II spec
-#define CFG_CNF_REJECT_ANY				0xFC18		//[AP] Switch for PM buffereing of Multicast Messages
+#define CFG_CNF_REJECT_ANY				0xFC18		//[AP] Switch for PM buffering of Multicast Messages
 
 #define CFG_CNF_ENCRYPTION				0xFC20		//select en/de-cryption of Tx/Rx messages
 #define CFG_CNF_AUTHENTICATION			0xFC21		//[STA] selects Authentication algorithm
@@ -844,13 +844,13 @@ XX1( CFG_SCAN,					SCAN_RS_STRCT, scan_result[32]		 ) /*Scan results											*
 
 
 #define HCF_SUCCESS					0x00	// OK
-#define HCF_ERR_TIME_OUT			0x04	// Expected Hermes event did not occure in expected time
+#define HCF_ERR_TIME_OUT			0x04	// Expected Hermes event did not occur in expected time
 #define HCF_ERR_NO_NIC				0x05	/* card not found (usually yanked away during hcfio_in_string
 										  	 * Also: card is either absent or disabled while it should be neither */
 #define HCF_ERR_LEN					0x08	/* buffer size insufficient
 		 								  	 *		  -	IFB_ConfigTable too small
 		 								  	 *		  -	hcf_get_info buffer has a size of 0 or 1 or less than needed
-		 							  		 *			to accomodate all data
+		 							  		 *			to accommodate all data
 		 							  		 *		  -	hcf_put_info: CFG_DLNV_DATA exceeds intermediate
 											 *		  buffer size */
 #define HCF_ERR_INCOMP_PRI			0x09	// primary functions are not compatible
@@ -1004,7 +1004,7 @@ XX1( CFG_SCAN,					SCAN_RS_STRCT, scan_result[32]		 ) /*Scan results											*
 #define CFG_CURRENT_LINK_STATUS			0x090B		//Latest link status got through 0xF200 LinkEvent
 
 /*============================================================ INFORMATION FRAMES =========================*/
-#define CFG_INFO_FRAME_MIN				0xF000		//lowest value representing an Informatio Frame
+#define CFG_INFO_FRAME_MIN				0xF000		//lowest value representing an Information Frame
 
 #define CFG_TALLIES						0xF100		//Communications Tallies
 #define CFG_SCAN						0xF101		//Scan results
diff --git a/drivers/staging/wlags49_h2/sta_h2.c b/drivers/staging/wlags49_h2/sta_h2.c
index f9a38523724c..00dffe2ed8f1 100644
--- a/drivers/staging/wlags49_h2/sta_h2.c
+++ b/drivers/staging/wlags49_h2/sta_h2.c
@@ -26,7 +26,7 @@
 
 
 #include "hcfcfg.h"				// to get hcf_16 etc defined as well as
-                                // possible settings which inluence mdd.h or dhf.h
+                                // possible settings which influence mdd.h or dhf.h
 #include "mdd.h"   				//to get COMP_ID_STA etc defined
 #include "dhf.h"   				//used to be "fhfmem.h", to get memblock,plugrecord,
 
diff --git a/drivers/staging/wlags49_h2/sta_h25.c b/drivers/staging/wlags49_h2/sta_h25.c
index 86ca1cdd8497..5b6f670d8ef2 100644
--- a/drivers/staging/wlags49_h2/sta_h25.c
+++ b/drivers/staging/wlags49_h2/sta_h25.c
@@ -25,7 +25,7 @@
 
 
 #include "hcfcfg.h"				// to get hcf_16 etc defined as well as
-                                // possible settings which inluence mdd.h or dhf.h
+                                // possible settings which influence mdd.h or dhf.h
 #include "mdd.h"   				//to get COMP_ID_STA etc defined
 #include "dhf.h"   				//used to be "fhfmem.h", to get memblock,plugrecord,
 
diff --git a/drivers/staging/wlags49_h2/wl_enc.h b/drivers/staging/wlags49_h2/wl_enc.h
index 46629f3b112b..1804611276b8 100644
--- a/drivers/staging/wlags49_h2/wl_enc.h
+++ b/drivers/staging/wlags49_h2/wl_enc.h
@@ -106,7 +106,7 @@ ENCSTRCT, *PENCSTRCT;
 
 
 /*******************************************************************************
- * function prrottypes
+ * function prototypes
  ******************************************************************************/
 int wl_wep_code( char *szCrypt, char *szDest, void *Data, int nLen );
 
diff --git a/drivers/staging/wlags49_h2/wl_if.h b/drivers/staging/wlags49_h2/wl_if.h
index 6a26130f5a3a..6d66dabf032c 100644
--- a/drivers/staging/wlags49_h2/wl_if.h
+++ b/drivers/staging/wlags49_h2/wl_if.h
@@ -95,7 +95,7 @@
 //              Manufacture ID: 0156,0003
 // Lowest measurment for noise floor seen is value 54
 // Highest signal strength in close proximity to the AP seen is value 118
-// Very good must be arround 100 (otherwise its never "full scale"
+// Very good must be around 100 (otherwise its never "full scale"
 // All other constants are derrived from these. This makes the signal gauge
 // work for me...
 #define HCF_MIN_SIGNAL_LEVEL        54
diff --git a/drivers/staging/wlags49_h2/wl_internal.h b/drivers/staging/wlags49_h2/wl_internal.h
index 553601f48873..b23078164149 100644
--- a/drivers/staging/wlags49_h2/wl_internal.h
+++ b/drivers/staging/wlags49_h2/wl_internal.h
@@ -11,7 +11,7 @@
  *
  *------------------------------------------------------------------------------
  *
- *   Header for defintions and macros internal to the drvier.
+ *   Header for definitions and macros internal to the drvier.
  *
  *------------------------------------------------------------------------------
  *
@@ -838,7 +838,7 @@ typedef struct dma_strct
 	DESC_STRCT  *rx_packet[NUM_RX_DESC];
 	DESC_STRCT  *rx_reclaim_desc, *tx_reclaim_desc; // Descriptors for host-reclaim purposes (see HCF)
 	int         tx_rsc_ind; // DMA Tx resource indicator is maintained in the MSF, not in the HCF
-	int         rx_rsc_ind; // Also added rx rsource indicator so that cleanup can be performed if alloc fails
+	int         rx_rsc_ind; // Also added rx resource indicator so that cleanup can be performed if alloc fails
 	int         status;
 } DMA_STRCT;
 
diff --git a/drivers/staging/wlags49_h2/wl_main.c b/drivers/staging/wlags49_h2/wl_main.c
index 204107829577..f5f120a62460 100644
--- a/drivers/staging/wlags49_h2/wl_main.c
+++ b/drivers/staging/wlags49_h2/wl_main.c
@@ -63,7 +63,7 @@
  *  constant definitions
  ******************************************************************************/
 
-/* Allow support for calling system fcns to access F/W iamge file */
+/* Allow support for calling system fcns to access F/W image file */
 #define __KERNEL_SYSCALLS__
 
 /*******************************************************************************
@@ -128,7 +128,7 @@
 #include <wl_pci.h>
 #endif  /* BUS_PCI */
 /*******************************************************************************
- *	macro defintions
+ *	macro definitions
  ******************************************************************************/
 #define VALID_PARAM(C) \
 	{ \
@@ -1163,7 +1163,7 @@ int 			rc;
 				CNV_INT_TO_LITTLE( lp->hcfCtx.IFB_FWIdentity.version_major ),
 				CNV_INT_TO_LITTLE( lp->hcfCtx.IFB_FWIdentity.version_minor ));
 
-	/* now we wil get the MAC address of the card */
+	/* now we will get the MAC address of the card */
 	lp->ltvRecord.len = 4;
 	if ( CNV_INT_TO_LITTLE( lp->hcfCtx.IFB_FWIdentity.comp_id ) == COMP_ID_FW_AP  ) {
 		lp->ltvRecord.typ = CFG_NIC_MAC_ADDR;
@@ -1374,7 +1374,7 @@ int wl_put_ltv_init( struct wl_private *lp )
 	lp->ltvRecord.len = 2;
 	lp->ltvRecord.typ = CFG_CNTL_OPT;
 
-	/* The Card Services build must ALWAYS configure for 16-bit I/O. PCI or
+	/* The Card Services build must ALWAYS be configured for 16-bit I/O. PCI or
 	   CardBus can be set to either 16/32 bit I/O, or Bus Master DMA, but only
 	   for Hermes-2.5 */
 #ifdef BUS_PCMCIA
@@ -1627,7 +1627,7 @@ int wl_put_ltv( struct wl_private *lp )
 		lp->ltvRecord.u.u16[0]  = CNV_INT_TO_LITTLE( lp->TxRateControl[0] );
 #endif  // WARP
 
-//;?skip temporarily to see whether the RID or something else is the probelm hcf_status = hcf_put_info( &lp->hcfCtx, (LTVP)&( lp->ltvRecord ));
+//;?skip temporarily to see whether the RID or something else is the problem hcf_status = hcf_put_info( &lp->hcfCtx, (LTVP)&( lp->ltvRecord ));
 
 		DBG_TRACE( DbgInfo, "CFG_TX_RATE_CNTL 2.4GHz           : 0x%04x\n",
 				   lp->TxRateControl[0] );
@@ -2474,7 +2474,7 @@ void wl_resume(struct net_device *dev)
  *
  *  DESCRIPTION:
  *
- *      This function perfroms a check on the device and calls wl_remove() if
+ *      This function performs a check on the device and calls wl_remove() if
  *  necessary. This function can be used for all bus types, but exists mostly
  *  for the benefit of the Card Services driver, as there are times when
  *  wl_remove() does not get called.
@@ -2596,7 +2596,7 @@ int wl_enable( struct wl_private *lp )
 			lp->portState = WVLAN_PORT_STATE_ENABLED;   //;?bad mnemonic, NIC iso PORT
 #ifdef ENABLE_DMA
 			if ( lp->use_dma ) {
-				wl_pci_dma_hcf_supply( lp );  //;?always succes?
+				wl_pci_dma_hcf_supply( lp );  //;?always successful?
 			}
 #endif
 		}
@@ -2874,7 +2874,7 @@ int wl_mbx( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      This function will perform the tedious task of endian translating all
- *  fields withtin a mailbox message which need translating.
+ *  fields within a mailbox message which need translating.
  *
  *  PARAMETERS:
  *
@@ -2989,7 +2989,7 @@ void wl_endian_translate_mailbox( ltv_t *ltv )
  *
  *  DESCRIPTION:
  *
- *      This function will process the mailbox data.
+ *      This function processes the mailbox data.
  *
  *  PARAMETERS:
  *
diff --git a/drivers/staging/wlags49_h2/wl_netdev.c b/drivers/staging/wlags49_h2/wl_netdev.c
index 824b85232353..fb421407e106 100644
--- a/drivers/staging/wlags49_h2/wl_netdev.c
+++ b/drivers/staging/wlags49_h2/wl_netdev.c
@@ -652,7 +652,6 @@ void wl_tx_timeout( struct net_device *dev )
     wl_unlock( lp, &flags );
 
     DBG_LEAVE( DbgInfo );
-    return;
 } // wl_tx_timeout
 /*============================================================================*/
 
@@ -836,8 +835,7 @@ int wl_tx( struct sk_buff *skb, struct net_device *dev, int port )
         txF->frame.port = port;
         /* Move the frame to the txQ */
         /* NOTE: Here's where we would do priority queueing */
-        list_del( &( txF->node ));
-        list_add( &( txF->node ), &( lp->txQ[0] ));
+        list_move(&(txF->node), &(lp->txQ[0]));
 
         lp->txQ_count++;
         if( lp->txQ_count >= DEFAULT_NUM_TX_FRAMES ) {
@@ -1252,7 +1250,7 @@ struct net_device * wl_device_alloc( void )
 
     netif_stop_queue( dev );
 
-    /* Allocate virutal devices for WDS support if needed */
+    /* Allocate virtual devices for WDS support if needed */
     WL_WDS_DEVICE_ALLOC( lp );
 
     DBG_LEAVE( DbgInfo );
@@ -1292,7 +1290,6 @@ void wl_device_dealloc( struct net_device *dev )
     free_netdev( dev );
 
     DBG_LEAVE( DbgInfo );
-    return;
 } // wl_device_dealloc
 /*============================================================================*/
 
@@ -1547,7 +1544,6 @@ void wl_wds_device_alloc( struct wl_private *lp )
     WL_WDS_NETIF_STOP_QUEUE( lp );
 
     DBG_LEAVE( DbgInfo );
-    return;
 } // wl_wds_device_alloc
 /*============================================================================*/
 
@@ -1593,7 +1589,6 @@ void wl_wds_device_dealloc( struct wl_private *lp )
     }
 
     DBG_LEAVE( DbgInfo );
-    return;
 } // wl_wds_device_dealloc
 /*============================================================================*/
 
@@ -1604,7 +1599,7 @@ void wl_wds_device_dealloc( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      Used to start the netif queues of all the "virtual" network devices
- *      which repesent the WDS ports.
+ *      which represent the WDS ports.
  *
  *  PARAMETERS:
  *
@@ -1629,8 +1624,6 @@ void wl_wds_netif_start_queue( struct wl_private *lp )
             }
         }
     }
-
-    return;
 } // wl_wds_netif_start_queue
 /*============================================================================*/
 
@@ -1641,7 +1634,7 @@ void wl_wds_netif_start_queue( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      Used to stop the netif queues of all the "virtual" network devices
- *      which repesent the WDS ports.
+ *      which represent the WDS ports.
  *
  *  PARAMETERS:
  *
@@ -1666,8 +1659,6 @@ void wl_wds_netif_stop_queue( struct wl_private *lp )
             }
         }
     }
-
-    return;
 } // wl_wds_netif_stop_queue
 /*============================================================================*/
 
@@ -1678,7 +1669,7 @@ void wl_wds_netif_stop_queue( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      Used to wake the netif queues of all the "virtual" network devices
- *      which repesent the WDS ports.
+ *      which represent the WDS ports.
  *
  *  PARAMETERS:
  *
@@ -1703,8 +1694,6 @@ void wl_wds_netif_wake_queue( struct wl_private *lp )
             }
         }
     }
-
-    return;
 } // wl_wds_netif_wake_queue
 /*============================================================================*/
 
@@ -1715,7 +1704,7 @@ void wl_wds_netif_wake_queue( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      Used to signal the network layer that carrier is present on all of the
- *      "virtual" network devices which repesent the WDS ports.
+ *      "virtual" network devices which represent the WDS ports.
  *
  *  PARAMETERS:
  *
@@ -1738,8 +1727,6 @@ void wl_wds_netif_carrier_on( struct wl_private *lp )
             }
         }
     }
-
-    return;
 } // wl_wds_netif_carrier_on
 /*============================================================================*/
 
@@ -1750,7 +1737,7 @@ void wl_wds_netif_carrier_on( struct wl_private *lp )
  *  DESCRIPTION:
  *
  *      Used to signal the network layer that carrier is NOT present on all of
- *      the "virtual" network devices which repesent the WDS ports.
+ *      the "virtual" network devices which represent the WDS ports.
  *
  *  PARAMETERS:
  *
@@ -1763,18 +1750,15 @@ void wl_wds_netif_carrier_on( struct wl_private *lp )
  ******************************************************************************/
 void wl_wds_netif_carrier_off( struct wl_private *lp )
 {
-    int count;
-    /*------------------------------------------------------------------------*/
+	int count;
 
-    if( lp != NULL ) {
-        for( count = 0; count < NUM_WDS_PORTS; count++ ) {
-            if( lp->wds_port[count].is_registered ) {
-                netif_carrier_off( lp->wds_port[count].dev );
-            }
-        }
-    }
+	if(lp != NULL) {
+		for(count = 0; count < NUM_WDS_PORTS; count++) {
+			if(lp->wds_port[count].is_registered)
+				netif_carrier_off(lp->wds_port[count].dev);
+		}
+	}
 
-    return;
 } // wl_wds_netif_carrier_off
 /*============================================================================*/
 
@@ -1810,22 +1794,19 @@ int wl_send_dma( struct wl_private *lp, struct sk_buff *skb, int port )
 
     DBG_FUNC( "wl_send_dma" );
 
-    if( lp == NULL )
-    {
+    if( lp == NULL ) {
         DBG_ERROR( DbgInfo, "Private adapter struct is NULL\n" );
         return FALSE;
     }
 
-    if( lp->dev == NULL )
-    {
+    if( lp->dev == NULL ) {
         DBG_ERROR( DbgInfo, "net_device struct in wl_private is NULL\n" );
         return FALSE;
     }
 
     /* AGAIN, ALL THE QUEUEING DONE HERE IN I/O MODE IS NOT PERFORMED */
 
-    if( skb == NULL )
-    {
+    if( skb == NULL ) {
         DBG_WARNING (DbgInfo, "Nothing to send.\n");
         return FALSE;
     }
@@ -1835,8 +1816,7 @@ int wl_send_dma( struct wl_private *lp, struct sk_buff *skb, int port )
     /* Get a free descriptor */
     desc = wl_pci_dma_get_tx_packet( lp );
 
-    if( desc == NULL )
-    {
+    if( desc == NULL ) {
         if( lp->netif_queue_on == TRUE ) {
             netif_stop_queue( lp->dev );
             WL_WDS_NETIF_STOP_QUEUE( lp );
@@ -1852,8 +1832,7 @@ int wl_send_dma( struct wl_private *lp, struct sk_buff *skb, int port )
 
     desc_next = desc->next_desc_addr;
 
-    if( desc_next->buf_addr == NULL )
-    {
+    if( desc_next->buf_addr == NULL ) {
         DBG_ERROR( DbgInfo, "DMA descriptor buf_addr is NULL\n" );
         return FALSE;
     }
diff --git a/drivers/staging/wlags49_h2/wl_pci.c b/drivers/staging/wlags49_h2/wl_pci.c
index 0b31b01bd490..a09c3ac793a2 100644
--- a/drivers/staging/wlags49_h2/wl_pci.c
+++ b/drivers/staging/wlags49_h2/wl_pci.c
@@ -223,7 +223,7 @@ int wl_adapter_init_module( void )
  ******************************************************************************/
 void wl_adapter_cleanup_module( void )
 {
-	//;?how comes wl_adapter_cleanup_module is located in a seemingly pci specific module
+	//;?how come wl_adapter_cleanup_module is located in a seemingly pci specific module
     DBG_FUNC( "wl_adapter_cleanup_module" );
     DBG_ENTER( DbgInfo );
 
@@ -385,7 +385,7 @@ int wl_adapter_is_open( struct net_device *dev )
  *  DESCRIPTION:
  *
  *      Registered in the pci_driver structure, this function is called when the
- *  PCI subsystem finds a new PCI device which matches the infomation contained
+ *  PCI subsystem finds a new PCI device which matches the information contained
  *  in the pci_device_id table.
  *
  *  PARAMETERS:
@@ -424,7 +424,7 @@ int __devinit wl_pci_probe( struct pci_dev *pdev,
  *  DESCRIPTION:
  *
  *      Registered in the pci_driver structure, this function is called when the
- *  PCI subsystem detects that a PCI device which matches the infomation
+ *  PCI subsystem detects that a PCI device which matches the information
  *  contained in the pci_device_id table has been removed.
  *
  *  PARAMETERS:
@@ -899,7 +899,7 @@ int wl_pci_dma_free_tx_packet( struct pci_dev *pdev, struct wl_private *lp,
  *  DESCRIPTION:
  *
  *      Allocates a single Rx packet, consisting of two descriptors and one
- *      contiguous buffer. THe buffer starts with the hermes-specific header.
+ *      contiguous buffer. The buffer starts with the hermes-specific header.
  *      One descriptor points at the start, the other at offset 0x3a of the
  *      buffer.
  *
diff --git a/drivers/staging/wlags49_h2/wl_priv.c b/drivers/staging/wlags49_h2/wl_priv.c
index f30e5ee4bca3..87e1e4123126 100644
--- a/drivers/staging/wlags49_h2/wl_priv.c
+++ b/drivers/staging/wlags49_h2/wl_priv.c
@@ -225,7 +225,7 @@ int wvlan_uil_connect( struct uilreq *urq, struct wl_private *lp )
  *
  *  DESCRIPTION:
  *
- *      Disonnect from the UIL after a request has been completed.
+ *      Disconnect from the UIL after a request has been completed.
  *
  *  PARAMETERS:
  *
diff --git a/drivers/staging/wlags49_h2/wl_profile.c b/drivers/staging/wlags49_h2/wl_profile.c
index 0e49272bc7a8..beabf5916df7 100644
--- a/drivers/staging/wlags49_h2/wl_profile.c
+++ b/drivers/staging/wlags49_h2/wl_profile.c
@@ -910,7 +910,7 @@ int parse_mac_address(char *value, u_char *byte_array)
 	memset(byte_field, '\0', 3);
 
 	while (value[value_offset] != '\0') {
-		/* Skip over the colon chars seperating the bytes, if they exist */
+		/* Skip over the colon chars separating the bytes, if they exist */
 		if (value[value_offset] == ':') {
 			value_offset++;
 			continue;
diff --git a/drivers/staging/wlags49_h2/wl_version.h b/drivers/staging/wlags49_h2/wl_version.h
index 3deacfac9d25..037b5266428c 100644
--- a/drivers/staging/wlags49_h2/wl_version.h
+++ b/drivers/staging/wlags49_h2/wl_version.h
@@ -152,7 +152,7 @@ err: define bus type;
 
 
 /*******************************************************************************
- *  bus architechture specific defines, includes, etc.
+ *  bus architecture specific defines, includes, etc.
  ******************************************************************************/
 /*
  * There doesn't seem to be a difference for PCMCIA and PCI anymore, at least
diff --git a/drivers/staging/wlags49_h2/wl_wext.c b/drivers/staging/wlags49_h2/wl_wext.c
index 7ff0a108da13..f553366cccc5 100644
--- a/drivers/staging/wlags49_h2/wl_wext.c
+++ b/drivers/staging/wlags49_h2/wl_wext.c
@@ -62,6 +62,7 @@
 #include <linux/if_arp.h>
 #include <linux/ioport.h>
 #include <linux/delay.h>
+#include <linux/etherdevice.h>
 #include <asm/uaccess.h>
 
 #include <debug.h>
@@ -173,7 +174,7 @@ static int hermes_clear_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr)
 
 	switch (key_idx) {
 	case 0:
-		if (memcmp(addr, "\xff\xff\xff\xff\xff\xff", ETH_ALEN) != 0) {
+		if (!is_broadcast_ether_addr(addr)) {
 			ltv->len = 7;
 			ltv->typ = CFG_REMOVE_TKIP_MAPPED_KEY;
 			memcpy(&ltv->u.u8[0], addr, ETH_ALEN);
@@ -605,7 +606,7 @@ retry:
 		if (retries < 10) {
 			retries++;
 
-			/* Holding the lock too long, make a gap to allow other processes */
+			/* Holding the lock too long, makes a gap to allow other processes */
 			wl_unlock(lp, &flags);
 			wl_lock( lp, &flags );
 
@@ -617,7 +618,7 @@ retry:
 				goto out_unlock;
 			}
 
-			/* Holding the lock too long, make a gap to allow other processes */
+			/* Holding the lock too long, makes a gap to allow other processes */
 			wl_unlock(lp, &flags);
 			wl_lock( lp, &flags );
 
@@ -630,7 +631,7 @@ retry:
 		}
 	}
 
-	/* Holding the lock too long, make a gap to allow other processes */
+	/* Holding the lock too long, makes a gap to allow other processes */
 	wl_unlock(lp, &flags);
 	wl_lock( lp, &flags );
 
@@ -693,7 +694,7 @@ retry:
 
 	/* Encryption */
 
-	/* Holding the lock too long, make a gap to allow other processes */
+	/* Holding the lock too long, makes a gap to allow other processes */
 	wl_unlock(lp, &flags);
 	wl_lock( lp, &flags );
 
@@ -720,7 +721,7 @@ retry:
 
 	// Retry Limits and Lifetime - NOT SUPPORTED
 
-	/* Holding the lock too long, make a gap to allow other processes */
+	/* Holding the lock too long, makes a gap to allow other processes */
 	wl_unlock(lp, &flags);
 	wl_lock( lp, &flags );
 
@@ -2595,7 +2596,7 @@ static int wireless_set_scan(struct net_device *dev, struct iw_request_info *inf
 	int		    retries = 0;
 	/*------------------------------------------------------------------------*/
 
-	//;? Note: shows results as trace, retruns always 0 unless BUSY
+	//;? Note: shows results as trace, returns always 0 unless BUSY
 
 	DBG_FUNC( "wireless_set_scan" );
 	DBG_ENTER( DbgInfo );
@@ -2645,7 +2646,7 @@ retry:
 
 	DBG_TRACE( DbgInfo, "CFG_SCAN_CHANNEL result      : 0x%x\n", status );
 
-	// Holding the lock too long, make a gap to allow other processes
+	// Holding the lock too long, makes a gap to allow other processes
 	wl_unlock(lp, &flags);
 	wl_lock( lp, &flags );
 
@@ -2656,7 +2657,7 @@ retry:
 			DBG_TRACE( DbgInfo, "Reset card to recover, attempt: %d\n", retries );
 			wl_reset( dev );
 
-			// Holding the lock too long, make a gap to allow other processes
+			// Holding the lock too long, makes a gap to allow other processes
 			wl_unlock(lp, &flags);
 			wl_lock( lp, &flags );
 
@@ -2673,7 +2674,7 @@ retry:
 
 	status = hcf_put_info( &( lp->hcfCtx ), (LTVP)&( lp->ltvRecord ));
 
-	// Holding the lock too long, make a gap to allow other processes
+	// Holding the lock too long, makes a gap to allow other processes
 	wl_unlock(lp, &flags);
 	wl_lock( lp, &flags );
 
@@ -2681,7 +2682,7 @@ retry:
 
 	/* Initiate the scan */
 	/* NOTE: Using HCF_ACT_SCAN has been removed, as using HCF_ACT_ACS_SCAN to
-	   retrieve probe responses must always be used to support WPA */
+	   retrieve probe response must always be used to support WPA */
 	status = hcf_action( &( lp->hcfCtx ), HCF_ACT_ACS_SCAN );
 
 	if( status == HCF_SUCCESS ) {
@@ -3054,7 +3055,7 @@ static void flush_tx(struct wl_private *lp)
 	 * Make sure that there is no data queued up in the firmware
 	 * before setting the TKIP keys. If this check is not
 	 * performed, some data may be sent out with incorrect MIC
-	 * and cause synchronizarion errors with the AP
+	 * and cause synchronization errors with the AP
 	 */
 	/* Check every 1ms for 100ms */
 	for (count = 0; count < 100; count++) {
@@ -3654,7 +3655,7 @@ void wl_wext_event_ap( struct net_device *dev )
 	   this event BEFORE sending the association event, as there are timing
 	   issues with the hostap supplicant. The supplicant will attempt to process
 	   an EAPOL-Key frame from an AP before receiving this information, which
-	   is required properly process the said frame. */
+	   is required for a proper processed frame. */
 	wl_wext_event_assoc_ie( dev );
 
 	/* Get the BSSID */
diff --git a/drivers/staging/wlags49_h25/Kconfig b/drivers/staging/wlags49_h25/Kconfig
index bf5664a51cd4..dd8dc4d62c64 100644
--- a/drivers/staging/wlags49_h25/Kconfig
+++ b/drivers/staging/wlags49_h25/Kconfig
@@ -8,4 +8,4 @@ config WLAGS49_H25
 	Driver for wireless cards using Agere's HERMES II.5 chipset
 	which are identified with Manufacture ID: 0156,0004
 	The software is a modified version of wl_lkm_722_abg.tar.gz
-	from the Agere Systems website, addapted for Ubuntu 9.04.
+	from the Agere Systems website, adapted for Ubuntu 9.04.
diff --git a/drivers/staging/wlags49_h25/TODO b/drivers/staging/wlags49_h25/TODO
index 94032b6ac2b5..ec71ad3245e4 100644
--- a/drivers/staging/wlags49_h25/TODO
+++ b/drivers/staging/wlags49_h25/TODO
@@ -1,4 +1,4 @@
-First of all, the best thing would be that this driver becomes obsolte by
+First of all, the best thing would be that this driver becomes obsolete by
 adding support for Hermes II and Hermes II.5 cards to the existing orinoco
 driver. The orinoco driver currently only supports Hermes I based cards.
 Since this will not happen by magic and has not happened until now this
@@ -10,11 +10,11 @@ list.
 
 TODO:
 	- verify against a Hermes II.5 card
-	- verify with WPA encription (both with H2 and H2.5 cards)
+	- verify with WPA encryption (both with H2 and H2.5 cards)
 	- sometimes the card does not initialize correctly, retry mechanisms
 	  are build in to catch most cases but not all
 	- once the driver runs it is very stable, but I have the impression
-	  some the crittical sections take to long
+	  some the critical sections take to long
 	- the driver is split into a Hermes II and a Hermes II.5 part, it
 	  would be nice to handle both with one module instead of two
 	- review by the wireless developer community
@@ -25,7 +25,7 @@ TODO:
 DONE:
 	- verified against a Hermes II card (Thomson Speedtouch 110 PCMCIA
 	  card)
-	- verified with WEP encription
+	- verified with WEP encryption
 
 Please send any patches or complaints about this driver to Greg
 Kroah-Hartman <greg@kroah.com> and Cc: Henk de Groot <pe1dnn@amsat.org>
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index 0970127344e6..18c06a59c091 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -1,7 +1,7 @@
 /* cfg80211 Interface for prism2_usb module */
 
 
-/* Prism2 channell/frequency/bitrate declarations */
+/* Prism2 channel/frequency/bitrate declarations */
 static const struct ieee80211_channel prism2_channels[] = {
 	{ .center_freq = 2412 },
 	{ .center_freq = 2417 },
@@ -329,9 +329,9 @@ int prism2_get_station(struct wiphy *wiphy, struct net_device *dev,
 
 int prism2_scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 {
-	struct net_device *dev = request->wdev->netdev;
+	struct net_device *dev;
 	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
-	wlandevice_t *wlandev = dev->ml_priv;
+	wlandevice_t *wlandev;
 	struct p80211msg_dot11req_scan msg1;
 	struct p80211msg_dot11req_scan_results msg2;
 	struct cfg80211_bss *bss;
@@ -345,6 +345,9 @@ int prism2_scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 	if (!request)
 		return -EINVAL;
 
+	dev = request->wdev->netdev;
+	wlandev = dev->ml_priv;
+
 	if (priv->scan_request && priv->scan_request != request)
 		return -EBUSY;
 
@@ -499,7 +502,7 @@ int prism2_connect(struct wiphy *wiphy, struct net_device *dev,
 			goto exit;
 	}
 
-	/* Set the authorisation */
+	/* Set the authorization */
 	if ((sme->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM) ||
 		((sme->auth_type == NL80211_AUTHTYPE_AUTOMATIC) && !is_wep))
 			msg_join.authtype.data = P80211ENUM_authalg_opensystem;
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 7843dfdaa3cf..f180c3d8b012 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -2140,11 +2140,7 @@ exit_proc:
 ----------------------------------------------------------------*/
 int hfa384x_drvr_getconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len)
 {
-	int result;
-
-	result = hfa384x_dorrid_wait(hw, rid, buf, len);
-
-	return result;
+	return hfa384x_dorrid_wait(hw, rid, buf, len);
 }
 
 /*----------------------------------------------------------------
@@ -3790,7 +3786,7 @@ static void hfa384x_ctlxout_callback(struct urb *urb)
 #endif
 	if ((urb->status == -ESHUTDOWN) ||
 	    (urb->status == -ENODEV) || (hw == NULL))
-		goto done;
+		return;
 
 retry:
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -3803,7 +3799,7 @@ retry:
 	 */
 	if (list_empty(&hw->ctlxq.active)) {
 		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-		goto done;
+		return;
 	}
 
 	/*
@@ -3886,9 +3882,6 @@ delresp:
 
 	if (run_queue)
 		hfa384x_usbctlxq_run(hw);
-
-done:
-	;
 }
 
 /*----------------------------------------------------------------
@@ -3985,15 +3978,10 @@ static void hfa384x_usbctlx_resptimerfn(unsigned long data)
 		if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0) {
 			spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
 			hfa384x_usbctlxq_run(hw);
-			goto done;
+			return;
 		}
 	}
-
 	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-
-done:
-	;
-
 }
 
 /*----------------------------------------------------------------
@@ -4057,23 +4045,20 @@ static void hfa384x_usb_throttlefn(unsigned long data)
 static int hfa384x_usbctlx_submit(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx)
 {
 	unsigned long flags;
-	int ret;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
 
 	if (hw->wlandev->hwremoved) {
 		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-		ret = -ENODEV;
-	} else {
-		ctlx->state = CTLX_PENDING;
-		list_add_tail(&ctlx->list, &hw->ctlxq.pending);
-
-		spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-		hfa384x_usbctlxq_run(hw);
-		ret = 0;
+		return -ENODEV;
 	}
 
-	return ret;
+	ctlx->state = CTLX_PENDING;
+	list_add_tail(&ctlx->list, &hw->ctlxq.pending);
+	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
+	hfa384x_usbctlxq_run(hw);
+
+	return 0;
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index f53a27a2e3fe..3df753b51e89 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -559,17 +559,17 @@ void p80211skb_rxmeta_detach(struct sk_buff *skb)
 	/* Sanity checks */
 	if (skb == NULL) {	/* bad skb */
 		pr_debug("Called w/ null skb.\n");
-		goto exit;
+		return;
 	}
 	frmmeta = P80211SKB_FRMMETA(skb);
 	if (frmmeta == NULL) {	/* no magic */
 		pr_debug("Called w/ bad frmmeta magic.\n");
-		goto exit;
+		return;
 	}
 	rxmeta = frmmeta->rx;
 	if (rxmeta == NULL) {	/* bad meta ptr */
 		pr_debug("Called w/ bad rxmeta ptr.\n");
-		goto exit;
+		return;
 	}
 
 	/* Free rxmeta */
@@ -577,8 +577,6 @@ void p80211skb_rxmeta_detach(struct sk_buff *skb)
 
 	/* Clear skb->cb */
 	memset(skb->cb, 0, sizeof(skb->cb));
-exit:
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -660,5 +658,4 @@ void p80211skb_free(struct wlandevice *wlandev, struct sk_buff *skb)
 	else
 		printk(KERN_ERR "Freeing an skb (%p) w/ no frmmeta.\n", skb);
 	dev_kfree_skb(skb);
-	return;
 }
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index 0f51b4ab3631..750330f064f9 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -240,10 +240,7 @@ void p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 {
 	/* Enqueue for post-irq processing */
 	skb_queue_tail(&wlandev->nsd_rxq, skb);
-
 	tasklet_schedule(&wlandev->rx_bh);
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -464,7 +461,7 @@ failed:
 /*----------------------------------------------------------------
 * p80211knetdev_set_multicast_list
 *
-* Called from higher lavers whenever there's a need to set/clear
+* Called from higher layers whenever there's a need to set/clear
 * promiscuous mode or rewrite the multicast list.
 *
 * Arguments:
@@ -644,7 +641,7 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 	p80211item_unk392_t *mibattr;
 	p80211item_pstr6_t *macaddr;
 	p80211item_uint32_t *resultcode;
-	int result = 0;
+	int result;
 
 	/* If we're running, we don't allow MAC address changes */
 	if (netif_running(dev))
@@ -806,15 +803,13 @@ int wlan_setup(wlandevice_t *wlandev, struct device *physdev)
 * Arguments:
 *	wlandev		ptr to the wlandev structure for the
 *			interface.
-* Returns:
-*	zero on success, non-zero otherwise.
 * Call Context:
 *	Should be process thread.  We'll assume it might be
 *	interrupt though.  When we add support for statically
 *	compiled drivers, this function will be called in the
 *	context of the kernel startup code.
 ----------------------------------------------------------------*/
-int wlan_unsetup(wlandevice_t *wlandev)
+void wlan_unsetup(wlandevice_t *wlandev)
 {
 	struct wireless_dev *wdev;
 
@@ -827,8 +822,6 @@ int wlan_unsetup(wlandevice_t *wlandev)
 		free_netdev(wlandev->netdev);
 		wlandev->netdev = NULL;
 	}
-
-	return 0;
 }
 
 /*----------------------------------------------------------------
@@ -852,13 +845,7 @@ int wlan_unsetup(wlandevice_t *wlandev)
 ----------------------------------------------------------------*/
 int register_wlandev(wlandevice_t *wlandev)
 {
-	int i = 0;
-
-	i = register_netdev(wlandev->netdev);
-	if (i)
-		return i;
-
-	return 0;
+	return register_netdev(wlandev->netdev);
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
index 85884176b661..2fecca2302f4 100644
--- a/drivers/staging/wlan-ng/p80211netdev.h
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -235,7 +235,7 @@ int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum,
 		u8 *iv, u8 *icv);
 
 int wlan_setup(wlandevice_t *wlandev, struct device *physdev);
-int wlan_unsetup(wlandevice_t *wlandev);
+void wlan_unsetup(wlandevice_t *wlandev);
 int register_wlandev(wlandevice_t *wlandev);
 int unregister_wlandev(wlandevice_t *wlandev);
 void p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index 179194e7d2aa..cdfd808d6854 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -73,7 +73,7 @@
 #include "p80211req.h"
 
 static void p80211req_handlemsg(wlandevice_t *wlandev, struct p80211msg *msg);
-static int p80211req_mibset_mibget(wlandevice_t *wlandev,
+static void p80211req_mibset_mibget(wlandevice_t *wlandev,
 				   struct p80211msg_dot11req_mibget *mib_msg,
 				   int isget);
 
@@ -155,32 +155,29 @@ static void p80211req_handlemsg(wlandevice_t *wlandev, struct p80211msg *msg)
 	switch (msg->msgcode) {
 
 	case DIDmsg_lnxreq_hostwep:{
-			struct p80211msg_lnxreq_hostwep *req =
-			    (struct p80211msg_lnxreq_hostwep *) msg;
-			wlandev->hostwep &=
-			    ~(HOSTWEP_DECRYPT | HOSTWEP_ENCRYPT);
-			if (req->decrypt.data == P80211ENUM_truth_true)
-				wlandev->hostwep |= HOSTWEP_DECRYPT;
-			if (req->encrypt.data == P80211ENUM_truth_true)
-				wlandev->hostwep |= HOSTWEP_ENCRYPT;
-
-			break;
-		}
+		struct p80211msg_lnxreq_hostwep *req =
+			(struct p80211msg_lnxreq_hostwep *) msg;
+		wlandev->hostwep &=
+				~(HOSTWEP_DECRYPT | HOSTWEP_ENCRYPT);
+		if (req->decrypt.data == P80211ENUM_truth_true)
+			wlandev->hostwep |= HOSTWEP_DECRYPT;
+		if (req->encrypt.data == P80211ENUM_truth_true)
+			wlandev->hostwep |= HOSTWEP_ENCRYPT;
+
+	break;
+	}
 	case DIDmsg_dot11req_mibget:
 	case DIDmsg_dot11req_mibset:{
-			int isget = (msg->msgcode == DIDmsg_dot11req_mibget);
-			struct p80211msg_dot11req_mibget *mib_msg =
-			    (struct p80211msg_dot11req_mibget *) msg;
-			p80211req_mibset_mibget(wlandev, mib_msg, isget);
-		}
-	default:
-		;
+		int isget = (msg->msgcode == DIDmsg_dot11req_mibget);
+		struct p80211msg_dot11req_mibget *mib_msg =
+			(struct p80211msg_dot11req_mibget *) msg;
+		p80211req_mibset_mibget(wlandev, mib_msg, isget);
+	break;
+	}
 	}			/* switch msg->msgcode */
-
-	return;
 }
 
-static int p80211req_mibset_mibget(wlandevice_t *wlandev,
+static void p80211req_mibset_mibget(wlandevice_t *wlandev,
 				   struct p80211msg_dot11req_mibget *mib_msg,
 				   int isget)
 {
@@ -190,76 +187,65 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 
 	switch (mibitem->did) {
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey0:{
-			if (!isget)
-				wep_change_key(wlandev, 0, key, pstr->len);
-			break;
-		}
+		if (!isget)
+			wep_change_key(wlandev, 0, key, pstr->len);
+	break;
+	}
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey1:{
-			if (!isget)
-				wep_change_key(wlandev, 1, key, pstr->len);
-			break;
-		}
+		if (!isget)
+			wep_change_key(wlandev, 1, key, pstr->len);
+	break;
+	}
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey2:{
-			if (!isget)
-				wep_change_key(wlandev, 2, key, pstr->len);
-			break;
-		}
+		if (!isget)
+			wep_change_key(wlandev, 2, key, pstr->len);
+	break;
+	}
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey3:{
-			if (!isget)
-				wep_change_key(wlandev, 3, key, pstr->len);
-			break;
-		}
+		if (!isget)
+			wep_change_key(wlandev, 3, key, pstr->len);
+	break;
+	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID:{
-			u32 *data = (u32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
-			if (isget) {
-				*data =
-				    wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
-			} else {
-				wlandev->hostwep &= ~(HOSTWEP_DEFAULTKEY_MASK);
-
-				wlandev->hostwep |=
-				    (*data & HOSTWEP_DEFAULTKEY_MASK);
-			}
-			break;
+		if (isget) {
+			*data = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
+		} else {
+			wlandev->hostwep &= ~(HOSTWEP_DEFAULTKEY_MASK);
+			wlandev->hostwep |= (*data & HOSTWEP_DEFAULTKEY_MASK);
 		}
+	break;
+	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked:{
-			u32 *data = (u32 *) mibitem->data;
-
-			if (isget) {
-				if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
-					*data = P80211ENUM_truth_true;
-				else
-					*data = P80211ENUM_truth_false;
-			} else {
-				wlandev->hostwep &= ~(HOSTWEP_PRIVACYINVOKED);
-				if (*data == P80211ENUM_truth_true)
-					wlandev->hostwep |=
-					    HOSTWEP_PRIVACYINVOKED;
-			}
-			break;
+		u32 *data = (u32 *) mibitem->data;
+
+		if (isget) {
+			if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
+				*data = P80211ENUM_truth_true;
+			else
+				*data = P80211ENUM_truth_false;
+		} else {
+			wlandev->hostwep &= ~(HOSTWEP_PRIVACYINVOKED);
+			if (*data == P80211ENUM_truth_true)
+				wlandev->hostwep |= HOSTWEP_PRIVACYINVOKED;
 		}
+	break;
+	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted:{
-			u32 *data = (u32 *) mibitem->data;
-
-			if (isget) {
-				if (wlandev->hostwep &
-				    HOSTWEP_EXCLUDEUNENCRYPTED)
-					*data = P80211ENUM_truth_true;
-				else
-					*data = P80211ENUM_truth_false;
-			} else {
-				wlandev->hostwep &=
-				    ~(HOSTWEP_EXCLUDEUNENCRYPTED);
-				if (*data == P80211ENUM_truth_true)
-					wlandev->hostwep |=
-					    HOSTWEP_EXCLUDEUNENCRYPTED;
-			}
-			break;
+		u32 *data = (u32 *) mibitem->data;
+
+		if (isget) {
+			if (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED)
+				*data = P80211ENUM_truth_true;
+			else
+				*data = P80211ENUM_truth_false;
+		} else {
+			wlandev->hostwep &= ~(HOSTWEP_EXCLUDEUNENCRYPTED);
+			if (*data == P80211ENUM_truth_true)
+				wlandev->hostwep |= HOSTWEP_EXCLUDEUNENCRYPTED;
 		}
-	default:
-		;
+	break;
+	}
 	}
-
-	return 0;
 }
diff --git a/drivers/staging/wlan-ng/p80211types.h b/drivers/staging/wlan-ng/p80211types.h
index f043090ef86d..8cb4fc6448a0 100644
--- a/drivers/staging/wlan-ng/p80211types.h
+++ b/drivers/staging/wlan-ng/p80211types.h
@@ -197,7 +197,7 @@
 					P80211DID_LSB_ACCESS)
 
 /*----------------------------------------------------------------*/
-/* The following structure types are used for the represenation */
+/* The following structure types are used for the representation */
 /*  of ENUMint type metadata. */
 
 typedef struct p80211enumpair {
diff --git a/drivers/staging/wlan-ng/p80211wep.c b/drivers/staging/wlan-ng/p80211wep.c
index 80c2d3b672bb..77e50a4aa7e9 100644
--- a/drivers/staging/wlan-ng/p80211wep.c
+++ b/drivers/staging/wlan-ng/p80211wep.c
@@ -134,10 +134,8 @@ int wep_change_key(wlandevice_t *wlandev, int keynum, u8 *key, int keylen)
 		return -1;
 
 #ifdef WEP_DEBUG
-	printk(KERN_DEBUG
-	       "WEP key %d len %d = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-	       keynum, keylen, key[0], key[1], key[2], key[3], key[4], key[5],
-	       key[6], key[7]);
+	printk(KERN_DEBUG "WEP key %d len %d = %*phC\n", keynum, keylen,
+			  8, key);
 #endif
 
 	wlandev->wep_keylens[keynum] = keylen;
@@ -184,10 +182,8 @@ int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override,
 	keylen += 3;		/* add in IV bytes */
 
 #ifdef WEP_DEBUG
-	printk(KERN_DEBUG
-	       "D %d: %02x %02x %02x (%d %d) %02x:%02x:%02x:%02x:%02x\n", len,
-	       key[0], key[1], key[2], keyidx, keylen, key[3], key[4], key[5],
-	       key[6], key[7]);
+	printk(KERN_DEBUG "D %d: %*ph (%d %d) %*phC\n", len, 3, key,
+			  keyidx, keylen, 5, key + 3);
 #endif
 
 	/* set up the RC4 state */
@@ -263,10 +259,8 @@ int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum,
 	keylen += 3;		/* add in IV bytes */
 
 #ifdef WEP_DEBUG
-	printk(KERN_DEBUG
-	       "E %d (%d/%d %d) %02x %02x %02x %02x:%02x:%02x:%02x:%02x\n", len,
-	       iv[3], keynum, keylen, key[0], key[1], key[2], key[3], key[4],
-	       key[5], key[6], key[7]);
+	printk(KERN_DEBUG "E %d (%d/%d %d) %*ph %*phC\n", len,
+			  iv[3], keynum, keylen, 3, key, 5, key + 3);
 #endif
 
 	/* set up the RC4 state */
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 66c9aa972310..0dfd2a4933ef 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -123,27 +123,27 @@ struct imgchunk {
 /* s-record image processing */
 
 /* Data records */
-unsigned int ns3data;
-struct s3datarec s3data[S3DATA_MAX];
+static unsigned int ns3data;
+static struct s3datarec s3data[S3DATA_MAX];
 
 /* Plug records */
-unsigned int ns3plug;
-struct s3plugrec s3plug[S3PLUG_MAX];
+static unsigned int ns3plug;
+static struct s3plugrec s3plug[S3PLUG_MAX];
 
 /* CRC records */
-unsigned int ns3crc;
-struct s3crcrec s3crc[S3CRC_MAX];
+static unsigned int ns3crc;
+static struct s3crcrec s3crc[S3CRC_MAX];
 
 /* Info records */
-unsigned int ns3info;
-struct s3inforec s3info[S3INFO_MAX];
+static unsigned int ns3info;
+static struct s3inforec s3info[S3INFO_MAX];
 
 /* S7 record (there _better_ be only one) */
-u32 startaddr;
+static u32 startaddr;
 
 /* Load image chunks */
-unsigned int nfchunks;
-struct imgchunk fchunk[CHUNKS_MAX];
+static unsigned int nfchunks;
+static struct imgchunk fchunk[CHUNKS_MAX];
 
 /* Note that for the following pdrec_t arrays, the len and code */
 /*   fields are stored in HOST byte order. The mkpdrlist() function */
@@ -151,11 +151,11 @@ struct imgchunk fchunk[CHUNKS_MAX];
 /*----------------------------------------------------------------*/
 /* PDA, built from [card|newfile]+[addfile1+addfile2...] */
 
-struct pda pda;
-hfa384x_compident_t nicid;
-hfa384x_caplevel_t rfid;
-hfa384x_caplevel_t macid;
-hfa384x_caplevel_t priid;
+static struct pda pda;
+static hfa384x_compident_t nicid;
+static hfa384x_caplevel_t rfid;
+static hfa384x_caplevel_t macid;
+static hfa384x_caplevel_t priid;
 
 /*================================================================*/
 /* Local Function Declarations */
@@ -237,7 +237,7 @@ int prism2_fwtry(struct usb_device *udev, wlandevice_t *wlandev)
 *	0	- success
 *	~0	- failure
 ----------------------------------------------------------------*/
-int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev)
+static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev)
 {
 	signed int result = 0;
 	struct p80211msg_dot11req_mibget getmsg;
@@ -376,7 +376,7 @@ int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev)
 *	0	success
 *	~0	failure
 ----------------------------------------------------------------*/
-int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
+static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 	     struct s3crcrec *s3crc, unsigned int ns3crc)
 {
 	int result = 0;
@@ -440,7 +440,7 @@ int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 * Returns:
 *	nothing
 ----------------------------------------------------------------*/
-void free_chunks(struct imgchunk *fchunk, unsigned int *nfchunks)
+static void free_chunks(struct imgchunk *fchunk, unsigned int *nfchunks)
 {
 	int i;
 	for (i = 0; i < *nfchunks; i++)
@@ -462,7 +462,7 @@ void free_chunks(struct imgchunk *fchunk, unsigned int *nfchunks)
 * Returns:
 *	nothing
 ----------------------------------------------------------------*/
-void free_srecs(void)
+static void free_srecs(void)
 {
 	ns3data = 0;
 	memset(s3data, 0, sizeof(s3data));
@@ -489,7 +489,7 @@ void free_srecs(void)
 *	0	- success
 *	~0	- failure (probably an errno)
 ----------------------------------------------------------------*/
-int mkimage(struct imgchunk *clist, unsigned int *ccnt)
+static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 {
 	int result = 0;
 	int i;
@@ -582,7 +582,7 @@ int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 *	0	- success
 *	~0	- failure (probably an errno)
 ----------------------------------------------------------------*/
-int mkpdrlist(struct pda *pda)
+static int mkpdrlist(struct pda *pda)
 {
 	int result = 0;
 	u16 *pda16 = (u16 *) pda->buf;
@@ -656,7 +656,7 @@ int mkpdrlist(struct pda *pda)
 *	0	success
 *	~0	failure
 ----------------------------------------------------------------*/
-int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
+static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 	      struct s3plugrec *s3plug, unsigned int ns3plug, struct pda *pda)
 {
 	int result = 0;
@@ -764,7 +764,7 @@ int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 *	0	- success
 *	~0	- failure (probably an errno)
 ----------------------------------------------------------------*/
-int read_cardpda(struct pda *pda, wlandevice_t *wlandev)
+static int read_cardpda(struct pda *pda, wlandevice_t *wlandev)
 {
 	int result = 0;
 	struct p80211msg_p2req_readpda msg;
@@ -806,7 +806,7 @@ int read_cardpda(struct pda *pda, wlandevice_t *wlandev)
 *
 * Note also that the start address record, originally an S7 record in
 * the srec file, is expected in the fw file to be like a data record but
-* with a certain address to make it identiable.
+* with a certain address to make it identifiable.
 *
 * Here's the SREC format that the fw should have come from:
 * S[37]nnaaaaaaaaddd...dddcc
@@ -854,7 +854,7 @@ int read_cardpda(struct pda *pda, wlandevice_t *wlandev)
 *	0	- success
 *	~0	- failure (probably an errno)
 ----------------------------------------------------------------*/
-int read_fwfile(const struct ihex_binrec *record)
+static int read_fwfile(const struct ihex_binrec *record)
 {
 	int		i;
 	int		rcnt = 0;
@@ -978,7 +978,7 @@ int read_fwfile(const struct ihex_binrec *record)
 *	0	success
 *	~0	failure
 ----------------------------------------------------------------*/
-int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
+static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	       unsigned int nfchunks)
 {
 	int result = 0;
@@ -1130,7 +1130,7 @@ free_result:
 	return result;
 }
 
-int validate_identity(void)
+static int validate_identity(void)
 {
 	int i;
 	int result = 1;
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 1dfd9aa5e9fe..8d2277bb898f 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -242,7 +242,6 @@ static int prism2sta_close(wlandevice_t *wlandev)
 ----------------------------------------------------------------*/
 static void prism2sta_reset(wlandevice_t *wlandev)
 {
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -988,7 +987,6 @@ static void prism2sta_inf_handover(wlandevice_t *wlandev,
 				   hfa384x_InfFrame_t *inf)
 {
 	pr_debug("received infoframe:HANDOVER (unhandled)\n");
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1035,8 +1033,6 @@ static void prism2sta_inf_tallies(wlandevice_t *wlandev,
 		for (i = 0; i < cnt; i++, dst++, src16++)
 			*dst += le16_to_cpu(*src16);
 	}
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1093,8 +1089,6 @@ static void prism2sta_inf_scanresults(wlandevice_t *wlandev,
 		printk(KERN_ERR "setconfig(joinreq) failed, result=%d\n",
 		       result);
 	}
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1194,7 +1188,6 @@ static void prism2sta_inf_chinforesults(wlandevice_t *wlandev,
 	atomic_set(&hw->channel_info.done, 2);
 
 	hw->channel_info.count = n;
-	return;
 }
 
 void prism2sta_processing_defer(struct work_struct *data)
@@ -1218,7 +1211,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 
 	/* Now let's handle the linkstatus stuff */
 	if (hw->link_status == hw->link_status_new)
-		goto failed;
+		return;
 
 	hw->link_status = hw->link_status_new;
 
@@ -1272,7 +1265,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 				pr_debug
 				    ("getconfig(0x%02x) failed, result = %d\n",
 				     HFA384x_RID_CURRENTBSSID, result);
-				goto failed;
+				return;
 			}
 
 			result = hfa384x_drvr_getconfig(hw,
@@ -1282,7 +1275,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 				pr_debug
 				    ("getconfig(0x%02x) failed, result = %d\n",
 				     HFA384x_RID_CURRENTSSID, result);
-				goto failed;
+				return;
 			}
 			prism2mgmt_bytestr2pstr((hfa384x_bytestr_t *) &ssid,
 						(p80211pstrd_t *) &
@@ -1296,7 +1289,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 				pr_debug
 				    ("getconfig(0x%02x) failed, result = %d\n",
 				     HFA384x_RID_PORTSTATUS, result);
-				goto failed;
+				return;
 			}
 			wlandev->macmode =
 			    (portstatus == HFA384x_PSTATUS_CONN_IBSS) ?
@@ -1355,7 +1348,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 		if (result) {
 			pr_debug("getconfig(0x%02x) failed, result = %d\n",
 				 HFA384x_RID_CURRENTBSSID, result);
-			goto failed;
+			return;
 		}
 
 		result = hfa384x_drvr_getconfig(hw,
@@ -1364,7 +1357,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 		if (result) {
 			pr_debug("getconfig(0x%02x) failed, result = %d\n",
 				 HFA384x_RID_CURRENTSSID, result);
-			goto failed;
+			return;
 		}
 		prism2mgmt_bytestr2pstr((hfa384x_bytestr_t *) &ssid,
 					(p80211pstrd_t *) &wlandev->ssid);
@@ -1443,14 +1436,10 @@ void prism2sta_processing_defer(struct work_struct *data)
 		/* This is bad, IO port problems? */
 		printk(KERN_WARNING
 		       "unknown linkstatus=0x%02x\n", hw->link_status);
-		goto failed;
-		break;
+		return;
 	}
 
 	wlandev->linkstatus = (hw->link_status == HFA384x_LINK_CONNECTED);
-
-failed:
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1478,8 +1467,6 @@ static void prism2sta_inf_linkstatus(wlandevice_t *wlandev,
 	hw->link_status_new = le16_to_cpu(inf->info.linkstatus.linkstatus);
 
 	schedule_work(&hw->link_bh);
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1540,8 +1527,6 @@ static void prism2sta_inf_assocstatus(wlandevice_t *wlandev,
 			printk(KERN_WARNING
 "authfail assocstatus info frame received for authenticated station.\n");
 	}
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1731,7 +1716,6 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 		       "setconfig(authenticatestation) failed, result=%d\n",
 		       result);
 	}
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1758,8 +1742,6 @@ static void prism2sta_inf_psusercnt(wlandevice_t *wlandev,
 	hfa384x_t *hw = (hfa384x_t *) wlandev->priv;
 
 	hw->psusercount = le16_to_cpu(inf->info.psusercnt.usercnt);
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1825,7 +1807,6 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 		       "Unknown info type=0x%02x\n", inf->infotype);
 		break;
 	}
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1850,8 +1831,6 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 void prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status)
 {
 	pr_debug("TxExc status=0x%x.\n", status);
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1875,7 +1854,6 @@ void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status)
 	pr_debug("Tx Complete, status=0x%04x\n", status);
 	/* update linux network stats */
 	wlandev->linux_stats.tx_packets++;
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1897,7 +1875,6 @@ void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status)
 void prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 {
 	p80211netdev_rx(wlandev, skb);
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1919,7 +1896,6 @@ void prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 void prism2sta_ev_alloc(wlandevice_t *wlandev)
 {
 	netif_wake_queue(wlandev->netdev);
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -1988,12 +1964,12 @@ void prism2sta_commsqual_defer(struct work_struct *data)
 	int result = 0;
 
 	if (hw->wlandev->hwremoved)
-		goto done;
+		return;
 
 	/* we don't care if we're in AP mode */
 	if ((wlandev->macmode == WLAN_MACMODE_NONE) ||
 	    (wlandev->macmode == WLAN_MACMODE_ESS_AP)) {
-		goto done;
+		return;
 	}
 
 	/* It only makes sense to poll these in non-IBSS */
@@ -2004,7 +1980,7 @@ void prism2sta_commsqual_defer(struct work_struct *data)
 
 		if (result) {
 			printk(KERN_ERR "error fetching commsqual\n");
-			goto done;
+			return;
 		}
 
 		pr_debug("commsqual %d %d %d\n",
@@ -2021,7 +1997,7 @@ void prism2sta_commsqual_defer(struct work_struct *data)
 	if (result) {
 		pr_debug("get signal rate failed, result = %d\n",
 			 result);
-		goto done;
+		return;
 	}
 
 	switch (mibitem->data) {
@@ -2048,7 +2024,7 @@ void prism2sta_commsqual_defer(struct work_struct *data)
 	if (result) {
 		pr_debug("getconfig(0x%02x) failed, result = %d\n",
 			 HFA384x_RID_CURRENTBSSID, result);
-		goto done;
+		return;
 	}
 
 	result = hfa384x_drvr_getconfig(hw,
@@ -2057,16 +2033,13 @@ void prism2sta_commsqual_defer(struct work_struct *data)
 	if (result) {
 		pr_debug("getconfig(0x%02x) failed, result = %d\n",
 			 HFA384x_RID_CURRENTSSID, result);
-		goto done;
+		return;
 	}
 	prism2mgmt_bytestr2pstr((hfa384x_bytestr_t *) &ssid,
 				(p80211pstrd_t *) &wlandev->ssid);
 
 	/* Reschedule timer */
 	mod_timer(&hw->commsqual_timer, jiffies + HZ);
-
-done:
-	;
 }
 
 void prism2sta_commsqual_timer(unsigned long data)
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 64ffd70eb7dc..f775c5453845 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -6,6 +6,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/sizes.h>
 #include <linux/module.h>
 
 #ifdef CONFIG_MTRR
@@ -329,6 +330,7 @@ static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 {
 	u16 xres, yres;
 	struct xgi_hw_device_info *hw_info = &xgifb_info->hw_info;
+	unsigned long required_mem;
 
 	if (xgifb_info->chip == XG21) {
 		if (xgifb_info->display2 == XGIFB_DISP_LCD) {
@@ -345,13 +347,13 @@ static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 			}
 
 		}
-		return myindex;
+		goto check_memory;
 
 	}
 
 	/* FIXME: for now, all is valid on XG27 */
 	if (xgifb_info->chip == XG27)
-		return myindex;
+		goto check_memory;
 
 	if (!(XGIbios_mode[myindex].chipset & MD_XGI315))
 		return -1;
@@ -539,6 +541,12 @@ static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 	case XGIFB_DISP_NONE:
 		break;
 	}
+
+check_memory:
+	required_mem = XGIbios_mode[myindex].xres * XGIbios_mode[myindex].yres *
+		       XGIbios_mode[myindex].bpp / 8;
+	if (required_mem > xgifb_info->video_size)
+		return -1;
 	return myindex;
 
 }
@@ -913,17 +921,10 @@ static void XGIfb_post_setmode(struct xgifb_video_info *xgifb_info)
 			}
 
 			if ((filter >= 0) && (filter <= 7)) {
-				pr_debug("FilterTable[%d]-%d: %02x %02x %02x %02x\n",
+				pr_debug("FilterTable[%d]-%d: %*ph\n",
 					 filter_tb, filter,
-					 XGI_TV_filter[filter_tb].
-						filter[filter][0],
-					 XGI_TV_filter[filter_tb].
-						filter[filter][1],
-					 XGI_TV_filter[filter_tb].
-						filter[filter][2],
-					 XGI_TV_filter[filter_tb].
-						filter[filter][3]
-				);
+					 4, XGI_TV_filter[filter_tb].
+						   filter[filter]);
 				xgifb_reg_set(
 					XGIPART2,
 					0x35,
@@ -1404,11 +1405,10 @@ static int XGIfb_pan_display(struct fb_var_screeninfo *var,
 		if (var->yoffset < 0 || var->yoffset >= info->var.yres_virtual
 				|| var->xoffset)
 			return -EINVAL;
-	} else {
-		if (var->xoffset + info->var.xres > info->var.xres_virtual
+	} else if (var->xoffset + info->var.xres > info->var.xres_virtual
 				|| var->yoffset + info->var.yres
-						> info->var.yres_virtual)
-			return -EINVAL;
+						> info->var.yres_virtual) {
+		return -EINVAL;
 	}
 	err = XGIfb_pan_var(var, info);
 	if (err < 0)
@@ -1471,6 +1471,9 @@ static int XGIfb_get_dram_size(struct xgifb_video_info *xgifb_info)
 		xgifb_reg_set(XGISR, IND_SIS_DRAM_SIZE, 0x51);
 
 	reg = xgifb_reg_get(XGISR, IND_SIS_DRAM_SIZE);
+	if (!reg)
+		return -1;
+
 	switch ((reg & XGI_DRAM_SIZE_MASK) >> 4) {
 	case XGI_DRAM_SIZE_1MB:
 		xgifb_info->video_size = 0x100000;
@@ -1701,6 +1704,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	struct fb_info *fb_info;
 	struct xgifb_video_info *xgifb_info;
 	struct xgi_hw_device_info *hw_info;
+	unsigned long video_size_max;
 
 	fb_info = framebuffer_alloc(sizeof(*xgifb_info), &pdev->dev);
 	if (!fb_info)
@@ -1721,6 +1725,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->subsysvendor = pdev->subsystem_vendor;
 	xgifb_info->subsysdevice = pdev->subsystem_device;
 
+	video_size_max = pci_resource_len(pdev, 0);
 	xgifb_info->video_base = pci_resource_start(pdev, 0);
 	xgifb_info->mmio_base = pci_resource_start(pdev, 1);
 	xgifb_info->mmio_size = pci_resource_len(pdev, 1);
@@ -1777,10 +1782,10 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	hw_info->jChipType = xgifb_info->chip;
 
 	if (XGIfb_get_dram_size(xgifb_info)) {
-		dev_err(&pdev->dev,
-			"Fatal error: Unable to determine RAM size.\n");
-		ret = -ENODEV;
-		goto error_disable;
+		xgifb_info->video_size = min_t(unsigned long, video_size_max,
+						SZ_16M);
+	} else if (xgifb_info->video_size > video_size_max) {
+		xgifb_info->video_size = video_size_max;
 	}
 
 	/* Enable PCI_LINEAR_ADDRESSING and MMIO_ENABLE  */
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index 69078d933a47..77137e4452a0 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -3,9 +3,7 @@
 #include "../../video/sis/initdef.h"
 
 #define VB_XGI301C      0x0020 /* for 301C */
-#define VB_YPbPr1080i   0x03
 
-#define LVDSCRT1Len     15
 #define SupportCRT2in301C       0x0100  /* for 301C */
 #define SetCHTVOverScan         0x8000
 
@@ -22,15 +20,6 @@
 
 #define XGI_CRT2_PORT_00        (0x00 - 0x030)
 
-/* =============================================================
-   for 310
-============================================================== */
-#define ModeSoftSetting              0x04
-
-/* ---------------- SetMode Stack */
-#define CRT1Len           15
-#define VCLKLen           4
-
 #define SupportAllCRT2      0x0078
 #define NoSupportTV         0x0070
 #define NoSupportHiVisionTV 0x0060
@@ -115,16 +104,6 @@
 #define ActiveHiTV            0x08
 #define ActiveYPbPr           0x10
 
-/* --------------------------------------------------------- */
-/* translated from asm code 301def.h */
-/*  */
-/* --------------------------------------------------------- */
-#define LVDSCRT1Len_H        8
-#define LVDSCRT1Len_V        7
-#define LCDDesDataLen        6
-#define LVDSDesDataLen2      8
-#define LCDDesDataLen2       8
-
 #define NTSC1024x768HT       1908
 
 #define YPbPrTV525iHT        1716 /* YPbPr */
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index 80dba6a425ba..7739dbd9f029 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -1269,7 +1269,7 @@ static unsigned char GetXG27FPBits(struct vb_device_info *pVBInfo)
 	if (temp <= 2)
 		temp &= 0x03;
 	else
-		temp = ((temp & 0x04) >> 1) || ((~temp) & 0x01);
+		temp = ((temp & 0x04) >> 1) | ((~temp) & 0x01);
 
 	xgifb_reg_set(pVBInfo->P3d4, 0x4A, CR4A);
 
@@ -1299,8 +1299,6 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	outb(0x67, (pVBInfo->BaseAddr + 0x12)); /* 3c2 <- 67 ,ynlai */
 
-	pVBInfo->ISXPDOS = 0;
-
 	pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14;
 	pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24;
 	pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10;
@@ -1494,7 +1492,6 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	XGINew_SetModeScratch(HwDeviceExtension, pVBInfo);
 
 	xgifb_reg_set(pVBInfo->P3d4, 0x8c, 0x87);
-	xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x31);
 
 	return 1;
 } /* end of init */
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index e81149fc66e3..e95a1655a6ce 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -23,20 +23,18 @@ static const unsigned short XGINew_VGA_DAC[] = {
 
 void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 {
-	pVBInfo->StandTable = (struct SiS_StandTable_S *) &XGI330_StandTable;
-	pVBInfo->EModeIDTable = (struct XGI_ExtStruct *) XGI330_EModeIDTable;
-	pVBInfo->RefIndex = (struct XGI_Ext2Struct *) XGI330_RefIndex;
-	pVBInfo->XGINEWUB_CRT1Table
-			= (struct XGI_CRT1TableStruct *) XGI_CRT1Table;
-
-	pVBInfo->MCLKData = (struct SiS_MCLKData *) XGI340New_MCLKData;
-	pVBInfo->ECLKData = (struct XGI_ECLKDataStruct *) XGI340_ECLKData;
-	pVBInfo->VCLKData = (struct SiS_VCLKData *) XGI_VCLKData;
-	pVBInfo->VBVCLKData = (struct SiS_VBVCLKData *) XGI_VBVCLKData;
+	pVBInfo->StandTable = &XGI330_StandTable;
+	pVBInfo->EModeIDTable = XGI330_EModeIDTable;
+	pVBInfo->RefIndex = XGI330_RefIndex;
+	pVBInfo->XGINEWUB_CRT1Table = XGI_CRT1Table;
+
+	pVBInfo->MCLKData = XGI340New_MCLKData;
+	pVBInfo->ECLKData = XGI340_ECLKData;
+	pVBInfo->VCLKData = XGI_VCLKData;
+	pVBInfo->VBVCLKData = XGI_VBVCLKData;
 	pVBInfo->ScreenOffset = XGI330_ScreenOffset;
-	pVBInfo->StResInfo = (struct SiS_StResInfo_S *) XGI330_StResInfo;
-	pVBInfo->ModeResInfo
-			= (struct SiS_ModeResInfo_S *) XGI330_ModeResInfo;
+	pVBInfo->StResInfo = XGI330_StResInfo;
+	pVBInfo->ModeResInfo = XGI330_ModeResInfo;
 
 	pVBInfo->LCDResInfo = 0;
 	pVBInfo->LCDTypeInfo = 0;
@@ -56,24 +54,9 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	pVBInfo->SR21 = 0xa3;
 	pVBInfo->SR22 = 0xfb;
 
-	pVBInfo->NTSCTiming = XGI330_NTSCTiming;
-	pVBInfo->PALTiming = XGI330_PALTiming;
-	pVBInfo->HiTVExtTiming = XGI330_HiTVExtTiming;
-	pVBInfo->HiTVSt1Timing = XGI330_HiTVSt1Timing;
-	pVBInfo->HiTVSt2Timing = XGI330_HiTVSt2Timing;
-	pVBInfo->HiTVTextTiming = XGI330_HiTVTextTiming;
-	pVBInfo->YPbPr750pTiming = XGI330_YPbPr750pTiming;
-	pVBInfo->YPbPr525pTiming = XGI330_YPbPr525pTiming;
-	pVBInfo->YPbPr525iTiming = XGI330_YPbPr525iTiming;
-	pVBInfo->HiTVGroup3Data = XGI330_HiTVGroup3Data;
-	pVBInfo->HiTVGroup3Simu = XGI330_HiTVGroup3Simu;
-	pVBInfo->HiTVGroup3Text = XGI330_HiTVGroup3Text;
-	pVBInfo->Ren525pGroup3 = XGI330_Ren525pGroup3;
-	pVBInfo->Ren750pGroup3 = XGI330_Ren750pGroup3;
-
-	pVBInfo->TimingH = (struct XGI_TimingHStruct *) XGI_TimingH;
-	pVBInfo->TimingV = (struct XGI_TimingVStruct *) XGI_TimingV;
-	pVBInfo->UpdateCRT1 = (struct XGI_XG21CRT1Struct *) XGI_UpdateCRT1Table;
+	pVBInfo->TimingH = XGI_TimingH;
+	pVBInfo->TimingV = XGI_TimingV;
+	pVBInfo->UpdateCRT1 = XGI_UpdateCRT1Table;
 
 	/* 310 customization related */
 	if ((pVBInfo->VBType & VB_SIS301LV) || (pVBInfo->VBType & VB_SIS302LV))
@@ -86,8 +69,7 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 
 	if (ChipType == XG27) {
 		unsigned char temp;
-		pVBInfo->MCLKData
-			= (struct SiS_MCLKData *) XGI27New_MCLKData;
+		pVBInfo->MCLKData = XGI27New_MCLKData;
 		pVBInfo->CR40 = XGI27_cr41;
 		pVBInfo->XGINew_CR97 = 0xc1;
 		pVBInfo->SR15 = XG27_SR13;
@@ -116,11 +98,9 @@ static void XGI_SetSeqRegs(unsigned short ModeNo,
 	i = XGI_SetCRT2ToLCDA;
 	if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
 		tempah |= 0x01;
-	} else {
-		if (pVBInfo->VBInfo & (SetCRT2ToTV | SetCRT2ToLCD)) {
-			if (pVBInfo->VBInfo & SetInSlaveMode)
-				tempah |= 0x01;
-		}
+	} else if (pVBInfo->VBInfo & (SetCRT2ToTV | SetCRT2ToLCD)) {
+		if (pVBInfo->VBInfo & SetInSlaveMode)
+			tempah |= 0x01;
 	}
 
 	tempah |= 0x20; /* screen off */
@@ -165,10 +145,9 @@ static void XGI_SetATTRegs(unsigned short ModeNo,
 		if ((modeflag & Charx8Dot) && i == 0x13) { /* ifndef Dot9 */
 			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
 				ARdata = 0;
-			} else {
-				if ((pVBInfo->VBInfo &
+			} else if ((pVBInfo->VBInfo &
 				     (SetCRT2ToTV | SetCRT2ToLCD)) &&
-				    (pVBInfo->VBInfo & SetInSlaveMode))
+				    (pVBInfo->VBInfo & SetInSlaveMode)) {
 					ARdata = 0;
 			}
 		}
@@ -258,59 +237,45 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeNo,
 		}
 
 		if (pVBInfo->VBInfo & SetCRT2ToHiVision) { /* for HiTV */
-			if ((pVBInfo->VBType & VB_SIS301LV) &&
-			    (pVBInfo->VBExtInfo == VB_YPbPr1080i)) {
-				tempax |= SupportYPbPr750p;
-				if ((pVBInfo->VBInfo & SetInSlaveMode) &&
-				    ((resinfo == 3) ||
-				     (resinfo == 4) ||
-				     (resinfo > 7)))
+			tempax |= SupportHiVision;
+			if ((pVBInfo->VBInfo & SetInSlaveMode) &&
+			    ((resinfo == 4) ||
+			     (resinfo == 3 &&
+			      (pVBInfo->SetFlag & TVSimuMode)) ||
+			     (resinfo > 7)))
 					return 0;
-			} else {
-				tempax |= SupportHiVision;
-				if ((pVBInfo->VBInfo & SetInSlaveMode) &&
-				    ((resinfo == 4) ||
-				     (resinfo == 3 &&
-				      (pVBInfo->SetFlag & TVSimuMode)) ||
-				     (resinfo > 7)))
-						return 0;
-			}
-		} else {
-			if (pVBInfo->VBInfo & (SetCRT2ToAVIDEO |
+		} else if (pVBInfo->VBInfo & (SetCRT2ToAVIDEO |
 					       SetCRT2ToSVIDEO |
 					       SetCRT2ToSCART |
 					       SetCRT2ToYPbPr525750 |
 					       SetCRT2ToHiVision)) {
-				tempax |= SupportTV;
-
-				if (pVBInfo->VBType & (VB_SIS301B |
-						       VB_SIS302B |
-						       VB_SIS301LV |
-						       VB_SIS302LV |
-						       VB_XGI301C))
-					tempax |= SupportTV1024;
-
-				if (!(pVBInfo->VBInfo & TVSetPAL) &&
-				    (modeflag & NoSupportSimuTV) &&
-				    (pVBInfo->VBInfo & SetInSlaveMode) &&
-				    (!(pVBInfo->VBInfo & SetNotSimuMode)))
-					return 0;
-			}
+			tempax |= SupportTV;
+
+			if (pVBInfo->VBType & (VB_SIS301B |
+					       VB_SIS302B |
+					       VB_SIS301LV |
+					       VB_SIS302LV |
+					       VB_XGI301C))
+				tempax |= SupportTV1024;
+
+			if (!(pVBInfo->VBInfo & TVSetPAL) &&
+			    (modeflag & NoSupportSimuTV) &&
+			    (pVBInfo->VBInfo & SetInSlaveMode) &&
+			    (!(pVBInfo->VBInfo & SetNotSimuMode)))
+				return 0;
 		}
-	} else { /* for LVDS */
-		if (pVBInfo->VBInfo & SetCRT2ToLCD) {
-			tempax |= SupportLCD;
+	} else if (pVBInfo->VBInfo & SetCRT2ToLCD) { /* for LVDS */
+		tempax |= SupportLCD;
 
-			if (resinfo > 0x08)
-				return 0; /* 1024x768 */
+		if (resinfo > 0x08)
+			return 0; /* 1024x768 */
 
-			if (pVBInfo->LCDResInfo < Panel_1024x768) {
-				if (resinfo > 0x07)
-					return 0; /* 800x600 */
+		if (pVBInfo->LCDResInfo < Panel_1024x768) {
+			if (resinfo > 0x07)
+				return 0; /* 800x600 */
 
-				if (resinfo == 0x04)
-					return 0; /* 512x384 */
-			}
+			if (resinfo == 0x04)
+				return 0; /* 512x384 */
 		}
 	}
 
@@ -969,13 +934,8 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 			}
 
 			/* 301lv */
-			if ((pVBInfo->VBType & VB_SIS301LV) &&
-			    !(pVBInfo->VBExtInfo == VB_YPbPr1080i)) {
-				if (pVBInfo->VBExtInfo == YPbPr750p)
-					VCLKIndex = XGI_YPbPr750pVCLK;
-				else if (pVBInfo->VBExtInfo == YPbPr525p)
-					VCLKIndex = YPbPr525pVCLK;
-				else if (pVBInfo->SetFlag & RPLLDIV2XO)
+			if (pVBInfo->VBType & VB_SIS301LV) {
+				if (pVBInfo->SetFlag & RPLLDIV2XO)
 					VCLKIndex = YPbPr525iVCLK_2;
 				else
 					VCLKIndex = YPbPr525iVCLK;
@@ -991,13 +951,11 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 								Ext_CRTVCLK;
 			VCLKIndex &= IndexMask;
 		}
-	} else { /* LVDS */
-		if ((pVBInfo->LCDResInfo == Panel_800x600) ||
-		    (pVBInfo->LCDResInfo == Panel_320x480))
-			VCLKIndex = VCLK40; /* LVDSXlat1VCLK */
-		else
-			VCLKIndex = VCLK65_315 + 2; /* LVDSXlat2VCLK,
-						       LVDSXlat3VCLK  */
+	} else if ((pVBInfo->LCDResInfo == Panel_800x600) ||
+		   (pVBInfo->LCDResInfo == Panel_320x480)) { /* LVDS */
+		VCLKIndex = VCLK40; /* LVDSXlat1VCLK */
+	} else {
+		VCLKIndex = VCLK65_315 + 2; /* LVDSXlat2VCLK, LVDSXlat3VCLK */
 	}
 
 	return VCLKIndex;
@@ -1352,7 +1310,7 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 		unsigned short RefreshRateTableIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short i, tempdx, tempcx, tempbx, tempal, modeflag, table;
+	unsigned short i, tempdx, tempbx, tempal, modeflag, table;
 
 	struct XGI330_LCDDataTablStruct *tempdi = NULL;
 
@@ -1377,15 +1335,6 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 		tempal = (tempal & 0x0f);
 	}
 
-	tempcx = LCDLenList[tempbx];
-
-	if (pVBInfo->LCDInfo & EnableScalingLCD) { /* ScaleLCD */
-		if ((tempbx == 5) || (tempbx) == 7)
-			tempcx = LCDDesDataLen2;
-		else if ((tempbx == 3) || (tempbx == 8))
-			tempcx = LVDSDesDataLen2;
-	}
-
 	switch (tempbx) {
 	case 0:
 	case 1:
@@ -1403,14 +1352,6 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 	case 5:
 		tempdi = XGI_LCDDesDataTable;
 		break;
-	case 6:
-		tempdi = XGI_EPLCHLCDRegPtr;
-		break;
-	case 7:
-	case 8:
-	case 9:
-		tempdi = NULL;
-		break;
 	default:
 		break;
 	}
@@ -1764,62 +1705,20 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 		default:
 			break;
 		}
-	} else if (table == 6) {
-		switch (tempdi[i].DATAPTR) {
-		case 0:
-			return &XGI_CH7017LV1024x768[tempal];
-			break;
-		case 1:
-			return &XGI_CH7017LV1400x1050[tempal];
-			break;
-		default:
-			break;
-		}
 	}
 	return NULL;
 }
 
-static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
+static struct SiS_TVData const *XGI_GetTVPtr(unsigned short ModeNo,
 		unsigned short ModeIdIndex,
 		unsigned short RefreshRateTableIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short i, tempdx, tempbx, tempal, modeflag, table;
-	struct XGI330_TVDataTablStruct *tempdi = NULL;
+	unsigned short i, tempdx, tempal, modeflag;
 
-	tempbx = BX;
 	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 	tempal = tempal & 0x3f;
-	table = tempbx;
-
-	switch (tempbx) {
-	case 0:
-		tempdi = NULL;
-		break;
-	case 1:
-		tempdi = NULL;
-		break;
-	case 2:
-	case 6:
-		tempdi = xgifb_chrontel_tv;
-		break;
-	case 3:
-		tempdi = NULL;
-		break;
-	case 4:
-		tempdi = XGI_TVDataTable;
-		break;
-	case 5:
-		tempdi = NULL;
-		break;
-	default:
-		break;
-	}
-
-	if (tempdi == NULL) /* OEMUtil */
-		return NULL;
-
 	tempdx = pVBInfo->TVInfo;
 
 	if (pVBInfo->VBInfo & SetInSlaveMode)
@@ -1830,78 +1729,14 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 
 	i = 0;
 
-	while (tempdi[i].MASK != 0xffff) {
-		if ((tempdx & tempdi[i].MASK) == tempdi[i].CAP)
+	while (XGI_TVDataTable[i].MASK != 0xffff) {
+		if ((tempdx & XGI_TVDataTable[i].MASK) ==
+			XGI_TVDataTable[i].CAP)
 			break;
 		i++;
 	}
 
-	if (table == 0x04) {
-		switch (tempdi[i].DATAPTR) {
-		case 0:
-			return &XGI_ExtPALData[tempal];
-			break;
-		case 1:
-			return &XGI_ExtNTSCData[tempal];
-			break;
-		case 2:
-			return &XGI_StPALData[tempal];
-			break;
-		case 3:
-			return &XGI_StNTSCData[tempal];
-			break;
-		case 4:
-			return &XGI_ExtHiTVData[tempal];
-			break;
-		case 5:
-			return &XGI_St2HiTVData[tempal];
-			break;
-		case 6:
-			return &XGI_ExtYPbPr525iData[tempal];
-			break;
-		case 7:
-			return &XGI_ExtYPbPr525pData[tempal];
-			break;
-		case 8:
-			return &XGI_ExtYPbPr750pData[tempal];
-			break;
-		case 9:
-			return &XGI_StYPbPr525iData[tempal];
-			break;
-		case 10:
-			return &XGI_StYPbPr525pData[tempal];
-			break;
-		case 11:
-			return &XGI_StYPbPr750pData[tempal];
-			break;
-		case 12: /* avoid system hang */
-			return &XGI_ExtNTSCData[tempal];
-			break;
-		case 13:
-			return &XGI_St1HiTVData[tempal];
-			break;
-		default:
-			break;
-		}
-	} else if (table == 0x02) {
-		switch (tempdi[i].DATAPTR) {
-		case 0:
-			return &XGI_CHTVUNTSCData[tempal];
-			break;
-		case 1:
-			return &XGI_CHTVONTSCData[tempal];
-			break;
-		case 2:
-			return &XGI_CHTVUPALData[tempal];
-			break;
-		case 3:
-			return &XGI_CHTVOPALData[tempal];
-			break;
-		default:
-			break;
-		}
-	}
-	return NULL;
+	return &XGI_TVDataTable[i].DATAPTR[tempal];
 }
 
 static void XGI_GetLVDSData(unsigned short ModeNo, unsigned short ModeIdIndex,
@@ -1914,9 +1749,8 @@ static void XGI_GetLVDSData(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempbx = 2;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-		LCDPtr = (struct SiS_LVDSData *)XGI_GetLcdPtr(tempbx,
-				ModeNo, ModeIdIndex, RefreshRateTableIndex,
-				pVBInfo);
+		LCDPtr = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+				       RefreshRateTableIndex, pVBInfo);
 		pVBInfo->VGAHT = LCDPtr->VGAHT;
 		pVBInfo->VGAVT = LCDPtr->VGAVT;
 		pVBInfo->HT = LCDPtr->LCDHT;
@@ -1962,11 +1796,8 @@ static void XGI_ModCRT1Regs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempbx = 0;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-		LCDPtr = (struct XGI_LVDSCRT1HDataStruct *)
-				XGI_GetLcdPtr(tempbx, ModeNo,
-					      ModeIdIndex,
-					      RefreshRateTableIndex,
-					      pVBInfo);
+		LCDPtr = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+				       RefreshRateTableIndex, pVBInfo);
 
 		for (i = 0; i < 8; i++)
 			pVBInfo->TimingH[0].data[i] = LCDPtr[0].Reg[i];
@@ -1977,13 +1808,8 @@ static void XGI_ModCRT1Regs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempbx = 1;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-		LCDPtr1 = (struct XGI_LVDSCRT1VDataStruct *)
-				XGI_GetLcdPtr(
-					tempbx,
-					ModeNo,
-					ModeIdIndex,
-					RefreshRateTableIndex,
-					pVBInfo);
+		LCDPtr1 = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+					RefreshRateTableIndex, pVBInfo);
 		for (i = 0; i < 7; i++)
 			pVBInfo->TimingV[0].data[i] = LCDPtr1[0].Reg[i];
 	}
@@ -2075,23 +1901,11 @@ static void XGI_SetLVDSRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	tempbx = 3;
 	if (pVBInfo->LCDInfo & EnableScalingLCD)
-		LCDPtr1 =
-		    (struct XGI330_LCDDataDesStruct2 *)
-				XGI_GetLcdPtr(
-					  tempbx,
-					  ModeNo,
-					  ModeIdIndex,
-					  RefreshRateTableIndex,
-					  pVBInfo);
+		LCDPtr1 = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+					RefreshRateTableIndex, pVBInfo);
 	else
-		LCDPtr =
-		    (struct XGI_LCDDesStruct *)
-				XGI_GetLcdPtr(
-					  tempbx,
-					  ModeNo,
-					  ModeIdIndex,
-					  RefreshRateTableIndex,
-					  pVBInfo);
+		LCDPtr = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+				       RefreshRateTableIndex, pVBInfo);
 
 	XGI_GetLCDSync(&tempax, &tempbx, pVBInfo);
 	push1 = tempbx;
@@ -2438,8 +2252,8 @@ static void XGI_GetVCLKLen(unsigned char tempal, unsigned char *di_0,
 			| VB_SIS301LV | VB_SIS302LV | VB_XGI301C)) {
 		if ((!(pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)) &&
 		    (pVBInfo->SetFlag & ProgrammingCRT2)) {
-			*di_0 = (unsigned char) XGI_VBVCLKData[tempal].SR2B;
-			*di_1 = XGI_VBVCLKData[tempal].SR2C;
+			*di_0 = XGI_VBVCLKData[tempal].Part4_A;
+			*di_1 = XGI_VBVCLKData[tempal].Part4_B;
 		}
 	} else {
 		*di_0 = XGI_VCLKData[tempal].SR2B;
@@ -2634,21 +2448,16 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	temp = xgifb_reg_get(pVBInfo->P3d4, 0x38);
 
-	if (pVBInfo->IF_DEF_LCDA == 1) {
-
-		if (((HwDeviceExtension->jChipType >= XG20) ||
-		     (HwDeviceExtension->jChipType >= XG40)) &&
-		    (pVBInfo->IF_DEF_LVDS == 0)) {
-			if (pVBInfo->VBType &
-			    (VB_SIS302B |
-			     VB_SIS301LV |
-			     VB_SIS302LV |
-			     VB_XGI301C)) {
-				if (temp & EnableDualEdge) {
-					tempbx |= SetCRT2ToDualEdge;
-					if (temp & SetToLCDA)
-						tempbx |= XGI_SetCRT2ToLCDA;
-				}
+	if (pVBInfo->IF_DEF_LVDS == 0) {
+		if (pVBInfo->VBType &
+		    (VB_SIS302B |
+		     VB_SIS301LV |
+		     VB_SIS302LV |
+		     VB_XGI301C)) {
+			if (temp & EnableDualEdge) {
+				tempbx |= SetCRT2ToDualEdge;
+				if (temp & SetToLCDA)
+					tempbx |= XGI_SetCRT2ToLCDA;
 			}
 		}
 	}
@@ -2687,11 +2496,10 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 				temp = 0x09FC;
 			else
 				temp = 0x097C;
+		} else if (pVBInfo->IF_DEF_HiVision == 1) {
+			temp = 0x01FC;
 		} else {
-			if (pVBInfo->IF_DEF_HiVision == 1)
-				temp = 0x01FC;
-			else
-				temp = 0x017C;
+			temp = 0x017C;
 		}
 	} else { /* 3nd party chip */
 		temp = SetCRT2ToLCD;
@@ -2702,19 +2510,17 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 		tempbx = 0;
 	}
 
-	if (pVBInfo->IF_DEF_LCDA == 1) { /* Select Display Device */
-		if (!(pVBInfo->VBType & VB_NoLCD)) {
-			if (tempbx & XGI_SetCRT2ToLCDA) {
-				if (tempbx & SetSimuScanMode)
-					tempbx &= (~(SetCRT2ToLCD |
-						     SetCRT2ToRAMDAC |
-						     SwitchCRT2));
-				else
-					tempbx &= (~(SetCRT2ToLCD |
-						     SetCRT2ToRAMDAC |
-						     SetCRT2ToTV |
-						     SwitchCRT2));
-			}
+	if (!(pVBInfo->VBType & VB_NoLCD)) {
+		if (tempbx & XGI_SetCRT2ToLCDA) {
+			if (tempbx & SetSimuScanMode)
+				tempbx &= (~(SetCRT2ToLCD |
+					     SetCRT2ToRAMDAC |
+					     SwitchCRT2));
+			else
+				tempbx &= (~(SetCRT2ToLCD |
+					     SetCRT2ToRAMDAC |
+					     SetCRT2ToTV |
+					     SwitchCRT2));
 		}
 	}
 
@@ -2777,11 +2583,9 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 	if (!(tempbx & DisableCRT2Display)) {
 		if ((!(tempbx & DriverMode)) ||
 		    (!(modeflag & CRT2Mode))) {
-			if (pVBInfo->IF_DEF_LCDA == 1) {
-				if (!(tempbx & XGI_SetCRT2ToLCDA))
-					tempbx |= (SetInSlaveMode |
-						   SetSimuScanMode);
-			}
+			if (!(tempbx & XGI_SetCRT2ToLCDA))
+				tempbx |= (SetInSlaveMode |
+					   SetSimuScanMode);
 		}
 
 		/* LCD+TV can't support in slave mode
@@ -2867,19 +2671,17 @@ static void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 			if (pVBInfo->VBInfo & SetCRT2ToHiVision) {
 				if (pVBInfo->VBInfo & SetInSlaveMode)
 					tempbx &= (~RPLLDIV2XO);
-			} else {
-				if (tempbx &
-				    (TVSetYPbPr525p | TVSetYPbPr750p))
+			} else if (tempbx &
+				    (TVSetYPbPr525p | TVSetYPbPr750p)) {
 					tempbx &= (~RPLLDIV2XO);
-				else if (!(pVBInfo->VBType &
+			} else if (!(pVBInfo->VBType &
 					 (VB_SIS301B |
 					  VB_SIS302B |
 					  VB_SIS301LV |
 					  VB_SIS302LV |
 					  VB_XGI301C))) {
-					if (tempbx & TVSimuMode)
-						tempbx &= (~RPLLDIV2XO);
-				}
+				if (tempbx & TVSimuMode)
+					tempbx &= (~RPLLDIV2XO);
 			}
 		}
 	}
@@ -2960,20 +2762,6 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 			tempbx |= SetLCDtoNonExpanding;
 	}
 
-	if (pVBInfo->IF_DEF_ExpLink == 1) {
-		if (modeflag & HalfDCLK) {
-			if (!(tempbx & SetLCDtoNonExpanding)) {
-				tempbx |= XGI_EnableLVDSDDA;
-			} else {
-				if (pVBInfo->LCDResInfo == Panel_1024x768) {
-					if (resinfo == 4) {/* 512x384 */
-						tempbx |= XGI_EnableLVDSDDA;
-					}
-				}
-			}
-		}
-	}
-
 	if (pVBInfo->VBInfo & SetInSlaveMode) {
 		if (pVBInfo->VBInfo & SetNotSimuMode)
 			tempbx |= XGI_LCDVESATiming;
@@ -3122,33 +2910,6 @@ static void XGI_XG27BLSignalVDD(unsigned short tempbh, unsigned short tempbl,
 	xgifb_reg_and_or(pVBInfo->P3d4, 0x48, ~tempbh, tempbl);
 }
 
-/* --------------------------------------------------------------------- */
-/* Function : XGI_XG21SetPanelDelay */
-/* Input : */
-/* Output : */
-/* Description : */
-/* I/P : bl : 1 ; T1 : the duration between CPL on and signal on */
-/* : bl : 2 ; T2 : the duration signal on and Vdd on */
-/* : bl : 3 ; T3 : the duration between CPL off and signal off */
-/* : bl : 4 ; T4 : the duration signal off and Vdd off */
-/* --------------------------------------------------------------------- */
-static void XGI_XG21SetPanelDelay(struct xgifb_video_info *xgifb_info,
-		unsigned short tempbl,
-		struct vb_device_info *pVBInfo)
-{
-	if (tempbl == 1)
-		mdelay(xgifb_info->lvds_data.PSC_S1);
-
-	if (tempbl == 2)
-		mdelay(xgifb_info->lvds_data.PSC_S2);
-
-	if (tempbl == 3)
-		mdelay(xgifb_info->lvds_data.PSC_S3);
-
-	if (tempbl == 4)
-		mdelay(xgifb_info->lvds_data.PSC_S4);
-}
-
 static void XGI_DisplayOn(struct xgifb_video_info *xgifb_info,
 		struct xgi_hw_device_info *pXGIHWDE,
 		struct vb_device_info *pVBInfo)
@@ -3160,12 +2921,12 @@ static void XGI_DisplayOn(struct xgifb_video_info *xgifb_info,
 			if (!(XGI_XG21GetPSCValue(pVBInfo) & 0x1)) {
 				/* LVDS VDD on */
 				XGI_XG21BLSignalVDD(0x01, 0x01, pVBInfo);
-				XGI_XG21SetPanelDelay(xgifb_info, 2, pVBInfo);
+				mdelay(xgifb_info->lvds_data.PSC_S2);
 			}
 			if (!(XGI_XG21GetPSCValue(pVBInfo) & 0x20))
 				/* LVDS signal on */
 				XGI_XG21BLSignalVDD(0x20, 0x20, pVBInfo);
-			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
+			mdelay(xgifb_info->lvds_data.PSC_S3);
 			/* LVDS backlight on */
 			XGI_XG21BLSignalVDD(0x02, 0x02, pVBInfo);
 		} else {
@@ -3180,12 +2941,12 @@ static void XGI_DisplayOn(struct xgifb_video_info *xgifb_info,
 			if (!(XGI_XG27GetPSCValue(pVBInfo) & 0x1)) {
 				/* LVDS VDD on */
 				XGI_XG27BLSignalVDD(0x01, 0x01, pVBInfo);
-				XGI_XG21SetPanelDelay(xgifb_info, 2, pVBInfo);
+				mdelay(xgifb_info->lvds_data.PSC_S2);
 			}
 			if (!(XGI_XG27GetPSCValue(pVBInfo) & 0x20))
 				/* LVDS signal on */
 				XGI_XG27BLSignalVDD(0x20, 0x20, pVBInfo);
-			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
+			mdelay(xgifb_info->lvds_data.PSC_S3);
 			/* LVDS backlight on */
 			XGI_XG27BLSignalVDD(0x02, 0x02, pVBInfo);
 		} else {
@@ -3205,7 +2966,7 @@ void XGI_DisplayOff(struct xgifb_video_info *xgifb_info,
 		if (pVBInfo->IF_DEF_LVDS == 1) {
 			/* LVDS backlight off */
 			XGI_XG21BLSignalVDD(0x02, 0x00, pVBInfo);
-			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
+			mdelay(xgifb_info->lvds_data.PSC_S3);
 		} else {
 			/* DVO/DVI signal off */
 			XGI_XG21BLSignalVDD(0x20, 0x00, pVBInfo);
@@ -3216,7 +2977,7 @@ void XGI_DisplayOff(struct xgifb_video_info *xgifb_info,
 		if ((XGI_XG27GetPSCValue(pVBInfo) & 0x2)) {
 			/* LVDS backlight off */
 			XGI_XG27BLSignalVDD(0x02, 0x00, pVBInfo);
-			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
+			mdelay(xgifb_info->lvds_data.PSC_S3);
 		}
 
 		if (pVBInfo->IF_DEF_LVDS == 0)
@@ -3378,7 +3139,6 @@ static void XGI_GetCRT2Data(unsigned short ModeNo, unsigned short ModeIdIndex,
 	unsigned short tempax = 0, tempbx, modeflag, resinfo;
 
 	struct SiS_LCDData *LCDPtr = NULL;
-	struct SiS_TVData *TVPtr = NULL;
 
 	/* si+Ext_ResInfo */
 	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
@@ -3395,9 +3155,8 @@ static void XGI_GetCRT2Data(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempbx = 4;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-		LCDPtr = (struct SiS_LCDData *) XGI_GetLcdPtr(tempbx,
-				ModeNo, ModeIdIndex, RefreshRateTableIndex,
-				pVBInfo);
+		LCDPtr = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+				       RefreshRateTableIndex, pVBInfo);
 
 		pVBInfo->RVBHCMAX = LCDPtr->RVBHCMAX;
 		pVBInfo->RVBHCFACT = LCDPtr->RVBHCFACT;
@@ -3479,10 +3238,10 @@ static void XGI_GetCRT2Data(unsigned short ModeNo, unsigned short ModeIdIndex,
 	}
 
 	if (pVBInfo->VBInfo & (SetCRT2ToTV)) {
-		tempbx = 4;
-		TVPtr = (struct SiS_TVData *) XGI_GetTVPtr(tempbx,
-				ModeNo, ModeIdIndex, RefreshRateTableIndex,
-				pVBInfo);
+		struct SiS_TVData const *TVPtr;
+
+		TVPtr = XGI_GetTVPtr(ModeNo, ModeIdIndex, RefreshRateTableIndex,
+				     pVBInfo);
 
 		pVBInfo->RVBHCMAX = TVPtr->RVBHCMAX;
 		pVBInfo->RVBHCFACT = TVPtr->RVBHCFACT;
@@ -3882,16 +3641,9 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 				| VB_SIS302LV | VB_XGI301C)))
 			temp += 2;
 
-		if (pVBInfo->VBInfo & SetCRT2ToHiVision) {
-			if (pVBInfo->VBType & VB_SIS301LV) {
-				if (pVBInfo->VBExtInfo == VB_YPbPr1080i) {
-					if (resinfo == 7)
-						temp -= 2;
-				}
-			} else if (resinfo == 7) {
+		if ((pVBInfo->VBInfo & SetCRT2ToHiVision) &&
+		    !(pVBInfo->VBType & VB_SIS301LV) && (resinfo == 7))
 				temp -= 2;
-			}
-		}
 	}
 
 	/* 0x05 Horizontal Display Start */
@@ -4061,18 +3813,16 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 		} else {
 			tempbx -= 10;
 		}
-	} else {
-		if (pVBInfo->TVInfo & TVSimuMode) {
-			if (pVBInfo->TVInfo & TVSetPAL) {
-				if (pVBInfo->VBType & VB_SIS301LV) {
-					if (!(pVBInfo->TVInfo &
-					    (TVSetYPbPr525p |
-					     TVSetYPbPr750p |
-					     TVSetHiVision)))
-						tempbx += 40;
-				} else {
+	} else if (pVBInfo->TVInfo & TVSimuMode) {
+		if (pVBInfo->TVInfo & TVSetPAL) {
+			if (pVBInfo->VBType & VB_SIS301LV) {
+				if (!(pVBInfo->TVInfo &
+				    (TVSetYPbPr525p |
+				     TVSetYPbPr750p |
+				     TVSetHiVision)))
 					tempbx += 40;
-				}
+			} else {
+				tempbx += 40;
 			}
 		}
 	}
@@ -4154,7 +3904,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 {
 	unsigned short i, j, tempax, tempbx, tempcx, temp, push1, push2,
 			modeflag, resinfo, crt2crtc;
-	unsigned char *TimingPoint;
+	unsigned char const *TimingPoint;
 
 	unsigned long longtemp, tempeax, tempebx, temp2, tempecx;
 
@@ -4186,33 +3936,33 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempax = (tempax & 0xff00) >> 8;
 
 	xgifb_reg_set(pVBInfo->Part2Port, 0x0, tempax);
-	TimingPoint = pVBInfo->NTSCTiming;
+	TimingPoint = XGI330_NTSCTiming;
 
 	if (pVBInfo->TVInfo & TVSetPAL)
-		TimingPoint = pVBInfo->PALTiming;
+		TimingPoint = XGI330_PALTiming;
 
 	if (pVBInfo->VBInfo & SetCRT2ToHiVision) {
-		TimingPoint = pVBInfo->HiTVExtTiming;
+		TimingPoint = XGI330_HiTVExtTiming;
 
 		if (pVBInfo->VBInfo & SetInSlaveMode)
-			TimingPoint = pVBInfo->HiTVSt2Timing;
+			TimingPoint = XGI330_HiTVSt2Timing;
 
 		if (pVBInfo->SetFlag & TVSimuMode)
-			TimingPoint = pVBInfo->HiTVSt1Timing;
+			TimingPoint = XGI330_HiTVSt1Timing;
 
 		if (!(modeflag & Charx8Dot))
-			TimingPoint = pVBInfo->HiTVTextTiming;
+			TimingPoint = XGI330_HiTVTextTiming;
 	}
 
 	if (pVBInfo->VBInfo & SetCRT2ToYPbPr525750) {
 		if (pVBInfo->TVInfo & TVSetYPbPr525i)
-			TimingPoint = pVBInfo->YPbPr525iTiming;
+			TimingPoint = XGI330_YPbPr525iTiming;
 
 		if (pVBInfo->TVInfo & TVSetYPbPr525p)
-			TimingPoint = pVBInfo->YPbPr525pTiming;
+			TimingPoint = XGI330_YPbPr525pTiming;
 
 		if (pVBInfo->TVInfo & TVSetYPbPr750p)
-			TimingPoint = pVBInfo->YPbPr750pTiming;
+			TimingPoint = XGI330_YPbPr750pTiming;
 	}
 
 	for (i = 0x01, j = 0; i <= 0x2D; i++, j++)
@@ -4385,11 +4135,9 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 						temp += 1;
 				}
 			}
-		} else {
-			if (pVBInfo->VBInfo & SetInSlaveMode) {
-				if (ModeNo == 0x2f)
-					temp += 1;
-			}
+		} else if (pVBInfo->VBInfo & SetInSlaveMode) {
+			if (ModeNo == 0x2f)
+				temp += 1;
 		}
 	}
 
@@ -4644,8 +4392,8 @@ static void XGI_SetLCDRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	/* Customized LCDB Des no add */
 	tempbx = 5;
-	LCDBDesPtr = (struct XGI_LCDDesStruct *) XGI_GetLcdPtr(tempbx, ModeNo,
-			ModeIdIndex, RefreshRateTableIndex, pVBInfo);
+	LCDBDesPtr = XGI_GetLcdPtr(tempbx, ModeNo, ModeIdIndex,
+				   RefreshRateTableIndex, pVBInfo);
 	tempah = pVBInfo->LCDResInfo;
 	tempah &= PanelResInfo;
 
@@ -4876,7 +4624,7 @@ static void XGI_SetGroup3(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short i;
-	unsigned char *tempdi;
+	unsigned char const *tempdi;
 	unsigned short modeflag;
 
 	/* si+Ext_ResInfo */
@@ -4905,18 +4653,18 @@ static void XGI_SetGroup3(unsigned short ModeNo, unsigned short ModeIdIndex,
 		if (pVBInfo->TVInfo & TVSetYPbPr525i)
 			return;
 
-		tempdi = pVBInfo->HiTVGroup3Data;
+		tempdi = XGI330_HiTVGroup3Data;
 		if (pVBInfo->SetFlag & TVSimuMode) {
-			tempdi = pVBInfo->HiTVGroup3Simu;
+			tempdi = XGI330_HiTVGroup3Simu;
 			if (!(modeflag & Charx8Dot))
-				tempdi = pVBInfo->HiTVGroup3Text;
+				tempdi = XGI330_HiTVGroup3Text;
 		}
 
 		if (pVBInfo->TVInfo & TVSetYPbPr525p)
-			tempdi = pVBInfo->Ren525pGroup3;
+			tempdi = XGI330_Ren525pGroup3;
 
 		if (pVBInfo->TVInfo & TVSetYPbPr750p)
-			tempdi = pVBInfo->Ren750pGroup3;
+			tempdi = XGI330_Ren750pGroup3;
 
 		for (i = 0; i <= 0x3E; i++)
 			xgifb_reg_set(pVBInfo->Part3Port, i, tempdi[i]);
@@ -5054,13 +4802,11 @@ static void XGI_SetGroup4(unsigned short ModeNo, unsigned short ModeIdIndex,
 		if (pVBInfo->VBInfo & SetCRT2ToLCD) {
 			if (tempax > 800)
 				tempax -= 800;
-		} else {
-			if (pVBInfo->VGAHDE > 800) {
-				if (pVBInfo->VGAHDE == 1024)
-					tempax = (tempax * 25 / 32) - 1;
-				else
-					tempax = (tempax * 20 / 32) - 1;
-			}
+		} else if (pVBInfo->VGAHDE > 800) {
+			if (pVBInfo->VGAHDE == 1024)
+				tempax = (tempax * 25 / 32) - 1;
+			else
+				tempax = (tempax * 20 / 32) - 1;
 		}
 		tempax -= 1;
 
@@ -5101,9 +4847,7 @@ static void XGI_SetGroup4(unsigned short ModeNo, unsigned short ModeIdIndex,
 	}
 	/* end 301b */
 
-	if (pVBInfo->ISXPDOS == 0)
-		XGI_SetCRT2VCLK(ModeNo, ModeIdIndex, RefreshRateTableIndex,
-				pVBInfo);
+	XGI_SetCRT2VCLK(ModeNo, ModeIdIndex, RefreshRateTableIndex, pVBInfo);
 }
 
 static void XGINew_EnableCRT2(struct vb_device_info *pVBInfo)
@@ -6414,12 +6158,10 @@ static void XGI_EnableBridge(struct xgifb_video_info *xgifb_info,
 			if (pVBInfo->SetFlag & EnableChA) {
 				/* Power on */
 				xgifb_reg_set(pVBInfo->Part1Port, 0x1E, 0x20);
-			} else {
-				if (pVBInfo->VBInfo & SetCRT2ToDualEdge) {
-					/* Power on */
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x1E, 0x20);
-				}
+			} else if (pVBInfo->VBInfo & SetCRT2ToDualEdge) {
+				/* Power on */
+				xgifb_reg_set(pVBInfo->Part1Port,
+						0x1E, 0x20);
 			}
 		}
 
@@ -6607,7 +6349,6 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 	struct vb_device_info *pVBInfo = &VBINF;
 	pVBInfo->BaseAddr = xgifb_info->vga_base;
 	pVBInfo->IF_DEF_LVDS = 0;
-	pVBInfo->IF_DEF_LCDA = 1;
 
 	if (HwDeviceExtension->jChipType >= XG20) {
 		pVBInfo->IF_DEF_YPbPr = 0;
@@ -6678,16 +6419,14 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 				XGI_SetLCDAGroup(ModeNo, ModeIdIndex,
 						HwDeviceExtension, pVBInfo);
 			}
-		} else {
-			if (!(pVBInfo->VBInfo & SwitchCRT2)) {
-				XGI_SetCRT1Group(xgifb_info,
-						HwDeviceExtension, ModeNo,
-						ModeIdIndex, pVBInfo);
-				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
-					XGI_SetLCDAGroup(ModeNo, ModeIdIndex,
-							HwDeviceExtension,
-							pVBInfo);
-				}
+		} else if (!(pVBInfo->VBInfo & SwitchCRT2)) {
+			XGI_SetCRT1Group(xgifb_info,
+					HwDeviceExtension, ModeNo,
+					ModeIdIndex, pVBInfo);
+			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
+				XGI_SetLCDAGroup(ModeNo, ModeIdIndex,
+						HwDeviceExtension,
+						pVBInfo);
 			}
 		}
 
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
index 22c8eb9810d6..70158c2c68af 100644
--- a/drivers/staging/xgifb/vb_struct.h
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -43,13 +43,6 @@ struct XGI_LCDDesStruct {
 	unsigned short LCDVRS;
 };
 
-struct XGI_LCDDataTablStruct {
-	unsigned char  PANELID;
-	unsigned short MASK;
-	unsigned short CAP;
-	unsigned short DATAPTR;
-};
-
 struct XGI330_LCDDataDesStruct2 {
 	unsigned short LCDHDES;
 	unsigned short LCDHRS;
@@ -59,19 +52,6 @@ struct XGI330_LCDDataDesStruct2 {
 	unsigned short LCDVSync;
 };
 
-
-struct XGI330_TVDataStruct {
-	unsigned short RVBHCMAX;
-	unsigned short RVBHCFACT;
-	unsigned short VGAHT;
-	unsigned short VGAVT;
-	unsigned short TVHDE;
-	unsigned short TVVDE;
-	unsigned short RVBHRS;
-	unsigned char FlickerMode;
-	unsigned short HALFRVBHRS;
-};
-
 struct XGI330_LCDDataTablStruct {
 	unsigned char  PANELID;
 	unsigned short MASK;
@@ -82,7 +62,7 @@ struct XGI330_LCDDataTablStruct {
 struct XGI330_TVDataTablStruct {
 	unsigned short MASK;
 	unsigned short CAP;
-	unsigned short DATAPTR;
+	struct SiS_TVData const *DATAPTR;
 };
 
 
@@ -137,10 +117,10 @@ struct XGI21_LVDSCapStruct {
 	unsigned short LVDSVSYNC;
 	unsigned char  VCLKData1;
 	unsigned char  VCLKData2;
-	unsigned char  PSC_S1;
-	unsigned char  PSC_S2;
-	unsigned char  PSC_S3;
-	unsigned char  PSC_S4;
+	unsigned char  PSC_S1; /* Duration between CPL on and signal on */
+	unsigned char  PSC_S2; /* Duration signal on and Vdd on */
+	unsigned char  PSC_S3; /* Duration between CPL off and signal off */
+	unsigned char  PSC_S4; /* Duration signal off and Vdd off */
 	unsigned char  PSC_S5;
 };
 
@@ -155,7 +135,6 @@ struct XGI301C_Tap4TimingStruct {
 };
 
 struct vb_device_info {
-	unsigned char  ISXPDOS;
 	unsigned long   P3c4, P3d4, P3c0, P3ce, P3c2, P3cc;
 	unsigned long   P3ca, P3c6, P3c7, P3c8, P3c9, P3da;
 	unsigned long   Part0Port, Part1Port, Part2Port;
@@ -168,12 +147,10 @@ struct vb_device_info {
 	unsigned short   ModeType;
 	unsigned short   IF_DEF_LVDS, IF_DEF_TRUMPION, IF_DEF_DSTN;
 	unsigned short   IF_DEF_CRT2Monitor;
-	unsigned short   IF_DEF_LCDA, IF_DEF_YPbPr;
-	unsigned short   IF_DEF_ExpLink;
+	unsigned short   IF_DEF_YPbPr;
 	unsigned short   IF_DEF_HiVision;
 	unsigned short   LCDResInfo, LCDTypeInfo, VBType;/*301b*/
 	unsigned short   VBInfo, TVInfo, LCDInfo;
-	unsigned short   VBExtInfo;/*301lv*/
 	unsigned short   SetFlag;
 	unsigned short   NewFlickerMode;
 	unsigned short   SelectCRT2Rate;
@@ -197,20 +174,6 @@ struct vb_device_info {
 	struct SiS_MCLKData  *MCLKData;
 	struct XGI_ECLKDataStruct  *ECLKData;
 
-	unsigned char   *NTSCTiming;
-	unsigned char   *PALTiming;
-	unsigned char   *HiTVExtTiming;
-	unsigned char   *HiTVSt1Timing;
-	unsigned char   *HiTVSt2Timing;
-	unsigned char   *HiTVTextTiming;
-	unsigned char   *YPbPr750pTiming;
-	unsigned char   *YPbPr525pTiming;
-	unsigned char   *YPbPr525iTiming;
-	unsigned char   *HiTVGroup3Data;
-	unsigned char   *HiTVGroup3Simu;
-	unsigned char   *HiTVGroup3Text;
-	unsigned char   *Ren525pGroup3;
-	unsigned char   *Ren750pGroup3;
 	unsigned char   *ScreenOffset;
 	unsigned char   *pXGINew_DRAMTypeDefinition;
 	unsigned char   XGINew_CR97;
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index 1c168461411d..180aae042cea 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -403,13 +403,6 @@ static struct XGI_CRT1TableStruct XGI_CRT1Table[] = {
 	  0x03, 0xDE, 0xC0, 0x84, 0xBF, 0x04, 0x90} }  /* 0x47 */
 };
 
-static unsigned char XGI_CH7017LV1024x768[] = {
-	0x60, 0x02, 0x00, 0x07, 0x40, 0xED,
-	0xA3, 0xC8, 0xC7, 0xAC, 0xE0, 0x02};
-static unsigned char XGI_CH7017LV1400x1050[] = {
-	0x60, 0x03, 0x11, 0x00, 0x40, 0xE3,
-	0xAD, 0xDB, 0xF6, 0xAC, 0xE0, 0x02};
-
 /*add for new UNIVGABIOS*/
 static struct SiS_LCDData  XGI_StLCD1024x768Data[] = {
 	{62,  25, 800,  546, 1344, 806},
@@ -525,18 +518,7 @@ static struct SiS_LCDData  XGI_StLCD1600x1200Data[] = {
 	{1,   1,  2160, 1250, 2160, 1250}  /* 09 (1600x1200) */
 };
 
-static struct SiS_LCDData  XGI_CetLCD1400x1050Data[] = {
-	{1, 1, 1688, 1066, 1688, 1066}, /* 00 (320x200,320x400,
-					       640x200,640x400) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 01 (320x350,640x350) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 02 (360x400,720x400) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 03 (720x350) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 04 (640x480x60Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 05 (800x600x60Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 06 (1024x768x60Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* 07 (1280x1024x60Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}  /* 08 (1400x1050x60Hz) */
-};
+#define XGI_CetLCD1400x1050Data XGI_CetLCD1280x1024Data
 
 static struct SiS_LCDData  XGI_NoScalingData[] = {
 	{1, 1, 800,  449,  800,  449},
@@ -583,17 +565,7 @@ static struct SiS_LCDData xgifb_lcd_1280x1024x75[] = {
 	{1,   1,   1688, 1066, 1688, 1066}  /* ; 07 (1280x1024x75Hz) */
 };
 
-static struct SiS_LCDData  XGI_CetLCD1280x1024x75Data[] = {
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 00 (320x200,320x400,
-						 640x200,640x400) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 01 (320x350,640x350) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 02 (360x400,720x400) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 03 (720x350) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 04 (640x480x75Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 05 (800x600x75Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 06 (1024x768x75Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}  /* ; 07 (1280x1024x75Hz) */
-};
+#define XGI_CetLCD1280x1024x75Data XGI_CetLCD1280x1024Data
 
 static struct SiS_LCDData  XGI_NoScalingDatax75[] = {
 	{1, 1, 800,  449,  800,  449},  /* ; 00 (320x200, 320x400,
@@ -903,7 +875,7 @@ static struct XGI330_LCDDataDesStruct2 XGI_NoScalingDesDatax75[] =  {
 	{9, 1337, 0,   771,  112, 6}  /* ; 0A (1280x768x60Hz) */
 };
 
-static struct XGI330_TVDataStruct  XGI_StPALData[] = {
+static const struct SiS_TVData XGI_StPALData[] = {
 	{1, 1, 864, 525, 1270, 400, 100, 0, 760},
 	{1, 1, 864, 525, 1270, 350, 100, 0, 760},
 	{1, 1, 864, 525, 1270, 400,   0, 0, 720},
@@ -912,7 +884,7 @@ static struct XGI330_TVDataStruct  XGI_StPALData[] = {
 	{1, 1, 864, 525, 1270, 600,  50, 0,   0}
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtPALData[] = {
+static const struct SiS_TVData XGI_ExtPALData[] = {
 	{2,  1, 1080, 463, 1270, 500,  50, 0,  50},
 	{15, 7, 1152, 413, 1270, 500,  50, 0,  50},
 	{2,  1, 1080, 463, 1270, 500,  50, 0,  50},
@@ -923,7 +895,7 @@ static struct XGI330_TVDataStruct  XGI_ExtPALData[] = {
 	{3,  2, 1080, 619, 1270, 540, 438, 0, 438}
 };
 
-static struct XGI330_TVDataStruct  XGI_StNTSCData[] = {
+static const struct SiS_TVData XGI_StNTSCData[] = {
 	{1, 1, 858, 525, 1270, 400, 50, 0, 760},
 	{1, 1, 858, 525, 1270, 350, 50, 0, 640},
 	{1, 1, 858, 525, 1270, 400,  0, 0, 720},
@@ -931,7 +903,7 @@ static struct XGI330_TVDataStruct  XGI_StNTSCData[] = {
 	{1, 1, 858, 525, 1270, 480,  0, 0, 760}
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtNTSCData[] = {
+static const struct SiS_TVData XGI_ExtNTSCData[] = {
 	{9,     5, 1001, 453, 1270, 420, 171, 0, 171},
 	{12,    5,  858, 403, 1270, 420, 171, 0, 171},
 	{9,     5, 1001, 453, 1270, 420, 171, 0, 171},
@@ -943,7 +915,7 @@ static struct XGI330_TVDataStruct  XGI_ExtNTSCData[] = {
 	{3,     2, 1001, 533, 1270, 420,   0, 0,   0}
 };
 
-static struct XGI330_TVDataStruct  XGI_St1HiTVData[] = {
+static const struct SiS_TVData XGI_St1HiTVData[] = {
 	{1, 1, 892,  563, 690,  800, 0,     0, 0}, /* 00 (320x200,320x400,
 							  640x200,640x400) */
 	{1, 1, 892,  563, 690,  700, 0,     0, 0}, /* 01 (320x350,640x350) */
@@ -953,7 +925,7 @@ static struct XGI330_TVDataStruct  XGI_St1HiTVData[] = {
 	{8, 5, 1050, 683, 1648, 960, 0x150, 1, 0}  /* 05 (400x300,800x600) */
 };
 
-static struct XGI330_TVDataStruct  XGI_St2HiTVData[] = {
+static const struct SiS_TVData XGI_St2HiTVData[] = {
 	{3, 1, 840,  483, 1648, 960, 0x032, 0, 0}, /* 00 (320x200,320x400,
 							  640x200,640x400) */
 	{1, 1, 892,  563, 690,  700, 0,     0, 0}, /* 01 (320x350,640x350) */
@@ -963,7 +935,7 @@ static struct XGI330_TVDataStruct  XGI_St2HiTVData[] = {
 	{8, 5, 1050, 683, 1648, 960, 0x17C, 1, 0}  /* 05 (400x300,800x600) */
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtHiTVData[] = {
+static const struct SiS_TVData XGI_ExtHiTVData[] = {
 	{6,  1,  840,  563,  1632, 960, 0,     0, 0}, /* 00 (320x200,320x400,
 							     640x200,640x400) */
 	{3,  1,  960,  563,  1632, 960, 0,     0, 0}, /* 01 (320x350,640x350) */
@@ -978,7 +950,7 @@ static struct XGI330_TVDataStruct  XGI_ExtHiTVData[] = {
 	{8,  5,  1750, 803,  1648, 960, 0x128, 0, 0}  /* 0A (1280x720) */
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtYPbPr525iData[] = {
+static const struct SiS_TVData XGI_ExtYPbPr525iData[] = {
 	{  9,  5,  1001, 453, 1270, 420, 171,   0, 171},
 	{ 12,  5,   858, 403, 1270, 420, 171,   0, 171},
 	{  9,  5,  1001, 453, 1270, 420, 171,   0, 171},
@@ -990,7 +962,7 @@ static struct XGI330_TVDataStruct  XGI_ExtYPbPr525iData[] = {
 	{  3,   2, 1001, 533, 1250, 420,   0,   0,   0}
 };
 
-static struct XGI330_TVDataStruct  XGI_StYPbPr525iData[] = {
+static const struct SiS_TVData XGI_StYPbPr525iData[] = {
 	{1, 1, 858, 525, 1270, 400, 50, 0, 760},
 	{1, 1, 858, 525, 1270, 350, 50, 0, 640},
 	{1, 1, 858, 525, 1270, 400,  0, 0, 720},
@@ -998,7 +970,7 @@ static struct XGI330_TVDataStruct  XGI_StYPbPr525iData[] = {
 	{1, 1, 858, 525, 1270, 480,  0, 0, 760},
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtYPbPr525pData[] = {
+static const struct SiS_TVData XGI_ExtYPbPr525pData[] = {
 	{  9,   5,  1001, 453, 1270, 420, 171, 0, 171},
 	{ 12,   5,   858, 403, 1270, 420, 171, 0, 171},
 	{  9,   5,  1001, 453, 1270, 420, 171, 0, 171},
@@ -1010,7 +982,7 @@ static struct XGI330_TVDataStruct  XGI_ExtYPbPr525pData[] = {
 	{  3,   2,  1001, 533, 1270, 420,   0, 0,   0}
 };
 
-static struct XGI330_TVDataStruct  XGI_StYPbPr525pData[] = {
+static const struct SiS_TVData XGI_StYPbPr525pData[] = {
 	{1, 1, 1716, 525, 1270, 400, 50, 0, 760},
 	{1, 1, 1716, 525, 1270, 350, 50, 0, 640},
 	{1, 1, 1716, 525, 1270, 400,  0, 0, 720},
@@ -1018,7 +990,7 @@ static struct XGI330_TVDataStruct  XGI_StYPbPr525pData[] = {
 	{1, 1, 1716, 525, 1270, 480,  0, 0, 760},
 };
 
-static struct XGI330_TVDataStruct  XGI_ExtYPbPr750pData[] = {
+static const struct SiS_TVData XGI_ExtYPbPr750pData[] = {
 	{ 3, 1,  935, 470, 1130, 680,  50, 0, 0}, /* 00 (320x200,320x400,
 							 640x200,640x400) */
 	{24, 7,  935, 420, 1130, 680,  50, 0, 0}, /* 01 (320x350,640x350) */
@@ -1033,7 +1005,7 @@ static struct XGI330_TVDataStruct  XGI_ExtYPbPr750pData[] = {
 	{10, 9, 1320, 830, 1130, 640,  50, 0, 0}
 };
 
-static struct XGI330_TVDataStruct  XGI_StYPbPr750pData[] = {
+static const struct SiS_TVData XGI_StYPbPr750pData[] = {
 	{1, 1, 1650, 750, 1280, 400, 50, 0, 760},
 	{1, 1, 1650, 750, 1280, 350, 50, 0, 640},
 	{1, 1, 1650, 750, 1280, 400,  0, 0, 720},
@@ -1041,7 +1013,7 @@ static struct XGI330_TVDataStruct  XGI_StYPbPr750pData[] = {
 	{1, 1, 1650, 750, 1280, 480,  0, 0, 760},
 };
 
-static unsigned char XGI330_NTSCTiming[] = {
+static const unsigned char XGI330_NTSCTiming[] = {
 	0x17, 0x1d, 0x03, 0x09, 0x05, 0x06, 0x0c, 0x0c,
 	0x94, 0x49, 0x01, 0x0a, 0x06, 0x0d, 0x04, 0x0a,
 	0x06, 0x14, 0x0d, 0x04, 0x0a, 0x00, 0x85, 0x1b,
@@ -1052,7 +1024,7 @@ static unsigned char XGI330_NTSCTiming[] = {
 	0x00, 0x40, 0x44, 0x00, 0xdb, 0x02, 0x3b, 0x00
 };
 
-static unsigned char XGI330_PALTiming[] = {
+static const unsigned char XGI330_PALTiming[] = {
 	0x21, 0x5A, 0x35, 0x6e, 0x04, 0x38, 0x3d, 0x70,
 	0x94, 0x49, 0x01, 0x12, 0x06, 0x3e, 0x35, 0x6d,
 	0x06, 0x14, 0x3e, 0x35, 0x6d, 0x00, 0x45, 0x2b,
@@ -1063,7 +1035,7 @@ static unsigned char XGI330_PALTiming[] = {
 	0x00, 0x40, 0x3e, 0x00, 0xe1, 0x02, 0x28, 0x00
 };
 
-static unsigned char XGI330_HiTVExtTiming[] = {
+static const unsigned char XGI330_HiTVExtTiming[] = {
 	0x2D, 0x60, 0x2C, 0x5F, 0x08, 0x31, 0x3A, 0x64,
 	0x28, 0x02, 0x01, 0x3D, 0x06, 0x3E, 0x35, 0x6D,
 	0x06, 0x14, 0x3E, 0x35, 0x6D, 0x00, 0xC5, 0x3F,
@@ -1075,7 +1047,7 @@ static unsigned char XGI330_HiTVExtTiming[] = {
 	0x27, 0x00, 0xfc, 0xff, 0x6a, 0x00
 };
 
-static unsigned char XGI330_HiTVSt1Timing[] = {
+static const unsigned char XGI330_HiTVSt1Timing[] = {
 	0x32, 0x65, 0x2C, 0x5F, 0x08, 0x31, 0x3A, 0x65,
 	0x28, 0x02, 0x01, 0x3D, 0x06, 0x3E, 0x35, 0x6D,
 	0x06, 0x14, 0x3E, 0x35, 0x6D, 0x00, 0xC5, 0x3F,
@@ -1087,7 +1059,7 @@ static unsigned char XGI330_HiTVSt1Timing[] = {
 	0x0E, 0x00, 0xfc, 0xff, 0x2d, 0x00
 };
 
-static unsigned char XGI330_HiTVSt2Timing[] = {
+static const unsigned char XGI330_HiTVSt2Timing[] = {
 	0x32, 0x65, 0x2C, 0x5F, 0x08, 0x31, 0x3A, 0x64,
 	0x28, 0x02, 0x01, 0x3D, 0x06, 0x3E, 0x35, 0x6D,
 	0x06, 0x14, 0x3E, 0x35, 0x6D, 0x00, 0xC5, 0x3F,
@@ -1099,7 +1071,7 @@ static unsigned char XGI330_HiTVSt2Timing[] = {
 	0x27, 0x00, 0xFC, 0xff, 0x6a, 0x00
 };
 
-static unsigned char XGI330_HiTVTextTiming[] = {
+static const unsigned char XGI330_HiTVTextTiming[] = {
 	0x32, 0x65, 0x2C, 0x5F, 0x08, 0x31, 0x3A, 0x65,
 	0x28, 0x02, 0x01, 0x3D, 0x06, 0x3E, 0x35, 0x6D,
 	0x06, 0x14, 0x3E, 0x35, 0x6D, 0x00, 0xC5, 0x3F,
@@ -1111,7 +1083,7 @@ static unsigned char XGI330_HiTVTextTiming[] = {
 	0x11, 0x00, 0xFC, 0xFF, 0x32, 0x00
 };
 
-static unsigned char XGI330_YPbPr750pTiming[] = {
+static const unsigned char XGI330_YPbPr750pTiming[] = {
 	0x30, 0x1d, 0xe8, 0x09, 0x09, 0xed, 0x0c, 0x0c,
 	0x98, 0x0a, 0x01, 0x0c, 0x06, 0x0d, 0x04, 0x0a,
 	0x06, 0x14, 0x0d, 0x04, 0x0a, 0x00, 0x85, 0x3f,
@@ -1123,7 +1095,7 @@ static unsigned char XGI330_YPbPr750pTiming[] = {
 	0x11, 0x00, 0xfc, 0xff, 0x32, 0x00
 };
 
-static unsigned char XGI330_YPbPr525pTiming[] = {
+static const unsigned char XGI330_YPbPr525pTiming[] = {
 	0x3E, 0x11, 0x06, 0x09, 0x0b, 0x0c, 0x0c, 0x0c,
 	0x98, 0x0a, 0x01, 0x0d, 0x06, 0x0d, 0x04, 0x0a,
 	0x06, 0x14, 0x0d, 0x04, 0x0a, 0x00, 0x85, 0x3f,
@@ -1135,7 +1107,7 @@ static unsigned char XGI330_YPbPr525pTiming[] = {
 	0x11, 0x00, 0xFC, 0xFF, 0x32, 0x00
 };
 
-static unsigned char XGI330_YPbPr525iTiming[] = {
+static const unsigned char XGI330_YPbPr525iTiming[] = {
 	0x1B, 0x21, 0x03, 0x09, 0x05, 0x06, 0x0C, 0x0C,
 	0x94, 0x49, 0x01, 0x0A, 0x06, 0x0D, 0x04, 0x0A,
 	0x06, 0x14, 0x0D, 0x04, 0x0A, 0x00, 0x85, 0x1B,
@@ -1147,7 +1119,7 @@ static unsigned char XGI330_YPbPr525iTiming[] = {
 	0x44, 0x00, 0xDB, 0x02, 0x3B, 0x00
 };
 
-static unsigned char XGI330_HiTVGroup3Data[] = {
+static const unsigned char XGI330_HiTVGroup3Data[] = {
 	0x00, 0x1A, 0x22, 0x63, 0x62, 0x22, 0x08, 0x5F,
 	0x05, 0x21, 0xB2, 0xB2, 0x55, 0x77, 0x2A, 0xA6,
 	0x25, 0x2F, 0x47, 0xFA, 0xC8, 0xFF, 0x8E, 0x20,
@@ -1158,7 +1130,7 @@ static unsigned char XGI330_HiTVGroup3Data[] = {
 	0x18, 0x05, 0x18, 0x05, 0x4C, 0xA8, 0x01
 };
 
-static unsigned char XGI330_HiTVGroup3Simu[] = {
+static const unsigned char XGI330_HiTVGroup3Simu[] = {
 	0x00, 0x1A, 0x22, 0x63, 0x62, 0x22, 0x08, 0x95,
 	0xDB, 0x20, 0xB8, 0xB8, 0x55, 0x47, 0x2A, 0xA6,
 	0x25, 0x2F, 0x47, 0xFA, 0xC8, 0xFF, 0x8E, 0x20,
@@ -1169,7 +1141,7 @@ static unsigned char XGI330_HiTVGroup3Simu[] = {
 	0x18, 0x05, 0x18, 0x05, 0x4C, 0xA8, 0x01
 };
 
-static unsigned char XGI330_HiTVGroup3Text[] = {
+static const unsigned char XGI330_HiTVGroup3Text[] = {
 	0x00, 0x1A, 0x22, 0x63, 0x62, 0x22, 0x08, 0xA7,
 	0xF5, 0x20, 0xCE, 0xCE, 0x55, 0x47, 0x2A, 0xA6,
 	0x25, 0x2F, 0x47, 0xFA, 0xC8, 0xFF, 0x8E, 0x20,
@@ -1180,7 +1152,7 @@ static unsigned char XGI330_HiTVGroup3Text[] = {
 	0x18, 0x05, 0x18, 0x05, 0x4C, 0xA8, 0x01
 };
 
-static unsigned char XGI330_Ren525pGroup3[] = {
+static const unsigned char XGI330_Ren525pGroup3[] = {
 	0x00, 0x14, 0x15, 0x25, 0x55, 0x15, 0x0b, 0x13,
 	0xB1, 0x41, 0x62, 0x62, 0xFF, 0xF4, 0x45, 0xa6,
 	0x25, 0x2F, 0x67, 0xF6, 0xbf, 0xFF, 0x8E, 0x20,
@@ -1191,7 +1163,7 @@ static unsigned char XGI330_Ren525pGroup3[] = {
 	0x1a, 0x1F, 0x25, 0x2a, 0x4C, 0xAA, 0x01
 };
 
-static unsigned char XGI330_Ren750pGroup3[] = {
+static const unsigned char XGI330_Ren750pGroup3[] = {
 	0x00, 0x14, 0x15, 0x25, 0x55, 0x15, 0x0b, 0x7a,
 	0x54, 0x41, 0xE7, 0xE7, 0xFF, 0xF4, 0x45, 0xa6,
 	0x25, 0x2F, 0x67, 0xF6, 0xbf, 0xFF, 0x8E, 0x20,
@@ -1236,17 +1208,7 @@ static struct SiS_LVDSData  XGI_LVDS1280x1024Data_1[] = {
 	{1688, 1066, 1688, 1066}
 };
 
-static struct SiS_LVDSData  XGI_LVDS1280x1024Data_2[] = {
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{1344, 806, 1344, 806},
-	{800,  449, 1280, 801},
-	{800,  525, 1280, 813}
-};
+#define XGI_LVDS1280x1024Data_2 XGI_LVDS1024x768Data_2
 
 static struct SiS_LVDSData  XGI_LVDS1400x1050Data_1[] = {
 	{928,   416, 1688, 1066},
@@ -1532,42 +1494,6 @@ static struct XGI330_LCDDataDesStruct2 XGI_LVDSNoScalingDesDatax75[] = {
 	{0, 1328,   0,  771, 112, 6}  /* ; 0A (1280x768x75Hz) */
 };
 
-static struct SiS_LVDSData  XGI_CHTVUNTSCData[] = {
-	{ 840, 600,  840, 600},
-	{ 840, 600,  840, 600},
-	{ 840, 600,  840, 600},
-	{ 840, 600,  840, 600},
-	{ 784, 600,  784, 600},
-	{1064, 750, 1064, 750}
-};
-
-static struct SiS_LVDSData  XGI_CHTVONTSCData[] = {
-	{ 840, 525,  840, 525},
-	{ 840, 525,  840, 525},
-	{ 840, 525,  840, 525},
-	{ 840, 525,  840, 525},
-	{ 784, 525,  784, 525},
-	{1040, 700, 1040, 700}
-};
-
-static struct SiS_LVDSData  XGI_CHTVUPALData[] = {
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{ 840, 750,  840, 750},
-	{ 936, 836,  936, 836}
-};
-
-static struct SiS_LVDSData  XGI_CHTVOPALData[] = {
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{1008, 625, 1008, 625},
-	{840,  625,  840, 625},
-	{960,  750,  960, 750}
-};
-
 /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
 static struct XGI_LVDSCRT1HDataStruct  XGI_LVDSCRT11024x768_1_H[] = {
 	{ {0x4B, 0x27, 0x8F, 0x32, 0x1B, 0x00, 0x45, 0x00} }, /* 00 (320x) */
@@ -1933,49 +1859,21 @@ static struct XGI330_LCDDataTablStruct XGI_EPLLCDDesDataPtr[] = {
 	{0xFF, 0x0000, 0x0000, 0}
 };
 
-static struct XGI330_LCDDataTablStruct XGI_EPLCHLCDRegPtr[] = {
-	{Panel_1024x768, 0x0000, 0x0000, 0}, /* XGI_CH7017LV1024x768 */
-	{Panel_1400x1050, 0x0000, 0x0000, 1}, /* XGI_CH7017LV1400x1050 */
-	{0xFF, 0x0000, 0x0000, 0}
-};
-
-static struct XGI330_TVDataTablStruct XGI_TVDataTable[] = {
-	{0x09E1, 0x0001, 0}, /* XGI_ExtPALData */
-	{0x09E1, 0x0000, 1}, /* XGI_ExtNTSCData */
-	{0x09E1, 0x0801, 2}, /* XGI_StPALData */
-	{0x09E1, 0x0800, 3}, /* XGI_StNTSCData */
-	{0x49E0, 0x0100, 4}, /* XGI_ExtHiTVData */
-	{0x49E0, 0x4100, 5}, /* XGI_St2HiTVData */
-	{0x49E0, 0x4900, 13}, /* XGI_St1HiTVData */
-	{0x09E0, 0x0020, 6}, /* XGI_ExtYPbPr525iData */
-	{0x09E0, 0x0040, 7}, /* XGI_ExtYPbPr525pData */
-	{0x09E0, 0x0080, 8}, /* XGI_ExtYPbPr750pData */
-	{0x09E0, 0x0820, 9}, /* XGI_StYPbPr525iData */
-	{0x09E0, 0x0840, 10}, /* XGI_StYPbPr525pData */
-	{0x09E0, 0x0880, 11}, /* XGI_StYPbPr750pData */
-	{0xffff, 0x0000, 12}  /* END */
-};
-
-/* Chrontel 7017 TV List */
-static struct XGI330_TVDataTablStruct xgifb_chrontel_tv[] = {
-	{0x0011, 0x0000, 0}, /* UNTSC */
-	{0x0011, 0x0010, 1}, /* ONTSC */
-	{0x0011, 0x0001, 2}, /* UPAL */
-	{0x0011, 0x0011, 3}, /* OPAL */
-	{0xFFFF, 0x0000, 4}
-};
-
-static unsigned short LCDLenList[] = {
-	LVDSCRT1Len_H,
-	LVDSCRT1Len_V,
-	LVDSDataLen,
-	LCDDesDataLen,
-	LCDDataLen,
-	LCDDesDataLen,
-	0,
-	LCDDesDataLen,
-	LCDDesDataLen,
-	0
+static const struct XGI330_TVDataTablStruct XGI_TVDataTable[] = {
+	{0x09E1, 0x0001, XGI_ExtPALData},
+	{0x09E1, 0x0000, XGI_ExtNTSCData},
+	{0x09E1, 0x0801, XGI_StPALData},
+	{0x09E1, 0x0800, XGI_StNTSCData},
+	{0x49E0, 0x0100, XGI_ExtHiTVData},
+	{0x49E0, 0x4100, XGI_St2HiTVData},
+	{0x49E0, 0x4900, XGI_St1HiTVData},
+	{0x09E0, 0x0020, XGI_ExtYPbPr525iData},
+	{0x09E0, 0x0040, XGI_ExtYPbPr525pData},
+	{0x09E0, 0x0080, XGI_ExtYPbPr750pData},
+	{0x09E0, 0x0820, XGI_StYPbPr525iData},
+	{0x09E0, 0x0840, XGI_StYPbPr525pData},
+	{0x09E0, 0x0880, XGI_StYPbPr750pData},
+	{0xffff, 0x0000, XGI_ExtNTSCData},
 };
 
 /* Dual link only */
@@ -2336,7 +2234,7 @@ static struct SiS_VCLKData XGI_VCLKData[] = {
 	{0xFF, 0x00,   0}  /* End mark */
 };
 
-static struct SiS_VCLKData XGI_VBVCLKData[] = {
+static struct SiS_VBVCLKData XGI_VBVCLKData[] = {
 	{0x1B, 0xE1,  25}, /* 00 (25.175MHz) */
 	{0x4E, 0xE4,  28}, /* 01 (28.322MHz) */
 	{0x57, 0xE4,  31}, /* 02 (31.500MHz) */
diff --git a/drivers/staging/zcache/tmem.c b/drivers/staging/zcache/tmem.c
index eaa90213457b..56c8e606ad1c 100644
--- a/drivers/staging/zcache/tmem.c
+++ b/drivers/staging/zcache/tmem.c
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2009-2011, Dan Magenheimer, Oracle Corp.
  *
- * The primary purpose of Transcedent Memory ("tmem") is to map object-oriented
+ * The primary purpose of Transcendent Memory ("tmem") is to map object-oriented
  * "handles" (triples containing a pool id, and object id, and an index), to
  * pages in a page-accessible memory (PAM).  Tmem references the PAM pages via
  * an abstract "pampd" (PAM page-descriptor), which can be operated on by a
diff --git a/drivers/staging/zsmalloc/zsmalloc-main.c b/drivers/staging/zsmalloc/zsmalloc-main.c
index 8b0bcb626a7f..09a9d35d436f 100644
--- a/drivers/staging/zsmalloc/zsmalloc-main.c
+++ b/drivers/staging/zsmalloc/zsmalloc-main.c
@@ -75,9 +75,140 @@
 #include <linux/cpumask.h>
 #include <linux/cpu.h>
 #include <linux/vmalloc.h>
+#include <linux/hardirq.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
 
 #include "zsmalloc.h"
-#include "zsmalloc_int.h"
+
+/*
+ * This must be power of 2 and greater than of equal to sizeof(link_free).
+ * These two conditions ensure that any 'struct link_free' itself doesn't
+ * span more than 1 page which avoids complex case of mapping 2 pages simply
+ * to restore link_free pointer values.
+ */
+#define ZS_ALIGN		8
+
+/*
+ * A single 'zspage' is composed of up to 2^N discontiguous 0-order (single)
+ * pages. ZS_MAX_ZSPAGE_ORDER defines upper limit on N.
+ */
+#define ZS_MAX_ZSPAGE_ORDER 2
+#define ZS_MAX_PAGES_PER_ZSPAGE (_AC(1, UL) << ZS_MAX_ZSPAGE_ORDER)
+
+/*
+ * Object location (<PFN>, <obj_idx>) is encoded as
+ * as single (void *) handle value.
+ *
+ * Note that object index <obj_idx> is relative to system
+ * page <PFN> it is stored in, so for each sub-page belonging
+ * to a zspage, obj_idx starts with 0.
+ *
+ * This is made more complicated by various memory models and PAE.
+ */
+
+#ifndef MAX_PHYSMEM_BITS
+#ifdef CONFIG_HIGHMEM64G
+#define MAX_PHYSMEM_BITS 36
+#else /* !CONFIG_HIGHMEM64G */
+/*
+ * If this definition of MAX_PHYSMEM_BITS is used, OBJ_INDEX_BITS will just
+ * be PAGE_SHIFT
+ */
+#define MAX_PHYSMEM_BITS BITS_PER_LONG
+#endif
+#endif
+#define _PFN_BITS		(MAX_PHYSMEM_BITS - PAGE_SHIFT)
+#define OBJ_INDEX_BITS	(BITS_PER_LONG - _PFN_BITS)
+#define OBJ_INDEX_MASK	((_AC(1, UL) << OBJ_INDEX_BITS) - 1)
+
+#define MAX(a, b) ((a) >= (b) ? (a) : (b))
+/* ZS_MIN_ALLOC_SIZE must be multiple of ZS_ALIGN */
+#define ZS_MIN_ALLOC_SIZE \
+	MAX(32, (ZS_MAX_PAGES_PER_ZSPAGE << PAGE_SHIFT >> OBJ_INDEX_BITS))
+#define ZS_MAX_ALLOC_SIZE	PAGE_SIZE
+
+/*
+ * On systems with 4K page size, this gives 254 size classes! There is a
+ * trader-off here:
+ *  - Large number of size classes is potentially wasteful as free page are
+ *    spread across these classes
+ *  - Small number of size classes causes large internal fragmentation
+ *  - Probably its better to use specific size classes (empirically
+ *    determined). NOTE: all those class sizes must be set as multiple of
+ *    ZS_ALIGN to make sure link_free itself never has to span 2 pages.
+ *
+ *  ZS_MIN_ALLOC_SIZE and ZS_SIZE_CLASS_DELTA must be multiple of ZS_ALIGN
+ *  (reason above)
+ */
+#define ZS_SIZE_CLASS_DELTA	16
+#define ZS_SIZE_CLASSES		((ZS_MAX_ALLOC_SIZE - ZS_MIN_ALLOC_SIZE) / \
+					ZS_SIZE_CLASS_DELTA + 1)
+
+/*
+ * We do not maintain any list for completely empty or full pages
+ */
+enum fullness_group {
+	ZS_ALMOST_FULL,
+	ZS_ALMOST_EMPTY,
+	_ZS_NR_FULLNESS_GROUPS,
+
+	ZS_EMPTY,
+	ZS_FULL
+};
+
+/*
+ * We assign a page to ZS_ALMOST_EMPTY fullness group when:
+ *	n <= N / f, where
+ * n = number of allocated objects
+ * N = total number of objects zspage can store
+ * f = 1/fullness_threshold_frac
+ *
+ * Similarly, we assign zspage to:
+ *	ZS_ALMOST_FULL	when n > N / f
+ *	ZS_EMPTY	when n == 0
+ *	ZS_FULL		when n == N
+ *
+ * (see: fix_fullness_group())
+ */
+static const int fullness_threshold_frac = 4;
+
+struct size_class {
+	/*
+	 * Size of objects stored in this class. Must be multiple
+	 * of ZS_ALIGN.
+	 */
+	int size;
+	unsigned int index;
+
+	/* Number of PAGE_SIZE sized pages to combine to form a 'zspage' */
+	int pages_per_zspage;
+
+	spinlock_t lock;
+
+	/* stats */
+	u64 pages_allocated;
+
+	struct page *fullness_list[_ZS_NR_FULLNESS_GROUPS];
+};
+
+/*
+ * Placed within free objects to form a singly linked list.
+ * For every zspage, first_page->freelist gives head of this list.
+ *
+ * This must be power of 2 and less than or equal to ZS_ALIGN
+ */
+struct link_free {
+	/* Handle of next free chunk (encodes <PFN, obj_idx>) */
+	void *next;
+};
+
+struct zs_pool {
+	struct size_class size_class[ZS_SIZE_CLASSES];
+
+	gfp_t flags;	/* allocation flags used when growing pool */
+	const char *name;
+};
 
 /*
  * A zspage's class index and fullness group
@@ -88,6 +219,30 @@
 #define CLASS_IDX_MASK	((1 << CLASS_IDX_BITS) - 1)
 #define FULLNESS_MASK	((1 << FULLNESS_BITS) - 1)
 
+/*
+ * By default, zsmalloc uses a copy-based object mapping method to access
+ * allocations that span two pages. However, if a particular architecture
+ * 1) Implements local_flush_tlb_kernel_range() and 2) Performs VM mapping
+ * faster than copying, then it should be added here so that
+ * USE_PGTABLE_MAPPING is defined. This causes zsmalloc to use page table
+ * mapping rather than copying
+ * for object mapping.
+*/
+#if defined(CONFIG_ARM)
+#define USE_PGTABLE_MAPPING
+#endif
+
+struct mapping_area {
+#ifdef USE_PGTABLE_MAPPING
+	struct vm_struct *vm; /* vm area for mapping object that span pages */
+#else
+	char *vm_buf; /* copy buffer for objects that span pages */
+#endif
+	char *vm_addr; /* address of kmap_atomic()'ed pages */
+	enum zs_mapmode vm_mm; /* mapping mode */
+};
+
+
 /* per-cpu VM mapping areas for zspage accesses that cross page boundaries */
 static DEFINE_PER_CPU(struct mapping_area, zs_map_area);
 
@@ -470,16 +625,83 @@ static struct page *find_get_zspage(struct size_class *class)
 	return page;
 }
 
-static void zs_copy_map_object(char *buf, struct page *firstpage,
-				int off, int size)
+#ifdef USE_PGTABLE_MAPPING
+static inline int __zs_cpu_up(struct mapping_area *area)
+{
+	/*
+	 * Make sure we don't leak memory if a cpu UP notification
+	 * and zs_init() race and both call zs_cpu_up() on the same cpu
+	 */
+	if (area->vm)
+		return 0;
+	area->vm = alloc_vm_area(PAGE_SIZE * 2, NULL);
+	if (!area->vm)
+		return -ENOMEM;
+	return 0;
+}
+
+static inline void __zs_cpu_down(struct mapping_area *area)
+{
+	if (area->vm)
+		free_vm_area(area->vm);
+	area->vm = NULL;
+}
+
+static inline void *__zs_map_object(struct mapping_area *area,
+				struct page *pages[2], int off, int size)
+{
+	BUG_ON(map_vm_area(area->vm, PAGE_KERNEL, &pages));
+	area->vm_addr = area->vm->addr;
+	return area->vm_addr + off;
+}
+
+static inline void __zs_unmap_object(struct mapping_area *area,
+				struct page *pages[2], int off, int size)
+{
+	unsigned long addr = (unsigned long)area->vm_addr;
+	unsigned long end = addr + (PAGE_SIZE * 2);
+
+	flush_cache_vunmap(addr, end);
+	unmap_kernel_range_noflush(addr, PAGE_SIZE * 2);
+	local_flush_tlb_kernel_range(addr, end);
+}
+
+#else /* USE_PGTABLE_MAPPING */
+
+static inline int __zs_cpu_up(struct mapping_area *area)
+{
+	/*
+	 * Make sure we don't leak memory if a cpu UP notification
+	 * and zs_init() race and both call zs_cpu_up() on the same cpu
+	 */
+	if (area->vm_buf)
+		return 0;
+	area->vm_buf = (char *)__get_free_page(GFP_KERNEL);
+	if (!area->vm_buf)
+		return -ENOMEM;
+	return 0;
+}
+
+static inline void __zs_cpu_down(struct mapping_area *area)
+{
+	if (area->vm_buf)
+		free_page((unsigned long)area->vm_buf);
+	area->vm_buf = NULL;
+}
+
+static void *__zs_map_object(struct mapping_area *area,
+			struct page *pages[2], int off, int size)
 {
-	struct page *pages[2];
 	int sizes[2];
 	void *addr;
+	char *buf = area->vm_buf;
 
-	pages[0] = firstpage;
-	pages[1] = get_next_page(firstpage);
-	BUG_ON(!pages[1]);
+	/* disable page faults to match kmap_atomic() return conditions */
+	pagefault_disable();
+
+	/* no read fastpath */
+	if (area->vm_mm == ZS_MM_WO)
+		goto out;
 
 	sizes[0] = PAGE_SIZE - off;
 	sizes[1] = size - sizes[0];
@@ -491,18 +713,20 @@ static void zs_copy_map_object(char *buf, struct page *firstpage,
 	addr = kmap_atomic(pages[1]);
 	memcpy(buf + sizes[0], addr, sizes[1]);
 	kunmap_atomic(addr);
+out:
+	return area->vm_buf;
 }
 
-static void zs_copy_unmap_object(char *buf, struct page *firstpage,
-				int off, int size)
+static void __zs_unmap_object(struct mapping_area *area,
+			struct page *pages[2], int off, int size)
 {
-	struct page *pages[2];
 	int sizes[2];
 	void *addr;
+	char *buf = area->vm_buf;
 
-	pages[0] = firstpage;
-	pages[1] = get_next_page(firstpage);
-	BUG_ON(!pages[1]);
+	/* no write fastpath */
+	if (area->vm_mm == ZS_MM_RO)
+		goto out;
 
 	sizes[0] = PAGE_SIZE - off;
 	sizes[1] = size - sizes[0];
@@ -514,34 +738,31 @@ static void zs_copy_unmap_object(char *buf, struct page *firstpage,
 	addr = kmap_atomic(pages[1]);
 	memcpy(addr, buf + sizes[0], sizes[1]);
 	kunmap_atomic(addr);
+
+out:
+	/* enable page faults to match kunmap_atomic() return conditions */
+	pagefault_enable();
 }
 
+#endif /* USE_PGTABLE_MAPPING */
+
 static int zs_cpu_notifier(struct notifier_block *nb, unsigned long action,
 				void *pcpu)
 {
-	int cpu = (long)pcpu;
+	int ret, cpu = (long)pcpu;
 	struct mapping_area *area;
 
 	switch (action) {
 	case CPU_UP_PREPARE:
 		area = &per_cpu(zs_map_area, cpu);
-		/*
-		 * Make sure we don't leak memory if a cpu UP notification
-		 * and zs_init() race and both call zs_cpu_up() on the same cpu
-		 */
-		if (area->vm_buf)
-			return 0;
-		area->vm_buf = (char *)__get_free_page(GFP_KERNEL);
-		if (!area->vm_buf)
-			return -ENOMEM;
-		return 0;
+		ret = __zs_cpu_up(area);
+		if (ret)
+			return notifier_from_errno(ret);
 		break;
 	case CPU_DEAD:
 	case CPU_UP_CANCELED:
 		area = &per_cpu(zs_map_area, cpu);
-		if (area->vm_buf)
-			free_page((unsigned long)area->vm_buf);
-		area->vm_buf = NULL;
+		__zs_cpu_down(area);
 		break;
 	}
 
@@ -758,28 +979,36 @@ void *zs_map_object(struct zs_pool *pool, unsigned long handle,
 	enum fullness_group fg;
 	struct size_class *class;
 	struct mapping_area *area;
+	struct page *pages[2];
 
 	BUG_ON(!handle);
 
+	/*
+	 * Because we use per-cpu mapping areas shared among the
+	 * pools/users, we can't allow mapping in interrupt context
+	 * because it can corrupt another users mappings.
+	 */
+	BUG_ON(in_interrupt());
+
 	obj_handle_to_location(handle, &page, &obj_idx);
 	get_zspage_mapping(get_first_page(page), &class_idx, &fg);
 	class = &pool->size_class[class_idx];
 	off = obj_idx_to_offset(page, obj_idx, class->size);
 
 	area = &get_cpu_var(zs_map_area);
+	area->vm_mm = mm;
 	if (off + class->size <= PAGE_SIZE) {
 		/* this object is contained entirely within a page */
 		area->vm_addr = kmap_atomic(page);
 		return area->vm_addr + off;
 	}
 
-	/* disable page faults to match kmap_atomic() return conditions */
-	pagefault_disable();
+	/* this object spans two pages */
+	pages[0] = page;
+	pages[1] = get_next_page(page);
+	BUG_ON(!pages[1]);
 
-	if (mm != ZS_MM_WO)
-		zs_copy_map_object(area->vm_buf, page, off, class->size);
-	area->vm_addr = NULL;
-	return area->vm_buf;
+	return __zs_map_object(area, pages, off, class->size);
 }
 EXPORT_SYMBOL_GPL(zs_map_object);
 
@@ -793,17 +1022,6 @@ void zs_unmap_object(struct zs_pool *pool, unsigned long handle)
 	struct size_class *class;
 	struct mapping_area *area;
 
-	area = &__get_cpu_var(zs_map_area);
-	/* single-page object fastpath */
-	if (area->vm_addr) {
-		kunmap_atomic(area->vm_addr);
-		goto out;
-	}
-
-	/* no write fastpath */
-	if (area->vm_mm == ZS_MM_RO)
-		goto pfenable;
-
 	BUG_ON(!handle);
 
 	obj_handle_to_location(handle, &page, &obj_idx);
@@ -811,12 +1029,18 @@ void zs_unmap_object(struct zs_pool *pool, unsigned long handle)
 	class = &pool->size_class[class_idx];
 	off = obj_idx_to_offset(page, obj_idx, class->size);
 
-	zs_copy_unmap_object(area->vm_buf, page, off, class->size);
+	area = &__get_cpu_var(zs_map_area);
+	if (off + class->size <= PAGE_SIZE)
+		kunmap_atomic(area->vm_addr);
+	else {
+		struct page *pages[2];
 
-pfenable:
-	/* enable page faults to match kunmap_atomic() return conditions */
-	pagefault_enable();
-out:
+		pages[0] = page;
+		pages[1] = get_next_page(page);
+		BUG_ON(!pages[1]);
+
+		__zs_unmap_object(area, pages, off, class->size);
+	}
 	put_cpu_var(zs_map_area);
 }
 EXPORT_SYMBOL_GPL(zs_unmap_object);
diff --git a/drivers/staging/zsmalloc/zsmalloc_int.h b/drivers/staging/zsmalloc/zsmalloc_int.h
deleted file mode 100644
index 528051767733..000000000000
--- a/drivers/staging/zsmalloc/zsmalloc_int.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- * zsmalloc memory allocator
- *
- * Copyright (C) 2011  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the license that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-#ifndef _ZS_MALLOC_INT_H_
-#define _ZS_MALLOC_INT_H_
-
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-#include <linux/types.h>
-
-/*
- * This must be power of 2 and greater than of equal to sizeof(link_free).
- * These two conditions ensure that any 'struct link_free' itself doesn't
- * span more than 1 page which avoids complex case of mapping 2 pages simply
- * to restore link_free pointer values.
- */
-#define ZS_ALIGN		8
-
-/*
- * A single 'zspage' is composed of up to 2^N discontiguous 0-order (single)
- * pages. ZS_MAX_ZSPAGE_ORDER defines upper limit on N.
- */
-#define ZS_MAX_ZSPAGE_ORDER 2
-#define ZS_MAX_PAGES_PER_ZSPAGE (_AC(1, UL) << ZS_MAX_ZSPAGE_ORDER)
-
-/*
- * Object location (<PFN>, <obj_idx>) is encoded as
- * as single (void *) handle value.
- *
- * Note that object index <obj_idx> is relative to system
- * page <PFN> it is stored in, so for each sub-page belonging
- * to a zspage, obj_idx starts with 0.
- *
- * This is made more complicated by various memory models and PAE.
- */
-
-#ifndef MAX_PHYSMEM_BITS
-#ifdef CONFIG_HIGHMEM64G
-#define MAX_PHYSMEM_BITS 36
-#else /* !CONFIG_HIGHMEM64G */
-/*
- * If this definition of MAX_PHYSMEM_BITS is used, OBJ_INDEX_BITS will just
- * be PAGE_SHIFT
- */
-#define MAX_PHYSMEM_BITS BITS_PER_LONG
-#endif
-#endif
-#define _PFN_BITS		(MAX_PHYSMEM_BITS - PAGE_SHIFT)
-#define OBJ_INDEX_BITS	(BITS_PER_LONG - _PFN_BITS)
-#define OBJ_INDEX_MASK	((_AC(1, UL) << OBJ_INDEX_BITS) - 1)
-
-#define MAX(a, b) ((a) >= (b) ? (a) : (b))
-/* ZS_MIN_ALLOC_SIZE must be multiple of ZS_ALIGN */
-#define ZS_MIN_ALLOC_SIZE \
-	MAX(32, (ZS_MAX_PAGES_PER_ZSPAGE << PAGE_SHIFT >> OBJ_INDEX_BITS))
-#define ZS_MAX_ALLOC_SIZE	PAGE_SIZE
-
-/*
- * On systems with 4K page size, this gives 254 size classes! There is a
- * trader-off here:
- *  - Large number of size classes is potentially wasteful as free page are
- *    spread across these classes
- *  - Small number of size classes causes large internal fragmentation
- *  - Probably its better to use specific size classes (empirically
- *    determined). NOTE: all those class sizes must be set as multiple of
- *    ZS_ALIGN to make sure link_free itself never has to span 2 pages.
- *
- *  ZS_MIN_ALLOC_SIZE and ZS_SIZE_CLASS_DELTA must be multiple of ZS_ALIGN
- *  (reason above)
- */
-#define ZS_SIZE_CLASS_DELTA	16
-#define ZS_SIZE_CLASSES		((ZS_MAX_ALLOC_SIZE - ZS_MIN_ALLOC_SIZE) / \
-					ZS_SIZE_CLASS_DELTA + 1)
-
-/*
- * We do not maintain any list for completely empty or full pages
- */
-enum fullness_group {
-	ZS_ALMOST_FULL,
-	ZS_ALMOST_EMPTY,
-	_ZS_NR_FULLNESS_GROUPS,
-
-	ZS_EMPTY,
-	ZS_FULL
-};
-
-/*
- * We assign a page to ZS_ALMOST_EMPTY fullness group when:
- *	n <= N / f, where
- * n = number of allocated objects
- * N = total number of objects zspage can store
- * f = 1/fullness_threshold_frac
- *
- * Similarly, we assign zspage to:
- *	ZS_ALMOST_FULL	when n > N / f
- *	ZS_EMPTY	when n == 0
- *	ZS_FULL		when n == N
- *
- * (see: fix_fullness_group())
- */
-static const int fullness_threshold_frac = 4;
-
-struct mapping_area {
-	char *vm_buf; /* copy buffer for objects that span pages */
-	char *vm_addr; /* address of kmap_atomic()'ed pages */
-	enum zs_mapmode vm_mm; /* mapping mode */
-};
-
-struct size_class {
-	/*
-	 * Size of objects stored in this class. Must be multiple
-	 * of ZS_ALIGN.
-	 */
-	int size;
-	unsigned int index;
-
-	/* Number of PAGE_SIZE sized pages to combine to form a 'zspage' */
-	int pages_per_zspage;
-
-	spinlock_t lock;
-
-	/* stats */
-	u64 pages_allocated;
-
-	struct page *fullness_list[_ZS_NR_FULLNESS_GROUPS];
-};
-
-/*
- * Placed within free objects to form a singly linked list.
- * For every zspage, first_page->freelist gives head of this list.
- *
- * This must be power of 2 and less than or equal to ZS_ALIGN
- */
-struct link_free {
-	/* Handle of next free chunk (encodes <PFN, obj_idx>) */
-	void *next;
-};
-
-struct zs_pool {
-	struct size_class size_class[ZS_SIZE_CLASSES];
-
-	gfp_t flags;	/* allocation flags used when growing pool */
-	const char *name;
-};
-
-#endif
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index 0c4760fabfc0..240f7aa76ed1 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -662,7 +662,7 @@ int iscsi_extract_key_value(char *textbuf, char **key, char **value)
 {
 	*value = strchr(textbuf, '=');
 	if (!*value) {
-		pr_err("Unable to locate \"=\" seperator for key,"
+		pr_err("Unable to locate \"=\" separator for key,"
 				" ignoring request.\n");
 		return -1;
 	}
@@ -1269,7 +1269,7 @@ static int iscsi_check_value(struct iscsi_param *param, char *value)
 		comma_ptr = strchr(value, ',');
 
 		if (comma_ptr && !IS_TYPE_VALUE_LIST(param)) {
-			pr_err("Detected value seperator \",\", but"
+			pr_err("Detected value separator \",\", but"
 				" key \"%s\" does not allow a value list,"
 				" protocol error.\n", param->name);
 			return -1;
diff --git a/drivers/tty/Kconfig b/drivers/tty/Kconfig
index 830cd62d8492..d8e05eeab232 100644
--- a/drivers/tty/Kconfig
+++ b/drivers/tty/Kconfig
@@ -214,8 +214,8 @@ config CYCLADES
 	  If you haven't heard about it, it's safe to say N.
 
 config CYZ_INTR
-	bool "Cyclades-Z interrupt mode operation (EXPERIMENTAL)"
-	depends on EXPERIMENTAL && CYCLADES
+	bool "Cyclades-Z interrupt mode operation"
+	depends on CYCLADES
 	help
 	  The Cyclades-Z family of multiport cards allows 2 (two) driver op
 	  modes: polling and interrupt. In polling mode, the driver will check
@@ -285,7 +285,7 @@ config SYNCLINK_GT
 
 config NOZOMI
 	tristate "HSDPA Broadband Wireless Data Card - Globe Trotter"
-	depends on PCI && EXPERIMENTAL
+	depends on PCI
 	help
 	  If you have a HSDPA driver Broadband Wireless Data Card -
 	  Globe Trotter PCMCIA card, say Y here.
@@ -294,7 +294,7 @@ config NOZOMI
 	  will be called nozomi.
 
 config ISI
-	tristate "Multi-Tech multiport card support (EXPERIMENTAL)"
+	tristate "Multi-Tech multiport card support"
 	depends on SERIAL_NONSTANDARD && PCI
 	select FW_LOADER
 	help
@@ -317,7 +317,6 @@ config N_HDLC
 
 config N_GSM
 	tristate "GSM MUX line discipline support (EXPERIMENTAL)"
-	depends on EXPERIMENTAL
 	depends on NET
 	help
 	  This line discipline provides support for the GSM MUX protocol and
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index 6cc4358f68c1..42d0a2581a87 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -420,7 +420,7 @@ static void check_modem_status(struct serial_state *info)
 				tty_hangup(port->tty);
 		}
 	}
-	if (port->flags & ASYNC_CTS_FLOW) {
+	if (tty_port_cts_enabled(port)) {
 		if (port->tty->hw_stopped) {
 			if (!(status & SER_CTS)) {
 #if (defined(SERIAL_DEBUG_INTR) || defined(SERIAL_DEBUG_FLOW))
@@ -646,7 +646,7 @@ static void shutdown(struct tty_struct *tty, struct serial_state *info)
 	custom.adkcon = AC_UARTBRK;
 	mb();
 
-	if (tty->termios->c_cflag & HUPCL)
+	if (tty->termios.c_cflag & HUPCL)
 		info->MCR &= ~(SER_DTR|SER_RTS);
 	rtsdtr_ctrl(info->MCR);
 
@@ -670,7 +670,7 @@ static void change_speed(struct tty_struct *tty, struct serial_state *info,
 	int	bits;
 	unsigned long	flags;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	/* Byte size is always 8 bits plus parity bit if requested */
 
@@ -707,8 +707,8 @@ static void change_speed(struct tty_struct *tty, struct serial_state *info,
 	/* If the quotient is zero refuse the change */
 	if (!quot && old_termios) {
 		/* FIXME: Will need updating for new tty in the end */
-		tty->termios->c_cflag &= ~CBAUD;
-		tty->termios->c_cflag |= (old_termios->c_cflag & CBAUD);
+		tty->termios.c_cflag &= ~CBAUD;
+		tty->termios.c_cflag |= (old_termios->c_cflag & CBAUD);
 		baud = tty_get_baud_rate(tty);
 		if (!baud)
 			baud = 9600;
@@ -984,7 +984,7 @@ static void rs_throttle(struct tty_struct * tty)
 	if (I_IXOFF(tty))
 		rs_send_xchar(tty, STOP_CHAR(tty));
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		info->MCR &= ~SER_RTS;
 
 	local_irq_save(flags);
@@ -1012,7 +1012,7 @@ static void rs_unthrottle(struct tty_struct * tty)
 		else
 			rs_send_xchar(tty, START_CHAR(tty));
 	}
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		info->MCR |= SER_RTS;
 	local_irq_save(flags);
 	rtsdtr_ctrl(info->MCR);
@@ -1033,7 +1033,7 @@ static int get_serial_info(struct tty_struct *tty, struct serial_state *state,
 	if (!retinfo)
 		return -EFAULT;
 	memset(&tmp, 0, sizeof(tmp));
-	tty_lock();
+	tty_lock(tty);
 	tmp.line = tty->index;
 	tmp.port = state->port;
 	tmp.flags = state->tport.flags;
@@ -1042,7 +1042,7 @@ static int get_serial_info(struct tty_struct *tty, struct serial_state *state,
 	tmp.close_delay = state->tport.close_delay;
 	tmp.closing_wait = state->tport.closing_wait;
 	tmp.custom_divisor = state->custom_divisor;
-	tty_unlock();
+	tty_unlock(tty);
 	if (copy_to_user(retinfo,&tmp,sizeof(*retinfo)))
 		return -EFAULT;
 	return 0;
@@ -1059,12 +1059,12 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 	if (copy_from_user(&new_serial,new_info,sizeof(new_serial)))
 		return -EFAULT;
 
-	tty_lock();
+	tty_lock(tty);
 	change_spd = ((new_serial.flags ^ port->flags) & ASYNC_SPD_MASK) ||
 		new_serial.custom_divisor != state->custom_divisor;
 	if (new_serial.irq || new_serial.port != state->port ||
 			new_serial.xmit_fifo_size != state->xmit_fifo_size) {
-		tty_unlock();
+		tty_unlock(tty);
 		return -EINVAL;
 	}
   
@@ -1074,7 +1074,7 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 		    (new_serial.xmit_fifo_size != state->xmit_fifo_size) ||
 		    ((new_serial.flags & ~ASYNC_USR_MASK) !=
 		     (port->flags & ~ASYNC_USR_MASK))) {
-			tty_unlock();
+			tty_unlock(tty);
 			return -EPERM;
 		}
 		port->flags = ((port->flags & ~ASYNC_USR_MASK) |
@@ -1084,7 +1084,7 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 	}
 
 	if (new_serial.baud_base < 9600) {
-		tty_unlock();
+		tty_unlock(tty);
 		return -EINVAL;
 	}
 
@@ -1116,7 +1116,7 @@ check_and_exit:
 		}
 	} else
 		retval = startup(tty, state);
-	tty_unlock();
+	tty_unlock(tty);
 	return retval;
 }
 
@@ -1330,7 +1330,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 {
 	struct serial_state *info = tty->driver_data;
 	unsigned long flags;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 
 	change_speed(tty, info, old_termios);
 
@@ -1347,7 +1347,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	if (!(old_termios->c_cflag & CBAUD) &&
 	    (cflag & CBAUD)) {
 		info->MCR |= SER_DTR;
-		if (!(tty->termios->c_cflag & CRTSCTS) || 
+		if (!(tty->termios.c_cflag & CRTSCTS) || 
 		    !test_bit(TTY_THROTTLED, &tty->flags)) {
 			info->MCR |= SER_RTS;
 		}
@@ -1358,7 +1358,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle turning off CRTSCTS */
 	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		rs_start(tty);
 	}
@@ -1371,7 +1371,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	 * or not.  Hence, this may change.....
 	 */
 	if (!(old_termios->c_cflag & CLOCAL) &&
-	    (tty->termios->c_cflag & CLOCAL))
+	    (tty->termios.c_cflag & CLOCAL))
 		wake_up_interruptible(&info->open_wait);
 #endif
 }
@@ -1710,10 +1710,6 @@ static int __init amiga_serial_probe(struct platform_device *pdev)
 	serial_driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(serial_driver, &serial_ops);
 
-	error = tty_register_driver(serial_driver);
-	if (error)
-		goto fail_put_tty_driver;
-
 	state = rs_table;
 	state->port = (int)&custom.serdatr; /* Just to give it a value */
 	state->custom_divisor = 0;
@@ -1724,6 +1720,11 @@ static int __init amiga_serial_probe(struct platform_device *pdev)
 	state->icount.overrun = state->icount.brk = 0;
 	tty_port_init(&state->tport);
 	state->tport.ops = &amiga_port_ops;
+	tty_port_link_device(&state->tport, serial_driver, 0);
+
+	error = tty_register_driver(serial_driver);
+	if (error)
+		goto fail_put_tty_driver;
 
 	printk(KERN_INFO "ttyS0 is the amiga builtin serial port\n");
 
diff --git a/drivers/tty/bfin_jtag_comm.c b/drivers/tty/bfin_jtag_comm.c
index 61fc74fe1747..02b7d3a09696 100644
--- a/drivers/tty/bfin_jtag_comm.c
+++ b/drivers/tty/bfin_jtag_comm.c
@@ -263,6 +263,7 @@ static int __init bfin_jc_init(void)
 	bfin_jc_driver->subtype      = SERIAL_TYPE_NORMAL;
 	bfin_jc_driver->init_termios = tty_std_termios;
 	tty_set_operations(bfin_jc_driver, &bfin_jc_ops);
+	tty_port_link_device(&port, bfin_jc_driver, 0);
 
 	ret = tty_register_driver(bfin_jc_driver);
 	if (ret)
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index e61cabdd69df..0a6a0bc1b598 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -727,7 +727,7 @@ static void cyy_chip_modem(struct cyclades_card *cinfo, int chip,
 		else
 			tty_hangup(tty);
 	}
-	if ((mdm_change & CyCTS) && (info->port.flags & ASYNC_CTS_FLOW)) {
+	if ((mdm_change & CyCTS) && tty_port_cts_enabled(&info->port)) {
 		if (tty->hw_stopped) {
 			if (mdm_status & CyCTS) {
 				/* cy_start isn't used
@@ -1459,7 +1459,7 @@ static void cy_shutdown(struct cyclades_port *info, struct tty_struct *tty)
 			info->port.xmit_buf = NULL;
 			free_page((unsigned long)temp);
 		}
-		if (tty->termios->c_cflag & HUPCL)
+		if (tty->termios.c_cflag & HUPCL)
 			cyy_change_rts_dtr(info, 0, TIOCM_RTS | TIOCM_DTR);
 
 		cyy_issue_cmd(info, CyCHAN_CTL | CyDIS_RCVR);
@@ -1488,7 +1488,7 @@ static void cy_shutdown(struct cyclades_port *info, struct tty_struct *tty)
 			free_page((unsigned long)temp);
 		}
 
-		if (tty->termios->c_cflag & HUPCL)
+		if (tty->termios.c_cflag & HUPCL)
 			tty_port_lower_dtr_rts(&info->port);
 
 		set_bit(TTY_IO_ERROR, &tty->flags);
@@ -1599,7 +1599,7 @@ static int cy_open(struct tty_struct *tty, struct file *filp)
 	 * If the port is the middle of closing, bail out now
 	 */
 	if (tty_hung_up_p(filp) || (info->port.flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->port.close_wait,
+		wait_event_interruptible_tty(tty, info->port.close_wait,
 				!(info->port.flags & ASYNC_CLOSING));
 		return (info->port.flags & ASYNC_HUP_NOTIFY) ? -EAGAIN: -ERESTARTSYS;
 	}
@@ -1999,14 +1999,11 @@ static void cy_set_line_char(struct cyclades_port *info, struct tty_struct *tty)
 	int baud, baud_rate = 0;
 	int i;
 
-	if (!tty->termios) /* XXX can this happen at all? */
-		return;
-
 	if (info->line == -1)
 		return;
 
-	cflag = tty->termios->c_cflag;
-	iflag = tty->termios->c_iflag;
+	cflag = tty->termios.c_cflag;
+	iflag = tty->termios.c_iflag;
 
 	/*
 	 * Set up the tty->alt_speed kludge
@@ -2825,7 +2822,7 @@ static void cy_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	cy_set_line_char(info, tty);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
-			!(tty->termios->c_cflag & CRTSCTS)) {
+			!(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		cy_start(tty);
 	}
@@ -2837,7 +2834,7 @@ static void cy_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	 * or not.  Hence, this may change.....
 	 */
 	if (!(old_termios->c_cflag & CLOCAL) &&
-	    (tty->termios->c_cflag & CLOCAL))
+	    (tty->termios.c_cflag & CLOCAL))
 		wake_up_interruptible(&info->port.open_wait);
 #endif
 }				/* cy_set_termios */
@@ -2899,7 +2896,7 @@ static void cy_throttle(struct tty_struct *tty)
 			info->throttle = 1;
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		if (!cy_is_Z(card)) {
 			spin_lock_irqsave(&card->card_lock, flags);
 			cyy_change_rts_dtr(info, 0, TIOCM_RTS);
@@ -2938,7 +2935,7 @@ static void cy_unthrottle(struct tty_struct *tty)
 			cy_send_xchar(tty, START_CHAR(tty));
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		card = info->card;
 		if (!cy_is_Z(card)) {
 			spin_lock_irqsave(&card->card_lock, flags);
@@ -3289,9 +3286,10 @@ static unsigned short __devinit cyy_init_card(void __iomem *true_base_addr,
 static int __init cy_detect_isa(void)
 {
 #ifdef CONFIG_ISA
+	struct cyclades_card *card;
 	unsigned short cy_isa_irq, nboard;
 	void __iomem *cy_isa_address;
-	unsigned short i, j, cy_isa_nchan;
+	unsigned short i, j, k, cy_isa_nchan;
 	int isparam = 0;
 
 	nboard = 0;
@@ -3349,7 +3347,8 @@ static int __init cy_detect_isa(void)
 		}
 		/* fill the next cy_card structure available */
 		for (j = 0; j < NR_CARDS; j++) {
-			if (cy_card[j].base_addr == NULL)
+			card = &cy_card[j];
+			if (card->base_addr == NULL)
 				break;
 		}
 		if (j == NR_CARDS) {	/* no more cy_cards available */
@@ -3363,7 +3362,7 @@ static int __init cy_detect_isa(void)
 
 		/* allocate IRQ */
 		if (request_irq(cy_isa_irq, cyy_interrupt,
-				0, "Cyclom-Y", &cy_card[j])) {
+				0, "Cyclom-Y", card)) {
 			printk(KERN_ERR "Cyclom-Y/ISA found at 0x%lx, but "
 				"could not allocate IRQ#%d.\n",
 				(unsigned long)cy_isa_address, cy_isa_irq);
@@ -3372,16 +3371,16 @@ static int __init cy_detect_isa(void)
 		}
 
 		/* set cy_card */
-		cy_card[j].base_addr = cy_isa_address;
-		cy_card[j].ctl_addr.p9050 = NULL;
-		cy_card[j].irq = (int)cy_isa_irq;
-		cy_card[j].bus_index = 0;
-		cy_card[j].first_line = cy_next_channel;
-		cy_card[j].num_chips = cy_isa_nchan / CyPORTS_PER_CHIP;
-		cy_card[j].nports = cy_isa_nchan;
-		if (cy_init_card(&cy_card[j])) {
-			cy_card[j].base_addr = NULL;
-			free_irq(cy_isa_irq, &cy_card[j]);
+		card->base_addr = cy_isa_address;
+		card->ctl_addr.p9050 = NULL;
+		card->irq = (int)cy_isa_irq;
+		card->bus_index = 0;
+		card->first_line = cy_next_channel;
+		card->num_chips = cy_isa_nchan / CyPORTS_PER_CHIP;
+		card->nports = cy_isa_nchan;
+		if (cy_init_card(card)) {
+			card->base_addr = NULL;
+			free_irq(cy_isa_irq, card);
 			iounmap(cy_isa_address);
 			continue;
 		}
@@ -3393,9 +3392,10 @@ static int __init cy_detect_isa(void)
 			(unsigned long)(cy_isa_address + (CyISA_Ywin - 1)),
 			cy_isa_irq, cy_isa_nchan, cy_next_channel);
 
-		for (j = cy_next_channel;
-				j < cy_next_channel + cy_isa_nchan; j++)
-			tty_register_device(cy_serial_driver, j, NULL);
+		for (k = 0, j = cy_next_channel;
+				j < cy_next_channel + cy_isa_nchan; j++, k++)
+			tty_port_register_device(&card->ports[k].port,
+					cy_serial_driver, j, NULL);
 		cy_next_channel += cy_isa_nchan;
 	}
 	return nboard;
@@ -3695,10 +3695,11 @@ err:
 static int __devinit cy_pci_probe(struct pci_dev *pdev,
 		const struct pci_device_id *ent)
 {
+	struct cyclades_card *card;
 	void __iomem *addr0 = NULL, *addr2 = NULL;
 	char *card_name = NULL;
 	u32 uninitialized_var(mailbox);
-	unsigned int device_id, nchan = 0, card_no, i;
+	unsigned int device_id, nchan = 0, card_no, i, j;
 	unsigned char plx_ver;
 	int retval, irq;
 
@@ -3829,7 +3830,8 @@ static int __devinit cy_pci_probe(struct pci_dev *pdev,
 	}
 	/* fill the next cy_card structure available */
 	for (card_no = 0; card_no < NR_CARDS; card_no++) {
-		if (cy_card[card_no].base_addr == NULL)
+		card = &cy_card[card_no];
+		if (card->base_addr == NULL)
 			break;
 	}
 	if (card_no == NR_CARDS) {	/* no more cy_cards available */
@@ -3843,27 +3845,26 @@ static int __devinit cy_pci_probe(struct pci_dev *pdev,
 			device_id == PCI_DEVICE_ID_CYCLOM_Y_Hi) {
 		/* allocate IRQ */
 		retval = request_irq(irq, cyy_interrupt,
-				IRQF_SHARED, "Cyclom-Y", &cy_card[card_no]);
+				IRQF_SHARED, "Cyclom-Y", card);
 		if (retval) {
 			dev_err(&pdev->dev, "could not allocate IRQ\n");
 			goto err_unmap;
 		}
-		cy_card[card_no].num_chips = nchan / CyPORTS_PER_CHIP;
+		card->num_chips = nchan / CyPORTS_PER_CHIP;
 	} else {
 		struct FIRM_ID __iomem *firm_id = addr2 + ID_ADDRESS;
 		struct ZFW_CTRL __iomem *zfw_ctrl;
 
 		zfw_ctrl = addr2 + (readl(&firm_id->zfwctrl_addr) & 0xfffff);
 
-		cy_card[card_no].hw_ver = mailbox;
-		cy_card[card_no].num_chips = (unsigned int)-1;
-		cy_card[card_no].board_ctrl = &zfw_ctrl->board_ctrl;
+		card->hw_ver = mailbox;
+		card->num_chips = (unsigned int)-1;
+		card->board_ctrl = &zfw_ctrl->board_ctrl;
 #ifdef CONFIG_CYZ_INTR
 		/* allocate IRQ only if board has an IRQ */
 		if (irq != 0 && irq != 255) {
 			retval = request_irq(irq, cyz_interrupt,
-					IRQF_SHARED, "Cyclades-Z",
-					&cy_card[card_no]);
+					IRQF_SHARED, "Cyclades-Z", card);
 			if (retval) {
 				dev_err(&pdev->dev, "could not allocate IRQ\n");
 				goto err_unmap;
@@ -3873,17 +3874,17 @@ static int __devinit cy_pci_probe(struct pci_dev *pdev,
 	}
 
 	/* set cy_card */
-	cy_card[card_no].base_addr = addr2;
-	cy_card[card_no].ctl_addr.p9050 = addr0;
-	cy_card[card_no].irq = irq;
-	cy_card[card_no].bus_index = 1;
-	cy_card[card_no].first_line = cy_next_channel;
-	cy_card[card_no].nports = nchan;
-	retval = cy_init_card(&cy_card[card_no]);
+	card->base_addr = addr2;
+	card->ctl_addr.p9050 = addr0;
+	card->irq = irq;
+	card->bus_index = 1;
+	card->first_line = cy_next_channel;
+	card->nports = nchan;
+	retval = cy_init_card(card);
 	if (retval)
 		goto err_null;
 
-	pci_set_drvdata(pdev, &cy_card[card_no]);
+	pci_set_drvdata(pdev, card);
 
 	if (device_id == PCI_DEVICE_ID_CYCLOM_Y_Lo ||
 			device_id == PCI_DEVICE_ID_CYCLOM_Y_Hi) {
@@ -3909,14 +3910,15 @@ static int __devinit cy_pci_probe(struct pci_dev *pdev,
 
 	dev_info(&pdev->dev, "%s/PCI #%d found: %d channels starting from "
 		"port %d.\n", card_name, card_no + 1, nchan, cy_next_channel);
-	for (i = cy_next_channel; i < cy_next_channel + nchan; i++)
-		tty_register_device(cy_serial_driver, i, &pdev->dev);
+	for (j = 0, i = cy_next_channel; i < cy_next_channel + nchan; i++, j++)
+		tty_port_register_device(&card->ports[j].port,
+				cy_serial_driver, i, &pdev->dev);
 	cy_next_channel += nchan;
 
 	return 0;
 err_null:
-	cy_card[card_no].base_addr = NULL;
-	free_irq(irq, &cy_card[card_no]);
+	card->base_addr = NULL;
+	free_irq(irq, card);
 err_unmap:
 	iounmap(addr0);
 	if (addr2)
diff --git a/drivers/tty/ehv_bytechan.c b/drivers/tty/ehv_bytechan.c
index 4813684cb634..4ab936b7aac6 100644
--- a/drivers/tty/ehv_bytechan.c
+++ b/drivers/tty/ehv_bytechan.c
@@ -738,16 +738,17 @@ static int __devinit ehv_bc_tty_probe(struct platform_device *pdev)
 		goto error;
 	}
 
-	bc->dev = tty_register_device(ehv_bc_driver, i, &pdev->dev);
+	tty_port_init(&bc->port);
+	bc->port.ops = &ehv_bc_tty_port_ops;
+
+	bc->dev = tty_port_register_device(&bc->port, ehv_bc_driver, i,
+			&pdev->dev);
 	if (IS_ERR(bc->dev)) {
 		ret = PTR_ERR(bc->dev);
 		dev_err(&pdev->dev, "could not register tty (ret=%i)\n", ret);
 		goto error;
 	}
 
-	tty_port_init(&bc->port);
-	bc->port.ops = &ehv_bc_tty_port_ops;
-
 	dev_set_drvdata(&pdev->dev, bc);
 
 	dev_info(&pdev->dev, "registered /dev/%s%u for byte channel %u\n",
diff --git a/drivers/tty/hvc/Kconfig b/drivers/tty/hvc/Kconfig
index 0282a83f51fb..f47b734c6a7a 100644
--- a/drivers/tty/hvc/Kconfig
+++ b/drivers/tty/hvc/Kconfig
@@ -76,7 +76,7 @@ config HVC_XEN_FRONTEND
 
 config HVC_UDBG
        bool "udbg based fake hypervisor console"
-       depends on PPC && EXPERIMENTAL
+       depends on PPC
        select HVC_DRIVER
        default n
        help
diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 2d691eb7c40a..4a652999380f 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -299,20 +299,33 @@ static void hvc_unthrottle(struct tty_struct *tty)
 	hvc_kick();
 }
 
+static int hvc_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct hvc_struct *hp;
+	int rc;
+
+	/* Auto increments kref reference if found. */
+	if (!(hp = hvc_get_by_index(tty->index)))
+		return -ENODEV;
+
+	tty->driver_data = hp;
+
+	rc = tty_port_install(&hp->port, driver, tty);
+	if (rc)
+		tty_port_put(&hp->port);
+	return rc;
+}
+
 /*
  * The TTY interface won't be used until after the vio layer has exposed the vty
  * adapter to the kernel.
  */
 static int hvc_open(struct tty_struct *tty, struct file * filp)
 {
-	struct hvc_struct *hp;
+	struct hvc_struct *hp = tty->driver_data;
 	unsigned long flags;
 	int rc = 0;
 
-	/* Auto increments kref reference if found. */
-	if (!(hp = hvc_get_by_index(tty->index)))
-		return -ENODEV;
-
 	spin_lock_irqsave(&hp->port.lock, flags);
 	/* Check and then increment for fast path open. */
 	if (hp->port.count++ > 0) {
@@ -322,7 +335,6 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 	} /* else count == 0 */
 	spin_unlock_irqrestore(&hp->port.lock, flags);
 
-	tty->driver_data = hp;
 	tty_port_tty_set(&hp->port, tty);
 
 	if (hp->ops->notifier_add)
@@ -389,6 +401,11 @@ static void hvc_close(struct tty_struct *tty, struct file * filp)
 				hp->vtermno, hp->port.count);
 		spin_unlock_irqrestore(&hp->port.lock, flags);
 	}
+}
+
+static void hvc_cleanup(struct tty_struct *tty)
+{
+	struct hvc_struct *hp = tty->driver_data;
 
 	tty_port_put(&hp->port);
 }
@@ -541,7 +558,7 @@ static int hvc_write_room(struct tty_struct *tty)
 	struct hvc_struct *hp = tty->driver_data;
 
 	if (!hp)
-		return -1;
+		return 0;
 
 	return hp->outbuf_size - hp->n_outbuf;
 }
@@ -792,8 +809,10 @@ static void hvc_poll_put_char(struct tty_driver *driver, int line, char ch)
 #endif
 
 static const struct tty_operations hvc_ops = {
+	.install = hvc_install,
 	.open = hvc_open,
 	.close = hvc_close,
+	.cleanup = hvc_cleanup,
 	.write = hvc_write,
 	.hangup = hvc_hangup,
 	.unthrottle = hvc_unthrottle,
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index d56788c83974..cab5c7adf8e8 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -1102,27 +1102,20 @@ static struct hvcs_struct *hvcs_get_by_index(int index)
 	return NULL;
 }
 
-/*
- * This is invoked via the tty_open interface when a user app connects to the
- * /dev node.
- */
-static int hvcs_open(struct tty_struct *tty, struct file *filp)
+static int hvcs_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	struct hvcs_struct *hvcsd;
-	int rc, retval = 0;
-	unsigned long flags;
-	unsigned int irq;
 	struct vio_dev *vdev;
-	unsigned long unit_address;
-
-	if (tty->driver_data)
-		goto fast_open;
+	unsigned long unit_address, flags;
+	unsigned int irq;
+	int retval;
 
 	/*
 	 * Is there a vty-server that shares the same index?
 	 * This function increments the kref index.
 	 */
-	if (!(hvcsd = hvcs_get_by_index(tty->index))) {
+	hvcsd = hvcs_get_by_index(tty->index);
+	if (!hvcsd) {
 		printk(KERN_WARNING "HVCS: open failed, no device associated"
 				" with tty->index %d.\n", tty->index);
 		return -ENODEV;
@@ -1130,11 +1123,16 @@ static int hvcs_open(struct tty_struct *tty, struct file *filp)
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
 
-	if (hvcsd->connected == 0)
-		if ((retval = hvcs_partner_connect(hvcsd)))
-			goto error_release;
+	if (hvcsd->connected == 0) {
+		retval = hvcs_partner_connect(hvcsd);
+		if (retval) {
+			spin_unlock_irqrestore(&hvcsd->lock, flags);
+			printk(KERN_WARNING "HVCS: partner connect failed.\n");
+			goto err_put;
+		}
+	}
 
-	hvcsd->port.count = 1;
+	hvcsd->port.count = 0;
 	hvcsd->port.tty = tty;
 	tty->driver_data = hvcsd;
 
@@ -1155,37 +1153,48 @@ static int hvcs_open(struct tty_struct *tty, struct file *filp)
 	 * This must be done outside of the spinlock because it requests irqs
 	 * and will grab the spinlock and free the connection if it fails.
 	 */
-	if (((rc = hvcs_enable_device(hvcsd, unit_address, irq, vdev)))) {
-		tty_port_put(&hvcsd->port);
+	retval = hvcs_enable_device(hvcsd, unit_address, irq, vdev);
+	if (retval) {
 		printk(KERN_WARNING "HVCS: enable device failed.\n");
-		return rc;
+		goto err_put;
 	}
 
-	goto open_success;
+	retval = tty_port_install(&hvcsd->port, driver, tty);
+	if (retval)
+		goto err_irq;
 
-fast_open:
-	hvcsd = tty->driver_data;
+	return 0;
+err_irq:
+	spin_lock_irqsave(&hvcsd->lock, flags);
+	vio_disable_interrupts(hvcsd->vdev);
+	spin_unlock_irqrestore(&hvcsd->lock, flags);
+	free_irq(irq, hvcsd);
+err_put:
+	tty_port_put(&hvcsd->port);
+
+	return retval;
+}
+
+/*
+ * This is invoked via the tty_open interface when a user app connects to the
+ * /dev node.
+ */
+static int hvcs_open(struct tty_struct *tty, struct file *filp)
+{
+	struct hvcs_struct *hvcsd = tty->driver_data;
+	unsigned long flags;
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
-	tty_port_get(&hvcsd->port);
 	hvcsd->port.count++;
 	hvcsd->todo_mask |= HVCS_SCHED_READ;
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
 
-open_success:
 	hvcs_kick();
 
 	printk(KERN_INFO "HVCS: vty-server@%X connection opened.\n",
 		hvcsd->vdev->unit_address );
 
 	return 0;
-
-error_release:
-	spin_unlock_irqrestore(&hvcsd->lock, flags);
-	tty_port_put(&hvcsd->port);
-
-	printk(KERN_WARNING "HVCS: partner connect failed.\n");
-	return retval;
 }
 
 static void hvcs_close(struct tty_struct *tty, struct file *filp)
@@ -1236,7 +1245,6 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 		tty->driver_data = NULL;
 
 		free_irq(irq, hvcsd);
-		tty_port_put(&hvcsd->port);
 		return;
 	} else if (hvcsd->port.count < 0) {
 		printk(KERN_ERR "HVCS: vty-server@%X open_count: %d"
@@ -1245,6 +1253,12 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 	}
 
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
+}
+
+static void hvcs_cleanup(struct tty_struct * tty)
+{
+	struct hvcs_struct *hvcsd = tty->driver_data;
+
 	tty_port_put(&hvcsd->port);
 }
 
@@ -1431,8 +1445,10 @@ static int hvcs_chars_in_buffer(struct tty_struct *tty)
 }
 
 static const struct tty_operations hvcs_ops = {
+	.install = hvcs_install,
 	.open = hvcs_open,
 	.close = hvcs_close,
+	.cleanup = hvcs_cleanup,
 	.hangup = hvcs_hangup,
 	.write = hvcs_write,
 	.write_room = hvcs_write_room,
diff --git a/drivers/tty/hvc/hvsi.c b/drivers/tty/hvc/hvsi.c
index 6f5bc49c441f..0083bc1f63f4 100644
--- a/drivers/tty/hvc/hvsi.c
+++ b/drivers/tty/hvc/hvsi.c
@@ -1080,6 +1080,8 @@ static int __init hvsi_init(void)
 		struct hvsi_struct *hp = &hvsi_ports[i];
 		int ret = 1;
 
+		tty_port_link_device(&hp->port, hvsi_driver, i);
+
 		ret = request_irq(hp->virq, hvsi_interrupt, 0, "hvsi", hp);
 		if (ret)
 			printk(KERN_ERR "HVSI: couldn't reserve irq 0x%x (error %i)\n",
diff --git a/drivers/tty/hvc/hvsi_lib.c b/drivers/tty/hvc/hvsi_lib.c
index 59c135dd5d20..3396eb9d57a3 100644
--- a/drivers/tty/hvc/hvsi_lib.c
+++ b/drivers/tty/hvc/hvsi_lib.c
@@ -400,7 +400,7 @@ void hvsilib_close(struct hvsi_priv *pv, struct hvc_struct *hp)
 		spin_unlock_irqrestore(&hp->lock, flags);
 
 		/* Clear our own DTR */
-		if (!pv->tty || (pv->tty->termios->c_cflag & HUPCL))
+		if (!pv->tty || (pv->tty->termios.c_cflag & HUPCL))
 			hvsilib_write_mctrl(pv, 0);
 
 		/* Tear down the connection */
diff --git a/drivers/tty/ipwireless/network.c b/drivers/tty/ipwireless/network.c
index 57c8b481113f..d2af155dec8b 100644
--- a/drivers/tty/ipwireless/network.c
+++ b/drivers/tty/ipwireless/network.c
@@ -274,7 +274,12 @@ static void do_go_online(struct work_struct *work_go_online)
 		network->xaccm[0] = ~0U;
 		network->xaccm[3] = 0x60000000U;
 		network->raccm = ~0U;
-		ppp_register_channel(channel);
+		if (ppp_register_channel(channel) < 0) {
+			printk(KERN_ERR IPWIRELESS_PCCARD_NAME
+					": unable to register PPP channel\n");
+			kfree(channel);
+			return;
+		}
 		spin_lock_irqsave(&network->lock, flags);
 		network->ppp_channel = channel;
 	}
diff --git a/drivers/tty/ipwireless/tty.c b/drivers/tty/ipwireless/tty.c
index f8b5fa0093a3..160f0ad9589d 100644
--- a/drivers/tty/ipwireless/tty.c
+++ b/drivers/tty/ipwireless/tty.c
@@ -476,7 +476,7 @@ static int add_tty(int j,
 	mutex_init(&ttys[j]->ipw_tty_mutex);
 	tty_port_init(&ttys[j]->port);
 
-	tty_register_device(ipw_tty_driver, j, NULL);
+	tty_port_register_device(&ttys[j]->port, ipw_tty_driver, j, NULL);
 	ipwireless_associate_network_tty(network, channel_idx, ttys[j]);
 
 	if (secondary_channel_idx != -1)
diff --git a/drivers/tty/isicom.c b/drivers/tty/isicom.c
index e1235accab74..d7492e183607 100644
--- a/drivers/tty/isicom.c
+++ b/drivers/tty/isicom.c
@@ -600,7 +600,7 @@ static irqreturn_t isicom_interrupt(int irq, void *dev_id)
 					port->status &= ~ISI_DCD;
 			}
 
-			if (port->port.flags & ASYNC_CTS_FLOW) {
+			if (tty_port_cts_enabled(&port->port)) {
 				if (tty->hw_stopped) {
 					if (header & ISI_CTS) {
 						port->port.tty->hw_stopped = 0;
@@ -702,7 +702,7 @@ static void isicom_config_port(struct tty_struct *tty)
 
 		/* 1,2,3,4 => 57.6, 115.2, 230, 460 kbps resp. */
 		if (baud < 1 || baud > 4)
-			tty->termios->c_cflag &= ~CBAUDEX;
+			tty->termios.c_cflag &= ~CBAUDEX;
 		else
 			baud += 15;
 	}
@@ -1196,8 +1196,8 @@ static void isicom_set_termios(struct tty_struct *tty,
 	if (isicom_paranoia_check(port, tty->name, "isicom_set_termios"))
 		return;
 
-	if (tty->termios->c_cflag == old_termios->c_cflag &&
-			tty->termios->c_iflag == old_termios->c_iflag)
+	if (tty->termios.c_cflag == old_termios->c_cflag &&
+			tty->termios.c_iflag == old_termios->c_iflag)
 		return;
 
 	spin_lock_irqsave(&port->card->card_lock, flags);
@@ -1205,7 +1205,7 @@ static void isicom_set_termios(struct tty_struct *tty,
 	spin_unlock_irqrestore(&port->card->card_lock, flags);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
-			!(tty->termios->c_cflag & CRTSCTS)) {
+			!(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		isicom_start(tty);
 	}
@@ -1611,7 +1611,8 @@ static int __devinit isicom_probe(struct pci_dev *pdev,
 		goto errunri;
 
 	for (index = 0; index < board->port_count; index++)
-		tty_register_device(isicom_normal, board->index * 16 + index,
+		tty_port_register_device(&board->ports[index].port,
+				isicom_normal, board->index * 16 + index,
 				&pdev->dev);
 
 	return 0;
diff --git a/drivers/tty/moxa.c b/drivers/tty/moxa.c
index 324467d28a54..56e616b9109a 100644
--- a/drivers/tty/moxa.c
+++ b/drivers/tty/moxa.c
@@ -169,6 +169,7 @@ static DEFINE_SPINLOCK(moxa_lock);
 static unsigned long baseaddr[MAX_BOARDS];
 static unsigned int type[MAX_BOARDS];
 static unsigned int numports[MAX_BOARDS];
+static struct tty_port moxa_service_port;
 
 MODULE_AUTHOR("William Chen");
 MODULE_DESCRIPTION("MOXA Intellio Family Multiport Board Device Driver");
@@ -367,10 +368,10 @@ static int moxa_ioctl(struct tty_struct *tty,
 					tmp.dcd = 1;
 
 				ttyp = tty_port_tty_get(&p->port);
-				if (!ttyp || !ttyp->termios)
+				if (!ttyp)
 					tmp.cflag = p->cflag;
 				else
-					tmp.cflag = ttyp->termios->c_cflag;
+					tmp.cflag = ttyp->termios.c_cflag;
 				tty_kref_put(ttyp);
 copy:
 				if (copy_to_user(argm, &tmp, sizeof(tmp)))
@@ -834,7 +835,7 @@ static int moxa_init_board(struct moxa_board_conf *brd, struct device *dev)
 	const struct firmware *fw;
 	const char *file;
 	struct moxa_port *p;
-	unsigned int i;
+	unsigned int i, first_idx;
 	int ret;
 
 	brd->ports = kcalloc(MAX_PORTS_PER_BOARD, sizeof(*brd->ports),
@@ -887,6 +888,11 @@ static int moxa_init_board(struct moxa_board_conf *brd, struct device *dev)
 		mod_timer(&moxaTimer, jiffies + HZ / 50);
 	spin_unlock_bh(&moxa_lock);
 
+	first_idx = (brd - moxa_boards) * MAX_PORTS_PER_BOARD;
+	for (i = 0; i < brd->numPorts; i++)
+		tty_port_register_device(&brd->ports[i].port, moxaDriver,
+				first_idx + i, dev);
+
 	return 0;
 err_free:
 	kfree(brd->ports);
@@ -896,7 +902,7 @@ err:
 
 static void moxa_board_deinit(struct moxa_board_conf *brd)
 {
-	unsigned int a, opened;
+	unsigned int a, opened, first_idx;
 
 	mutex_lock(&moxa_openlock);
 	spin_lock_bh(&moxa_lock);
@@ -925,6 +931,10 @@ static void moxa_board_deinit(struct moxa_board_conf *brd)
 		mutex_lock(&moxa_openlock);
 	}
 
+	first_idx = (brd - moxa_boards) * MAX_PORTS_PER_BOARD;
+	for (a = 0; a < brd->numPorts; a++)
+		tty_unregister_device(moxaDriver, first_idx + a);
+
 	iounmap(brd->basemem);
 	brd->basemem = NULL;
 	kfree(brd->ports);
@@ -967,6 +977,7 @@ static int __devinit moxa_pci_probe(struct pci_dev *pdev,
 	board->basemem = ioremap_nocache(pci_resource_start(pdev, 2), 0x4000);
 	if (board->basemem == NULL) {
 		dev_err(&pdev->dev, "can't remap io space 2\n");
+		retval = -ENOMEM;
 		goto err_reg;
 	}
 
@@ -1031,9 +1042,14 @@ static int __init moxa_init(void)
 
 	printk(KERN_INFO "MOXA Intellio family driver version %s\n",
 			MOXA_VERSION);
-	moxaDriver = alloc_tty_driver(MAX_PORTS + 1);
-	if (!moxaDriver)
-		return -ENOMEM;
+
+	tty_port_init(&moxa_service_port);
+
+	moxaDriver = tty_alloc_driver(MAX_PORTS + 1,
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_DEV);
+	if (IS_ERR(moxaDriver))
+		return PTR_ERR(moxaDriver);
 
 	moxaDriver->name = "ttyMX";
 	moxaDriver->major = ttymajor;
@@ -1044,8 +1060,9 @@ static int __init moxa_init(void)
 	moxaDriver->init_termios.c_cflag = B9600 | CS8 | CREAD | CLOCAL | HUPCL;
 	moxaDriver->init_termios.c_ispeed = 9600;
 	moxaDriver->init_termios.c_ospeed = 9600;
-	moxaDriver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(moxaDriver, &moxa_ops);
+	/* Having one more port only for ioctls is ugly */
+	tty_port_link_device(&moxa_service_port, moxaDriver, MAX_PORTS);
 
 	if (tty_register_driver(moxaDriver)) {
 		printk(KERN_ERR "can't register MOXA Smartio tty driver!\n");
@@ -1178,7 +1195,7 @@ static int moxa_open(struct tty_struct *tty, struct file *filp)
 	mutex_lock(&ch->port.mutex);
 	if (!(ch->port.flags & ASYNC_INITIALIZED)) {
 		ch->statusflags = 0;
-		moxa_set_tty_param(tty, tty->termios);
+		moxa_set_tty_param(tty, &tty->termios);
 		MoxaPortLineCtrl(ch, 1, 1);
 		MoxaPortEnable(ch);
 		MoxaSetFifo(ch, ch->type == PORT_16550A);
@@ -1193,7 +1210,7 @@ static int moxa_open(struct tty_struct *tty, struct file *filp)
 static void moxa_close(struct tty_struct *tty, struct file *filp)
 {
 	struct moxa_port *ch = tty->driver_data;
-	ch->cflag = tty->termios->c_cflag;
+	ch->cflag = tty->termios.c_cflag;
 	tty_port_close(&ch->port, tty, filp);
 }
 
@@ -1464,7 +1481,7 @@ static void moxa_poll(unsigned long ignored)
 
 static void moxa_set_tty_param(struct tty_struct *tty, struct ktermios *old_termios)
 {
-	register struct ktermios *ts = tty->termios;
+	register struct ktermios *ts = &tty->termios;
 	struct moxa_port *ch = tty->driver_data;
 	int rts, cts, txflow, rxflow, xany, baud;
 
diff --git a/drivers/tty/mxser.c b/drivers/tty/mxser.c
index 90cc680c4f0e..cfda47dabd28 100644
--- a/drivers/tty/mxser.c
+++ b/drivers/tty/mxser.c
@@ -643,7 +643,7 @@ static int mxser_change_speed(struct tty_struct *tty,
 	int ret = 0;
 	unsigned char status;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 	if (!info->ioaddr)
 		return ret;
 
@@ -830,7 +830,7 @@ static void mxser_check_modem_status(struct tty_struct *tty,
 			wake_up_interruptible(&port->port.open_wait);
 	}
 
-	if (port->port.flags & ASYNC_CTS_FLOW) {
+	if (tty_port_cts_enabled(&port->port)) {
 		if (tty->hw_stopped) {
 			if (status & UART_MSR_CTS) {
 				tty->hw_stopped = 0;
@@ -1520,10 +1520,10 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 				
 				tty = tty_port_tty_get(port);
 
-				if (!tty || !tty->termios)
+				if (!tty)
 					ms.cflag = ip->normal_termios.c_cflag;
 				else
-					ms.cflag = tty->termios->c_cflag;
+					ms.cflag = tty->termios.c_cflag;
 				tty_kref_put(tty);
 				spin_lock_irq(&ip->slock);
 				status = inb(ip->ioaddr + UART_MSR);
@@ -1589,13 +1589,13 @@ static int mxser_ioctl_special(unsigned int cmd, void __user *argp)
 
 				tty = tty_port_tty_get(&ip->port);
 
-				if (!tty || !tty->termios) {
+				if (!tty) {
 					cflag = ip->normal_termios.c_cflag;
 					iflag = ip->normal_termios.c_iflag;
 					me->baudrate[p] = tty_termios_baud_rate(&ip->normal_termios);
 				} else {
-					cflag = tty->termios->c_cflag;
-					iflag = tty->termios->c_iflag;
+					cflag = tty->termios.c_cflag;
+					iflag = tty->termios.c_iflag;
 					me->baudrate[p] = tty_get_baud_rate(tty);
 				}
 				tty_kref_put(tty);
@@ -1853,7 +1853,7 @@ static void mxser_stoprx(struct tty_struct *tty)
 		}
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		info->MCR &= ~UART_MCR_RTS;
 		outb(info->MCR, info->ioaddr + UART_MCR);
 	}
@@ -1890,7 +1890,7 @@ static void mxser_unthrottle(struct tty_struct *tty)
 		}
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		info->MCR |= UART_MCR_RTS;
 		outb(info->MCR, info->ioaddr + UART_MCR);
 	}
@@ -1939,14 +1939,14 @@ static void mxser_set_termios(struct tty_struct *tty, struct ktermios *old_termi
 	spin_unlock_irqrestore(&info->slock, flags);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
-			!(tty->termios->c_cflag & CRTSCTS)) {
+			!(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		mxser_start(tty);
 	}
 
 	/* Handle sw stopped */
 	if ((old_termios->c_iflag & IXON) &&
-			!(tty->termios->c_iflag & IXON)) {
+			!(tty->termios.c_iflag & IXON)) {
 		tty->stopped = 0;
 
 		if (info->board->chip_flag) {
@@ -2337,11 +2337,36 @@ static struct tty_port_operations mxser_port_ops = {
  * The MOXA Smartio/Industio serial driver boot-time initialization code!
  */
 
+static bool allow_overlapping_vector;
+module_param(allow_overlapping_vector, bool, S_IRUGO);
+MODULE_PARM_DESC(allow_overlapping_vector, "whether we allow ISA cards to be configured such that vector overlabs IO ports (default=no)");
+
+static bool mxser_overlapping_vector(struct mxser_board *brd)
+{
+	return allow_overlapping_vector &&
+		brd->vector >= brd->ports[0].ioaddr &&
+		brd->vector < brd->ports[0].ioaddr + 8 * brd->info->nports;
+}
+
+static int mxser_request_vector(struct mxser_board *brd)
+{
+	if (mxser_overlapping_vector(brd))
+		return 0;
+	return request_region(brd->vector, 1, "mxser(vector)") ? 0 : -EIO;
+}
+
+static void mxser_release_vector(struct mxser_board *brd)
+{
+	if (mxser_overlapping_vector(brd))
+		return;
+	release_region(brd->vector, 1);
+}
+
 static void mxser_release_ISA_res(struct mxser_board *brd)
 {
 	free_irq(brd->irq, brd);
 	release_region(brd->ports[0].ioaddr, 8 * brd->info->nports);
-	release_region(brd->vector, 1);
+	mxser_release_vector(brd);
 }
 
 static int __devinit mxser_initbrd(struct mxser_board *brd,
@@ -2396,7 +2421,7 @@ static int __devinit mxser_initbrd(struct mxser_board *brd,
 
 static int __init mxser_get_ISA_conf(int cap, struct mxser_board *brd)
 {
-	int id, i, bits;
+	int id, i, bits, ret;
 	unsigned short regs[16], irq;
 	unsigned char scratch, scratch2;
 
@@ -2492,13 +2517,15 @@ static int __init mxser_get_ISA_conf(int cap, struct mxser_board *brd)
 				8 * brd->info->nports - 1);
 		return -EIO;
 	}
-	if (!request_region(brd->vector, 1, "mxser(vector)")) {
+
+	ret = mxser_request_vector(brd);
+	if (ret) {
 		release_region(brd->ports[0].ioaddr, 8 * brd->info->nports);
 		printk(KERN_ERR "mxser: can't request interrupt vector region: "
 				"0x%.8lx-0x%.8lx\n",
 				brd->ports[0].ioaddr, brd->ports[0].ioaddr +
 				8 * brd->info->nports - 1);
-		return -EIO;
+		return ret;
 	}
 	return brd->info->nports;
 
@@ -2598,7 +2625,8 @@ static int __devinit mxser_probe(struct pci_dev *pdev,
 		goto err_rel3;
 
 	for (i = 0; i < brd->info->nports; i++)
-		tty_register_device(mxvar_sdriver, brd->idx + i, &pdev->dev);
+		tty_port_register_device(&brd->ports[i].port, mxvar_sdriver,
+				brd->idx + i, &pdev->dev);
 
 	pci_set_drvdata(pdev, brd);
 
@@ -2695,7 +2723,8 @@ static int __init mxser_module_init(void)
 
 		brd->idx = m * MXSER_PORTS_PER_BOARD;
 		for (i = 0; i < brd->info->nports; i++)
-			tty_register_device(mxvar_sdriver, brd->idx + i, NULL);
+			tty_port_register_device(&brd->ports[i].port,
+					mxvar_sdriver, brd->idx + i, NULL);
 
 		m++;
 	}
diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index c43b683b6eb8..1e8e8ce55959 100644
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -108,7 +108,7 @@ struct gsm_mux_net {
  */
 
 struct gsm_msg {
-	struct gsm_msg *next;
+	struct list_head list;
 	u8 addr;		/* DLCI address + flags */
 	u8 ctrl;		/* Control byte + flags */
 	unsigned int len;	/* Length of data block (can be zero) */
@@ -245,8 +245,7 @@ struct gsm_mux {
 	unsigned int tx_bytes;		/* TX data outstanding */
 #define TX_THRESH_HI		8192
 #define TX_THRESH_LO		2048
-	struct gsm_msg *tx_head;	/* Pending data packets */
-	struct gsm_msg *tx_tail;
+	struct list_head tx_list;	/* Pending data packets */
 
 	/* Control messages */
 	struct timer_list t2_timer;	/* Retransmit timer for commands */
@@ -489,7 +488,7 @@ static void gsm_print_packet(const char *hdr, int addr, int cr,
 	default:
 		if (!(control & 0x01)) {
 			pr_cont("I N(S)%d N(R)%d",
-				(control & 0x0E) >> 1, (control & 0xE) >> 5);
+				(control & 0x0E) >> 1, (control & 0xE0) >> 5);
 		} else switch (control & 0x0F) {
 			case RR:
 				pr_cont("RR(%d)", (control & 0xE0) >> 5);
@@ -663,7 +662,7 @@ static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
 	m->len = len;
 	m->addr = addr;
 	m->ctrl = ctrl;
-	m->next = NULL;
+	INIT_LIST_HEAD(&m->list);
 	return m;
 }
 
@@ -673,22 +672,21 @@ static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
  *
  *	The tty device has called us to indicate that room has appeared in
  *	the transmit queue. Ram more data into the pipe if we have any
+ *	If we have been flow-stopped by a CMD_FCOFF, then we can only
+ *	send messages on DLCI0 until CMD_FCON
  *
  *	FIXME: lock against link layer control transmissions
  */
 
 static void gsm_data_kick(struct gsm_mux *gsm)
 {
-	struct gsm_msg *msg = gsm->tx_head;
+	struct gsm_msg *msg, *nmsg;
 	int len;
 	int skip_sof = 0;
 
-	/* FIXME: We need to apply this solely to data messages */
-	if (gsm->constipated)
-		return;
-
-	while (gsm->tx_head != NULL) {
-		msg = gsm->tx_head;
+	list_for_each_entry_safe(msg, nmsg, &gsm->tx_list, list) {
+		if (gsm->constipated && msg->addr)
+			continue;
 		if (gsm->encoding != 0) {
 			gsm->txframe[0] = GSM1_SOF;
 			len = gsm_stuff_frame(msg->data,
@@ -711,14 +709,13 @@ static void gsm_data_kick(struct gsm_mux *gsm)
 						len - skip_sof) < 0)
 			break;
 		/* FIXME: Can eliminate one SOF in many more cases */
-		gsm->tx_head = msg->next;
-		if (gsm->tx_head == NULL)
-			gsm->tx_tail = NULL;
 		gsm->tx_bytes -= msg->len;
-		kfree(msg);
 		/* For a burst of frames skip the extra SOF within the
 		   burst */
 		skip_sof = 1;
+
+		list_del(&msg->list);
+		kfree(msg);
 	}
 }
 
@@ -768,11 +765,7 @@ static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
 	msg->data = dp;
 
 	/* Add to the actual output queue */
-	if (gsm->tx_tail)
-		gsm->tx_tail->next = msg;
-	else
-		gsm->tx_head = msg;
-	gsm->tx_tail = msg;
+	list_add_tail(&msg->list, &gsm->tx_list);
 	gsm->tx_bytes += msg->len;
 	gsm_data_kick(gsm);
 }
@@ -875,7 +868,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 
 	/* dlci->skb is locked by tx_lock */
 	if (dlci->skb == NULL) {
-		dlci->skb = skb_dequeue(&dlci->skb_list);
+		dlci->skb = skb_dequeue_tail(&dlci->skb_list);
 		if (dlci->skb == NULL)
 			return 0;
 		first = 1;
@@ -886,7 +879,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 	if (len > gsm->mtu) {
 		if (dlci->adaption == 3) {
 			/* Over long frame, bin it */
-			kfree_skb(dlci->skb);
+			dev_kfree_skb_any(dlci->skb);
 			dlci->skb = NULL;
 			return 0;
 		}
@@ -899,8 +892,11 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 
 	/* FIXME: need a timer or something to kick this so it can't
 	   get stuck with no work outstanding and no buffer free */
-	if (msg == NULL)
+	if (msg == NULL) {
+		skb_queue_tail(&dlci->skb_list, dlci->skb);
+		dlci->skb = NULL;
 		return -ENOMEM;
+	}
 	dp = msg->data;
 
 	if (dlci->adaption == 4) { /* Interruptible framed (Packetised Data) */
@@ -912,7 +908,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 	skb_pull(dlci->skb, len);
 	__gsm_data_queue(dlci, msg);
 	if (last) {
-		kfree_skb(dlci->skb);
+		dev_kfree_skb_any(dlci->skb);
 		dlci->skb = NULL;
 	}
 	return size;
@@ -971,16 +967,22 @@ static void gsm_dlci_data_sweep(struct gsm_mux *gsm)
 static void gsm_dlci_data_kick(struct gsm_dlci *dlci)
 {
 	unsigned long flags;
+	int sweep;
+
+	if (dlci->constipated) 
+		return;
 
 	spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
 	/* If we have nothing running then we need to fire up */
+	sweep = (dlci->gsm->tx_bytes < TX_THRESH_LO);
 	if (dlci->gsm->tx_bytes == 0) {
 		if (dlci->net)
 			gsm_dlci_data_output_framed(dlci->gsm, dlci);
 		else
 			gsm_dlci_data_output(dlci->gsm, dlci);
-	} else if (dlci->gsm->tx_bytes < TX_THRESH_LO)
-		gsm_dlci_data_sweep(dlci->gsm);
+	}
+	if (sweep)
+ 		gsm_dlci_data_sweep(dlci->gsm);
 	spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
 }
 
@@ -1027,6 +1029,7 @@ static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
 {
 	int  mlines = 0;
 	u8 brk = 0;
+	int fc;
 
 	/* The modem status command can either contain one octet (v.24 signals)
 	   or two octets (v.24 signals + break signals). The length field will
@@ -1038,19 +1041,21 @@ static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
 	else {
 		brk = modem & 0x7f;
 		modem = (modem >> 7) & 0x7f;
-	};
+	}
 
 	/* Flow control/ready to communicate */
-	if (modem & MDM_FC) {
+	fc = (modem & MDM_FC) || !(modem & MDM_RTR);
+	if (fc && !dlci->constipated) {
 		/* Need to throttle our output on this device */
 		dlci->constipated = 1;
-	}
-	if (modem & MDM_RTC) {
-		mlines |= TIOCM_DSR | TIOCM_DTR;
+	} else if (!fc && dlci->constipated) {
 		dlci->constipated = 0;
 		gsm_dlci_data_kick(dlci);
 	}
+
 	/* Map modem bits */
+	if (modem & MDM_RTC)
+		mlines |= TIOCM_DSR | TIOCM_DTR;
 	if (modem & MDM_RTR)
 		mlines |= TIOCM_RTS | TIOCM_CTS;
 	if (modem & MDM_IC)
@@ -1061,7 +1066,7 @@ static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
 	/* Carrier drop -> hangup */
 	if (tty) {
 		if ((mlines & TIOCM_CD) == 0 && (dlci->modem_rx & TIOCM_CD))
-			if (!(tty->termios->c_cflag & CLOCAL))
+			if (!(tty->termios.c_cflag & CLOCAL))
 				tty_hangup(tty);
 		if (brk & 0x01)
 			tty_insert_flip_char(tty, 0, TTY_BREAK);
@@ -1190,6 +1195,8 @@ static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
 							u8 *data, int clen)
 {
 	u8 buf[1];
+	unsigned long flags;
+
 	switch (command) {
 	case CMD_CLD: {
 		struct gsm_dlci *dlci = gsm->dlci[0];
@@ -1206,16 +1213,18 @@ static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
 		gsm_control_reply(gsm, CMD_TEST, data, clen);
 		break;
 	case CMD_FCON:
-		/* Modem wants us to STFU */
-		gsm->constipated = 1;
-		gsm_control_reply(gsm, CMD_FCON, NULL, 0);
-		break;
-	case CMD_FCOFF:
 		/* Modem can accept data again */
 		gsm->constipated = 0;
-		gsm_control_reply(gsm, CMD_FCOFF, NULL, 0);
+		gsm_control_reply(gsm, CMD_FCON, NULL, 0);
 		/* Kick the link in case it is idling */
+		spin_lock_irqsave(&gsm->tx_lock, flags);
 		gsm_data_kick(gsm);
+		spin_unlock_irqrestore(&gsm->tx_lock, flags);
+		break;
+	case CMD_FCOFF:
+		/* Modem wants us to STFU */
+		gsm->constipated = 1;
+		gsm_control_reply(gsm, CMD_FCOFF, NULL, 0);
 		break;
 	case CMD_MSC:
 		/* Out of band modem line change indicator for a DLCI */
@@ -1668,7 +1677,7 @@ static void gsm_dlci_free(struct kref *ref)
 	dlci->gsm->dlci[dlci->addr] = NULL;
 	kfifo_free(dlci->fifo);
 	while ((dlci->skb = skb_dequeue(&dlci->skb_list)))
-		kfree_skb(dlci->skb);
+		dev_kfree_skb(dlci->skb);
 	kfree(dlci);
 }
 
@@ -2007,7 +2016,7 @@ void gsm_cleanup_mux(struct gsm_mux *gsm)
 {
 	int i;
 	struct gsm_dlci *dlci = gsm->dlci[0];
-	struct gsm_msg *txq;
+	struct gsm_msg *txq, *ntxq;
 	struct gsm_control *gc;
 
 	gsm->dead = 1;
@@ -2042,11 +2051,9 @@ void gsm_cleanup_mux(struct gsm_mux *gsm)
 		if (gsm->dlci[i])
 			gsm_dlci_release(gsm->dlci[i]);
 	/* Now wipe the queues */
-	for (txq = gsm->tx_head; txq != NULL; txq = gsm->tx_head) {
-		gsm->tx_head = txq->next;
+	list_for_each_entry_safe(txq, ntxq, &gsm->tx_list, list)
 		kfree(txq);
-	}
-	gsm->tx_tail = NULL;
+	INIT_LIST_HEAD(&gsm->tx_list);
 }
 EXPORT_SYMBOL_GPL(gsm_cleanup_mux);
 
@@ -2157,6 +2164,7 @@ struct gsm_mux *gsm_alloc_mux(void)
 	}
 	spin_lock_init(&gsm->lock);
 	kref_init(&gsm->ref);
+	INIT_LIST_HEAD(&gsm->tx_list);
 
 	gsm->t1 = T1;
 	gsm->t2 = T2;
@@ -2273,7 +2281,7 @@ static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 			gsm->error(gsm, *dp, flags);
 			break;
 		default:
-			WARN_ONCE("%s: unknown flag %d\n",
+			WARN_ONCE(1, "%s: unknown flag %d\n",
 			       tty_name(tty, buf), flags);
 			break;
 		}
@@ -2377,12 +2385,12 @@ static void gsmld_write_wakeup(struct tty_struct *tty)
 
 	/* Queue poll */
 	clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+	spin_lock_irqsave(&gsm->tx_lock, flags);
 	gsm_data_kick(gsm);
 	if (gsm->tx_bytes < TX_THRESH_LO) {
-		spin_lock_irqsave(&gsm->tx_lock, flags);
 		gsm_dlci_data_sweep(gsm);
-		spin_unlock_irqrestore(&gsm->tx_lock, flags);
 	}
+	spin_unlock_irqrestore(&gsm->tx_lock, flags);
 }
 
 /**
@@ -2868,14 +2876,14 @@ static const struct tty_port_operations gsm_port_ops = {
 	.dtr_rts = gsm_dtr_rts,
 };
 
-
-static int gsmtty_open(struct tty_struct *tty, struct file *filp)
+static int gsmtty_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	struct gsm_mux *gsm;
 	struct gsm_dlci *dlci;
-	struct tty_port *port;
 	unsigned int line = tty->index;
 	unsigned int mux = line >> 6;
+	bool alloc = false;
+	int ret;
 
 	line = line & 0x3F;
 
@@ -2889,14 +2897,35 @@ static int gsmtty_open(struct tty_struct *tty, struct file *filp)
 	gsm = gsm_mux[mux];
 	if (gsm->dead)
 		return -EL2HLT;
+	/* If DLCI 0 is not yet fully open return an error. This is ok from a locking
+	   perspective as we don't have to worry about this if DLCI0 is lost */
+	if (gsm->dlci[0] && gsm->dlci[0]->state != DLCI_OPEN) 
+		return -EL2NSYNC;
 	dlci = gsm->dlci[line];
-	if (dlci == NULL)
+	if (dlci == NULL) {
+		alloc = true;
 		dlci = gsm_dlci_alloc(gsm, line);
+	}
 	if (dlci == NULL)
 		return -ENOMEM;
-	port = &dlci->port;
-	port->count++;
+	ret = tty_port_install(&dlci->port, driver, tty);
+	if (ret) {
+		if (alloc)
+			dlci_put(dlci);
+		return ret;
+	}
+
 	tty->driver_data = dlci;
+
+	return 0;
+}
+
+static int gsmtty_open(struct tty_struct *tty, struct file *filp)
+{
+	struct gsm_dlci *dlci = tty->driver_data;
+	struct tty_port *port = &dlci->port;
+
+	port->count++;
 	dlci_get(dlci);
 	dlci_get(dlci->gsm->dlci[0]);
 	mux_get(dlci->gsm);
@@ -3043,13 +3072,13 @@ static void gsmtty_set_termios(struct tty_struct *tty, struct ktermios *old)
 	   the RPN control message. This however rapidly gets nasty as we
 	   then have to remap modem signals each way according to whether
 	   our virtual cable is null modem etc .. */
-	tty_termios_copy_hw(tty->termios, old);
+	tty_termios_copy_hw(&tty->termios, old);
 }
 
 static void gsmtty_throttle(struct tty_struct *tty)
 {
 	struct gsm_dlci *dlci = tty->driver_data;
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		dlci->modem_tx &= ~TIOCM_DTR;
 	dlci->throttled = 1;
 	/* Send an MSC with DTR cleared */
@@ -3059,7 +3088,7 @@ static void gsmtty_throttle(struct tty_struct *tty)
 static void gsmtty_unthrottle(struct tty_struct *tty)
 {
 	struct gsm_dlci *dlci = tty->driver_data;
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		dlci->modem_tx |= TIOCM_DTR;
 	dlci->throttled = 0;
 	/* Send an MSC with DTR set */
@@ -3085,6 +3114,7 @@ static int gsmtty_break_ctl(struct tty_struct *tty, int state)
 
 /* Virtual ttys for the demux */
 static const struct tty_operations gsmtty_ops = {
+	.install		= gsmtty_install,
 	.open			= gsmtty_open,
 	.close			= gsmtty_close,
 	.write			= gsmtty_write,
diff --git a/drivers/tty/n_r3964.c b/drivers/tty/n_r3964.c
index 5c6c31459a2f..1e6405070ce6 100644
--- a/drivers/tty/n_r3964.c
+++ b/drivers/tty/n_r3964.c
@@ -1065,7 +1065,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 
 	TRACE_L("read()");
 
-	tty_lock();
+	tty_lock(tty);
 
 	pClient = findClient(pInfo, task_pid(current));
 	if (pClient) {
@@ -1077,7 +1077,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 				goto unlock;
 			}
 			/* block until there is a message: */
-			wait_event_interruptible_tty(pInfo->read_wait,
+			wait_event_interruptible_tty(tty, pInfo->read_wait,
 					(pMsg = remove_msg(pInfo, pClient)));
 		}
 
@@ -1107,7 +1107,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 	}
 	ret = -EPERM;
 unlock:
-	tty_unlock();
+	tty_unlock(tty);
 	return ret;
 }
 
@@ -1156,7 +1156,7 @@ static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
 	pHeader->locks = 0;
 	pHeader->owner = NULL;
 
-	tty_lock();
+	tty_lock(tty);
 
 	pClient = findClient(pInfo, task_pid(current));
 	if (pClient) {
@@ -1175,7 +1175,7 @@ static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
 	add_tx_queue(pInfo, pHeader);
 	trigger_transmit(pInfo);
 
-	tty_unlock();
+	tty_unlock(tty);
 
 	return 0;
 }
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index ee1c268f5f9d..8c0b7b42319c 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -92,10 +92,18 @@ static inline int tty_put_user(struct tty_struct *tty, unsigned char x,
 
 static void n_tty_set_room(struct tty_struct *tty)
 {
-	/* tty->read_cnt is not read locked ? */
-	int	left = N_TTY_BUF_SIZE - tty->read_cnt - 1;
+	int left;
 	int old_left;
 
+	/* tty->read_cnt is not read locked ? */
+	if (I_PARMRK(tty)) {
+		/* Multiply read_cnt by 3, since each byte might take up to
+		 * three times as many spaces when PARMRK is set (depending on
+		 * its flags, e.g. parity error). */
+		left = N_TTY_BUF_SIZE - tty->read_cnt * 3 - 1;
+	} else
+		left = N_TTY_BUF_SIZE - tty->read_cnt - 1;
+
 	/*
 	 * If we are doing input canonicalization, and there are no
 	 * pending newlines, let characters through without limit, so
@@ -1432,6 +1440,12 @@ static void n_tty_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 	 */
 	if (tty->receive_room < TTY_THRESHOLD_THROTTLE)
 		tty_throttle(tty);
+
+        /* FIXME: there is a tiny race here if the receive room check runs
+           before the other work executes and empties the buffer (upping
+           the receiving room and unthrottling. We then throttle and get
+           stuck. This has been observed and traced down by Vincent Pillet/
+           We need to address this when we sort out out the rx path locking */
 }
 
 int is_ignored(int sig)
@@ -1460,7 +1474,7 @@ static void n_tty_set_termios(struct tty_struct *tty, struct ktermios *old)
 	BUG_ON(!tty);
 
 	if (old)
-		canon_change = (old->c_lflag ^ tty->termios->c_lflag) & ICANON;
+		canon_change = (old->c_lflag ^ tty->termios.c_lflag) & ICANON;
 	if (canon_change) {
 		memset(&tty->read_flags, 0, sizeof tty->read_flags);
 		tty->canon_head = tty->read_tail;
@@ -1728,7 +1742,8 @@ static ssize_t n_tty_read(struct tty_struct *tty, struct file *file,
 
 do_it_again:
 
-	BUG_ON(!tty->read_buf);
+	if (WARN_ON(!tty->read_buf))
+		return -EAGAIN;
 
 	c = job_control(tty, file);
 	if (c < 0)
@@ -1832,13 +1847,13 @@ do_it_again:
 
 		if (tty->icanon && !L_EXTPROC(tty)) {
 			/* N.B. avoid overrun if nr == 0 */
+			spin_lock_irqsave(&tty->read_lock, flags);
 			while (nr && tty->read_cnt) {
 				int eol;
 
 				eol = test_and_clear_bit(tty->read_tail,
 						tty->read_flags);
 				c = tty->read_buf[tty->read_tail];
-				spin_lock_irqsave(&tty->read_lock, flags);
 				tty->read_tail = ((tty->read_tail+1) &
 						  (N_TTY_BUF_SIZE-1));
 				tty->read_cnt--;
@@ -1856,15 +1871,19 @@ do_it_again:
 					if (tty_put_user(tty, c, b++)) {
 						retval = -EFAULT;
 						b--;
+						spin_lock_irqsave(&tty->read_lock, flags);
 						break;
 					}
 					nr--;
 				}
 				if (eol) {
 					tty_audit_push(tty);
+					spin_lock_irqsave(&tty->read_lock, flags);
 					break;
 				}
+				spin_lock_irqsave(&tty->read_lock, flags);
 			}
+			spin_unlock_irqrestore(&tty->read_lock, flags);
 			if (retval)
 				break;
 		} else {
diff --git a/drivers/tty/nozomi.c b/drivers/tty/nozomi.c
index e7592f9037da..b917c9424954 100644
--- a/drivers/tty/nozomi.c
+++ b/drivers/tty/nozomi.c
@@ -1473,8 +1473,8 @@ static int __devinit nozomi_card_init(struct pci_dev *pdev,
 		port->dc = dc;
 		tty_port_init(&port->port);
 		port->port.ops = &noz_tty_port_ops;
-		tty_dev = tty_register_device(ntty_driver, dc->index_start + i,
-							&pdev->dev);
+		tty_dev = tty_port_register_device(&port->port, ntty_driver,
+				dc->index_start + i, &pdev->dev);
 
 		if (IS_ERR(tty_dev)) {
 			ret = PTR_ERR(tty_dev);
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index 5505ffc91da4..a82b39939a9c 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -47,6 +47,7 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 	wake_up_interruptible(&tty->read_wait);
 	wake_up_interruptible(&tty->write_wait);
 	tty->packet = 0;
+	/* Review - krefs on tty_link ?? */
 	if (!tty->link)
 		return;
 	tty->link->packet = 0;
@@ -62,9 +63,9 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 		        mutex_unlock(&devpts_mutex);
 		}
 #endif
-		tty_unlock();
+		tty_unlock(tty);
 		tty_vhangup(tty->link);
-		tty_lock();
+		tty_lock(tty);
 	}
 }
 
@@ -231,8 +232,8 @@ out:
 static void pty_set_termios(struct tty_struct *tty,
 					struct ktermios *old_termios)
 {
-	tty->termios->c_cflag &= ~(CSIZE | PARENB);
-	tty->termios->c_cflag |= (CS8 | CREAD);
+	tty->termios.c_cflag &= ~(CSIZE | PARENB);
+	tty->termios.c_cflag |= (CS8 | CREAD);
 }
 
 /**
@@ -282,60 +283,110 @@ done:
 	return 0;
 }
 
-/* Traditional BSD devices */
-#ifdef CONFIG_LEGACY_PTYS
-
-static int pty_install(struct tty_driver *driver, struct tty_struct *tty)
+/**
+ *	pty_common_install		-	set up the pty pair
+ *	@driver: the pty driver
+ *	@tty: the tty being instantiated
+ *	@bool: legacy, true if this is BSD style
+ *
+ *	Perform the initial set up for the tty/pty pair. Called from the
+ *	tty layer when the port is first opened.
+ *
+ *	Locking: the caller must hold the tty_mutex
+ */
+static int pty_common_install(struct tty_driver *driver, struct tty_struct *tty,
+		bool legacy)
 {
 	struct tty_struct *o_tty;
+	struct tty_port *ports[2];
 	int idx = tty->index;
-	int retval;
+	int retval = -ENOMEM;
 
 	o_tty = alloc_tty_struct();
 	if (!o_tty)
-		return -ENOMEM;
+		goto err;
+	ports[0] = kmalloc(sizeof **ports, GFP_KERNEL);
+	ports[1] = kmalloc(sizeof **ports, GFP_KERNEL);
+	if (!ports[0] || !ports[1])
+		goto err_free_tty;
 	if (!try_module_get(driver->other->owner)) {
 		/* This cannot in fact currently happen */
-		retval = -ENOMEM;
 		goto err_free_tty;
 	}
 	initialize_tty_struct(o_tty, driver->other, idx);
 
-	/* We always use new tty termios data so we can do this
-	   the easy way .. */
-	retval = tty_init_termios(tty);
-	if (retval)
-		goto err_deinit_tty;
-
-	retval = tty_init_termios(o_tty);
-	if (retval)
-		goto err_free_termios;
+	if (legacy) {
+		/* We always use new tty termios data so we can do this
+		   the easy way .. */
+		retval = tty_init_termios(tty);
+		if (retval)
+			goto err_deinit_tty;
+
+		retval = tty_init_termios(o_tty);
+		if (retval)
+			goto err_free_termios;
+
+		driver->other->ttys[idx] = o_tty;
+		driver->ttys[idx] = tty;
+	} else {
+		memset(&tty->termios_locked, 0, sizeof(tty->termios_locked));
+		tty->termios = driver->init_termios;
+		memset(&o_tty->termios_locked, 0, sizeof(tty->termios_locked));
+		o_tty->termios = driver->other->init_termios;
+	}
 
 	/*
 	 * Everything allocated ... set up the o_tty structure.
 	 */
-	driver->other->ttys[idx] = o_tty;
 	tty_driver_kref_get(driver->other);
 	if (driver->subtype == PTY_TYPE_MASTER)
 		o_tty->count++;
 	/* Establish the links in both directions */
 	tty->link   = o_tty;
 	o_tty->link = tty;
+	tty_port_init(ports[0]);
+	tty_port_init(ports[1]);
+	o_tty->port = ports[0];
+	tty->port = ports[1];
 
 	tty_driver_kref_get(driver);
 	tty->count++;
-	driver->ttys[idx] = tty;
 	return 0;
 err_free_termios:
-	tty_free_termios(tty);
+	if (legacy)
+		tty_free_termios(tty);
 err_deinit_tty:
 	deinitialize_tty_struct(o_tty);
 	module_put(o_tty->driver->owner);
 err_free_tty:
+	kfree(ports[0]);
+	kfree(ports[1]);
 	free_tty_struct(o_tty);
+err:
 	return retval;
 }
 
+static void pty_cleanup(struct tty_struct *tty)
+{
+	kfree(tty->port);
+}
+
+/* Traditional BSD devices */
+#ifdef CONFIG_LEGACY_PTYS
+
+static int pty_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	return pty_common_install(driver, tty, true);
+}
+
+static void pty_remove(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct tty_struct *pair = tty->link;
+	driver->ttys[tty->index] = NULL;
+	if (pair)
+		pair->driver->ttys[pair->index] = NULL;
+}
+
 static int pty_bsd_ioctl(struct tty_struct *tty,
 			 unsigned int cmd, unsigned long arg)
 {
@@ -366,7 +417,9 @@ static const struct tty_operations master_pty_ops_bsd = {
 	.unthrottle = pty_unthrottle,
 	.set_termios = pty_set_termios,
 	.ioctl = pty_bsd_ioctl,
-	.resize = pty_resize
+	.cleanup = pty_cleanup,
+	.resize = pty_resize,
+	.remove = pty_remove
 };
 
 static const struct tty_operations slave_pty_ops_bsd = {
@@ -379,7 +432,9 @@ static const struct tty_operations slave_pty_ops_bsd = {
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
 	.set_termios = pty_set_termios,
-	.resize = pty_resize
+	.cleanup = pty_cleanup,
+	.resize = pty_resize,
+	.remove = pty_remove
 };
 
 static void __init legacy_pty_init(void)
@@ -389,12 +444,18 @@ static void __init legacy_pty_init(void)
 	if (legacy_count <= 0)
 		return;
 
-	pty_driver = alloc_tty_driver(legacy_count);
-	if (!pty_driver)
+	pty_driver = tty_alloc_driver(legacy_count,
+			TTY_DRIVER_RESET_TERMIOS |
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_ALLOC);
+	if (IS_ERR(pty_driver))
 		panic("Couldn't allocate pty driver");
 
-	pty_slave_driver = alloc_tty_driver(legacy_count);
-	if (!pty_slave_driver)
+	pty_slave_driver = tty_alloc_driver(legacy_count,
+			TTY_DRIVER_RESET_TERMIOS |
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_ALLOC);
+	if (IS_ERR(pty_slave_driver))
 		panic("Couldn't allocate pty slave driver");
 
 	pty_driver->driver_name = "pty_master";
@@ -410,7 +471,6 @@ static void __init legacy_pty_init(void)
 	pty_driver->init_termios.c_lflag = 0;
 	pty_driver->init_termios.c_ispeed = 38400;
 	pty_driver->init_termios.c_ospeed = 38400;
-	pty_driver->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW;
 	pty_driver->other = pty_slave_driver;
 	tty_set_operations(pty_driver, &master_pty_ops_bsd);
 
@@ -424,8 +484,6 @@ static void __init legacy_pty_init(void)
 	pty_slave_driver->init_termios.c_cflag = B38400 | CS8 | CREAD;
 	pty_slave_driver->init_termios.c_ispeed = 38400;
 	pty_slave_driver->init_termios.c_ospeed = 38400;
-	pty_slave_driver->flags = TTY_DRIVER_RESET_TERMIOS |
-					TTY_DRIVER_REAL_RAW;
 	pty_slave_driver->other = pty_driver;
 	tty_set_operations(pty_slave_driver, &slave_pty_ops_bsd);
 
@@ -497,78 +555,22 @@ static struct tty_struct *pts_unix98_lookup(struct tty_driver *driver,
 	return tty;
 }
 
-static void pty_unix98_shutdown(struct tty_struct *tty)
-{
-	tty_driver_remove_tty(tty->driver, tty);
-	/* We have our own method as we don't use the tty index */
-	kfree(tty->termios);
-}
-
 /* We have no need to install and remove our tty objects as devpts does all
    the work for us */
 
 static int pty_unix98_install(struct tty_driver *driver, struct tty_struct *tty)
 {
-	struct tty_struct *o_tty;
-	int idx = tty->index;
-
-	o_tty = alloc_tty_struct();
-	if (!o_tty)
-		return -ENOMEM;
-	if (!try_module_get(driver->other->owner)) {
-		/* This cannot in fact currently happen */
-		goto err_free_tty;
-	}
-	initialize_tty_struct(o_tty, driver->other, idx);
-
-	tty->termios = kzalloc(sizeof(struct ktermios[2]), GFP_KERNEL);
-	if (tty->termios == NULL)
-		goto err_free_mem;
-	*tty->termios = driver->init_termios;
-	tty->termios_locked = tty->termios + 1;
-
-	o_tty->termios = kzalloc(sizeof(struct ktermios[2]), GFP_KERNEL);
-	if (o_tty->termios == NULL)
-		goto err_free_mem;
-	*o_tty->termios = driver->other->init_termios;
-	o_tty->termios_locked = o_tty->termios + 1;
-
-	tty_driver_kref_get(driver->other);
-	if (driver->subtype == PTY_TYPE_MASTER)
-		o_tty->count++;
-	/* Establish the links in both directions */
-	tty->link   = o_tty;
-	o_tty->link = tty;
-	/*
-	 * All structures have been allocated, so now we install them.
-	 * Failures after this point use release_tty to clean up, so
-	 * there's no need to null out the local pointers.
-	 */
-	tty_driver_kref_get(driver);
-	tty->count++;
-	return 0;
-err_free_mem:
-	deinitialize_tty_struct(o_tty);
-	kfree(o_tty->termios);
-	kfree(tty->termios);
-	module_put(o_tty->driver->owner);
-err_free_tty:
-	free_tty_struct(o_tty);
-	return -ENOMEM;
-}
-
-static void ptm_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
-{
+	return pty_common_install(driver, tty, false);
 }
 
-static void pts_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
+static void pty_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
 {
 }
 
 static const struct tty_operations ptm_unix98_ops = {
 	.lookup = ptm_unix98_lookup,
 	.install = pty_unix98_install,
-	.remove = ptm_unix98_remove,
+	.remove = pty_unix98_remove,
 	.open = pty_open,
 	.close = pty_close,
 	.write = pty_write,
@@ -578,14 +580,14 @@ static const struct tty_operations ptm_unix98_ops = {
 	.unthrottle = pty_unthrottle,
 	.set_termios = pty_set_termios,
 	.ioctl = pty_unix98_ioctl,
-	.shutdown = pty_unix98_shutdown,
-	.resize = pty_resize
+	.resize = pty_resize,
+	.cleanup = pty_cleanup
 };
 
 static const struct tty_operations pty_unix98_ops = {
 	.lookup = pts_unix98_lookup,
 	.install = pty_unix98_install,
-	.remove = pts_unix98_remove,
+	.remove = pty_unix98_remove,
 	.open = pty_open,
 	.close = pty_close,
 	.write = pty_write,
@@ -594,7 +596,7 @@ static const struct tty_operations pty_unix98_ops = {
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
 	.set_termios = pty_set_termios,
-	.shutdown = pty_unix98_shutdown
+	.cleanup = pty_cleanup,
 };
 
 /**
@@ -622,26 +624,28 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 		return retval;
 
 	/* find a device that is not in use. */
-	tty_lock();
+	mutex_lock(&devpts_mutex);
 	index = devpts_new_index(inode);
-	tty_unlock();
 	if (index < 0) {
 		retval = index;
+		mutex_unlock(&devpts_mutex);
 		goto err_file;
 	}
 
+	mutex_unlock(&devpts_mutex);
+
 	mutex_lock(&tty_mutex);
-	mutex_lock(&devpts_mutex);
 	tty = tty_init_dev(ptm_driver, index);
-	mutex_unlock(&devpts_mutex);
-	tty_lock();
-	mutex_unlock(&tty_mutex);
 
 	if (IS_ERR(tty)) {
 		retval = PTR_ERR(tty);
 		goto out;
 	}
 
+	/* The tty returned here is locked so we can safely
+	   drop the mutex */
+	mutex_unlock(&tty_mutex);
+
 	set_bit(TTY_PTY_LOCK, &tty->flags); /* LOCK THE SLAVE */
 
 	tty_add_file(tty, filp);
@@ -654,15 +658,15 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 	if (retval)
 		goto err_release;
 
-	tty_unlock();
+	tty_unlock(tty);
 	return 0;
 err_release:
-	tty_unlock();
+	tty_unlock(tty);
 	tty_release(inode, filp);
 	return retval;
 out:
+	mutex_unlock(&tty_mutex);
 	devpts_kill_index(inode, index);
-	tty_unlock();
 err_file:
 	tty_free_file(filp);
 	return retval;
@@ -672,11 +676,21 @@ static struct file_operations ptmx_fops;
 
 static void __init unix98_pty_init(void)
 {
-	ptm_driver = alloc_tty_driver(NR_UNIX98_PTY_MAX);
-	if (!ptm_driver)
+	ptm_driver = tty_alloc_driver(NR_UNIX98_PTY_MAX,
+			TTY_DRIVER_RESET_TERMIOS |
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_DEV |
+			TTY_DRIVER_DEVPTS_MEM |
+			TTY_DRIVER_DYNAMIC_ALLOC);
+	if (IS_ERR(ptm_driver))
 		panic("Couldn't allocate Unix98 ptm driver");
-	pts_driver = alloc_tty_driver(NR_UNIX98_PTY_MAX);
-	if (!pts_driver)
+	pts_driver = tty_alloc_driver(NR_UNIX98_PTY_MAX,
+			TTY_DRIVER_RESET_TERMIOS |
+			TTY_DRIVER_REAL_RAW |
+			TTY_DRIVER_DYNAMIC_DEV |
+			TTY_DRIVER_DEVPTS_MEM |
+			TTY_DRIVER_DYNAMIC_ALLOC);
+	if (IS_ERR(pts_driver))
 		panic("Couldn't allocate Unix98 pts driver");
 
 	ptm_driver->driver_name = "pty_master";
@@ -692,8 +706,6 @@ static void __init unix98_pty_init(void)
 	ptm_driver->init_termios.c_lflag = 0;
 	ptm_driver->init_termios.c_ispeed = 38400;
 	ptm_driver->init_termios.c_ospeed = 38400;
-	ptm_driver->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW |
-		TTY_DRIVER_DYNAMIC_DEV | TTY_DRIVER_DEVPTS_MEM;
 	ptm_driver->other = pts_driver;
 	tty_set_operations(ptm_driver, &ptm_unix98_ops);
 
@@ -707,8 +719,6 @@ static void __init unix98_pty_init(void)
 	pts_driver->init_termios.c_cflag = B38400 | CS8 | CREAD;
 	pts_driver->init_termios.c_ispeed = 38400;
 	pts_driver->init_termios.c_ospeed = 38400;
-	pts_driver->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW |
-		TTY_DRIVER_DYNAMIC_DEV | TTY_DRIVER_DEVPTS_MEM;
 	pts_driver->other = ptm_driver;
 	tty_set_operations(pts_driver, &pty_unix98_ops);
 
diff --git a/drivers/tty/rocket.c b/drivers/tty/rocket.c
index 777d5f9cf6cc..9700d34b20a3 100644
--- a/drivers/tty/rocket.c
+++ b/drivers/tty/rocket.c
@@ -704,8 +704,8 @@ static void init_r_port(int board, int aiop, int chan, struct pci_dev *pci_dev)
 	spin_lock_init(&info->slock);
 	mutex_init(&info->write_mtx);
 	rp_table[line] = info;
-	tty_register_device(rocket_driver, line, pci_dev ? &pci_dev->dev :
-			NULL);
+	tty_port_register_device(&info->port, rocket_driver, line,
+			pci_dev ? &pci_dev->dev : NULL);
 }
 
 /*
@@ -720,7 +720,7 @@ static void configure_r_port(struct tty_struct *tty, struct r_port *info,
 	unsigned rocketMode;
 	int bits, baud, divisor;
 	CHANNEL_t *cp;
-	struct ktermios *t = tty->termios;
+	struct ktermios *t = &tty->termios;
 
 	cp = &info->channel;
 	cflag = t->c_cflag;
@@ -978,7 +978,7 @@ static int rp_open(struct tty_struct *tty, struct file *filp)
 			tty->alt_speed = 460800;
 
 		configure_r_port(tty, info, NULL);
-		if (tty->termios->c_cflag & CBAUD) {
+		if (tty->termios.c_cflag & CBAUD) {
 			sSetDTR(cp);
 			sSetRTS(cp);
 		}
@@ -1089,35 +1089,35 @@ static void rp_set_termios(struct tty_struct *tty,
 	if (rocket_paranoia_check(info, "rp_set_termios"))
 		return;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	/*
 	 * This driver doesn't support CS5 or CS6
 	 */
 	if (((cflag & CSIZE) == CS5) || ((cflag & CSIZE) == CS6))
-		tty->termios->c_cflag =
+		tty->termios.c_cflag =
 		    ((cflag & ~CSIZE) | (old_termios->c_cflag & CSIZE));
 	/* Or CMSPAR */
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	configure_r_port(tty, info, old_termios);
 
 	cp = &info->channel;
 
 	/* Handle transition to B0 status */
-	if ((old_termios->c_cflag & CBAUD) && !(tty->termios->c_cflag & CBAUD)) {
+	if ((old_termios->c_cflag & CBAUD) && !(tty->termios.c_cflag & CBAUD)) {
 		sClrDTR(cp);
 		sClrRTS(cp);
 	}
 
 	/* Handle transition away from B0 status */
-	if (!(old_termios->c_cflag & CBAUD) && (tty->termios->c_cflag & CBAUD)) {
-		if (!tty->hw_stopped || !(tty->termios->c_cflag & CRTSCTS))
+	if (!(old_termios->c_cflag & CBAUD) && (tty->termios.c_cflag & CBAUD)) {
+		if (!tty->hw_stopped || !(tty->termios.c_cflag & CRTSCTS))
 			sSetRTS(cp);
 		sSetDTR(cp);
 	}
 
-	if ((old_termios->c_cflag & CRTSCTS) && !(tty->termios->c_cflag & CRTSCTS)) {
+	if ((old_termios->c_cflag & CRTSCTS) && !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		rp_start(tty);
 	}
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index 3ed20e435e59..66c38a3f74ce 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -515,7 +515,7 @@ static void change_speed(struct m68k_serial *info, struct tty_struct *tty)
 	unsigned cflag;
 	int	i;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 	if (!(port = info->port))
 		return;
 
@@ -617,7 +617,7 @@ static void rs_set_ldisc(struct tty_struct *tty)
 	if (serial_paranoia_check(info, tty->name, "rs_set_ldisc"))
 		return;
 
-	info->is_cons = (tty->termios->c_line == N_TTY);
+	info->is_cons = (tty->termios.c_line == N_TTY);
 	
 	printk("ttyS%d console mode %s\n", info->line, info->is_cons ? "on" : "off");
 }
@@ -985,7 +985,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 	change_speed(info, tty);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		rs_start(tty);
 	}
@@ -1070,7 +1070,7 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
 		if (tty->ldisc.close)
 			(tty->ldisc.close)(tty);
 		tty->ldisc = ldiscs[N_TTY];
-		tty->termios->c_line = N_TTY;
+		tty->termios.c_line = N_TTY;
 		if (tty->ldisc.open)
 			(tty->ldisc.open)(tty);
 	}
@@ -1189,12 +1189,6 @@ rs68328_init(void)
 	serial_driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(serial_driver, &rs_ops);
 
-	if (tty_register_driver(serial_driver)) {
-		put_tty_driver(serial_driver);
-		printk(KERN_ERR "Couldn't register serial driver\n");
-		return -ENOMEM;
-	}
-
 	local_irq_save(flags);
 
 	for(i=0;i<NR_PORTS;i++) {
@@ -1224,8 +1218,17 @@ rs68328_init(void)
 			    0,
 			    "M68328_UART", info))
                 panic("Unable to attach 68328 serial interrupt\n");
+
+	    tty_port_link_device(&info->tport, serial_driver, i);
 	}
 	local_irq_restore(flags);
+
+	if (tty_register_driver(serial_driver)) {
+		put_tty_driver(serial_driver);
+		printk(KERN_ERR "Couldn't register serial driver\n");
+		return -ENOMEM;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/tty/serial/8250/8250.c b/drivers/tty/serial/8250/8250.c
index 8123f784bcda..3ba4234592bc 100644
--- a/drivers/tty/serial/8250/8250.c
+++ b/drivers/tty/serial/8250/8250.c
@@ -290,6 +290,9 @@ static const struct serial8250_config uart_config[] = {
 				  UART_FCR_R_TRIG_00 | UART_FCR_T_TRIG_00,
 		.flags		= UART_CAP_FIFO,
 	},
+	[PORT_8250_CIR] = {
+		.name		= "CIR port"
+	}
 };
 
 /* Uart divisor latch read */
@@ -1037,6 +1040,7 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 	unsigned char save_lcr, save_mcr;
 	struct uart_port *port = &up->port;
 	unsigned long flags;
+	unsigned int old_capabilities;
 
 	if (!port->iobase && !port->mapbase && !port->membase)
 		return;
@@ -1087,6 +1091,7 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 			/*
 			 * We failed; there's nothing here
 			 */
+			spin_unlock_irqrestore(&port->lock, flags);
 			DEBUG_AUTOCONF("IER test failed (%02x, %02x) ",
 				       scratch2, scratch3);
 			goto out;
@@ -1110,6 +1115,7 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 		status1 = serial_in(up, UART_MSR) & 0xF0;
 		serial_out(up, UART_MCR, save_mcr);
 		if (status1 != 0x90) {
+			spin_unlock_irqrestore(&port->lock, flags);
 			DEBUG_AUTOCONF("LOOP test failed (%02x) ",
 				       status1);
 			goto out;
@@ -1132,8 +1138,6 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 	serial_out(up, UART_FCR, UART_FCR_ENABLE_FIFO);
 	scratch = serial_in(up, UART_IIR) >> 6;
 
-	DEBUG_AUTOCONF("iir=%d ", scratch);
-
 	switch (scratch) {
 	case 0:
 		autoconfig_8250(up);
@@ -1167,19 +1171,13 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 
 	serial_out(up, UART_LCR, save_lcr);
 
-	if (up->capabilities != uart_config[port->type].flags) {
-		printk(KERN_WARNING
-		       "ttyS%d: detected caps %08x should be %08x\n",
-		       serial_index(port), up->capabilities,
-		       uart_config[port->type].flags);
-	}
-
 	port->fifosize = uart_config[up->port.type].fifo_size;
+	old_capabilities = up->capabilities; 
 	up->capabilities = uart_config[port->type].flags;
 	up->tx_loadsz = uart_config[port->type].tx_loadsz;
 
 	if (port->type == PORT_UNKNOWN)
-		goto out;
+		goto out_lock;
 
 	/*
 	 * Reset the UART.
@@ -1196,8 +1194,16 @@ static void autoconfig(struct uart_8250_port *up, unsigned int probeflags)
 	else
 		serial_out(up, UART_IER, 0);
 
- out:
+out_lock:
 	spin_unlock_irqrestore(&port->lock, flags);
+	if (up->capabilities != old_capabilities) {
+		printk(KERN_WARNING
+		       "ttyS%d: detected caps %08x should be %08x\n",
+		       serial_index(port), old_capabilities,
+		       up->capabilities);
+	}
+out:
+	DEBUG_AUTOCONF("iir=%d ", scratch);
 	DEBUG_AUTOCONF("type=%s\n", uart_config[port->type].name);
 }
 
@@ -1897,6 +1903,9 @@ static int serial8250_startup(struct uart_port *port)
 	unsigned char lsr, iir;
 	int retval;
 
+	if (port->type == PORT_8250_CIR)
+		return -ENODEV;
+
 	port->fifosize = uart_config[up->port.type].fifo_size;
 	up->tx_loadsz = uart_config[up->port.type].tx_loadsz;
 	up->capabilities = uart_config[up->port.type].flags;
@@ -2202,6 +2211,7 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	unsigned char cval, fcr = 0;
 	unsigned long flags;
 	unsigned int baud, quot;
+	int fifo_bug = 0;
 
 	switch (termios->c_cflag & CSIZE) {
 	case CS5:
@@ -2221,8 +2231,11 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	if (termios->c_cflag & CSTOPB)
 		cval |= UART_LCR_STOP;
-	if (termios->c_cflag & PARENB)
+	if (termios->c_cflag & PARENB) {
 		cval |= UART_LCR_PARITY;
+		if (up->bugs & UART_BUG_PARITY)
+			fifo_bug = 1;
+	}
 	if (!(termios->c_cflag & PARODD))
 		cval |= UART_LCR_EPAR;
 #ifdef CMSPAR
@@ -2246,7 +2259,7 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	if (up->capabilities & UART_CAP_FIFO && port->fifosize > 1) {
 		fcr = uart_config[port->type].fcr;
-		if (baud < 2400) {
+		if (baud < 2400 || fifo_bug) {
 			fcr &= ~UART_FCR_TRIGGER_MASK;
 			fcr |= UART_FCR_TRIGGER_1;
 		}
@@ -2336,7 +2349,7 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 			serial_port_out(port, UART_EFR, efr);
 	}
 
-#ifdef CONFIG_ARCH_OMAP
+#ifdef CONFIG_ARCH_OMAP1
 	/* Workaround to enable 115200 baud on OMAP1510 internal ports */
 	if (cpu_is_omap1510() && is_omap_port(up)) {
 		if (baud == 115200) {
@@ -2426,7 +2439,7 @@ static unsigned int serial8250_port_size(struct uart_8250_port *pt)
 {
 	if (pt->port.iotype == UPIO_AU)
 		return 0x1000;
-#ifdef CONFIG_ARCH_OMAP
+#ifdef CONFIG_ARCH_OMAP1
 	if (is_omap_port(pt))
 		return 0x16 << pt->port.regshift;
 #endif
@@ -2550,7 +2563,10 @@ static int serial8250_request_port(struct uart_port *port)
 {
 	struct uart_8250_port *up =
 		container_of(port, struct uart_8250_port, port);
-	int ret = 0;
+	int ret;
+
+	if (port->type == PORT_8250_CIR)
+		return -ENODEV;
 
 	ret = serial8250_request_std_resource(up);
 	if (ret == 0 && port->type == PORT_RSA) {
@@ -2569,6 +2585,9 @@ static void serial8250_config_port(struct uart_port *port, int flags)
 	int probeflags = PROBE_ANY;
 	int ret;
 
+	if (port->type == PORT_8250_CIR)
+		return;
+
 	/*
 	 * Find the region that we can probe for.  This in turn
 	 * tells us whether we can probe for the type of port.
@@ -2668,6 +2687,9 @@ static void __init serial8250_isa_init_ports(void)
 		return;
 	first = 0;
 
+	if (nr_uarts > UART_NR)
+		nr_uarts = UART_NR;
+
 	for (i = 0; i < nr_uarts; i++) {
 		struct uart_8250_port *up = &serial8250_ports[i];
 		struct uart_port *port = &up->port;
@@ -2677,6 +2699,7 @@ static void __init serial8250_isa_init_ports(void)
 
 		init_timer(&up->timer);
 		up->timer.function = serial8250_timeout;
+		up->cur_iotype = 0xFF;
 
 		/*
 		 * ALPHA_KLUDGE_MCR needs to be killed.
@@ -2728,13 +2751,9 @@ serial8250_register_ports(struct uart_driver *drv, struct device *dev)
 
 	for (i = 0; i < nr_uarts; i++) {
 		struct uart_8250_port *up = &serial8250_ports[i];
-		up->cur_iotype = 0xFF;
-	}
-
-	serial8250_isa_init_ports();
 
-	for (i = 0; i < nr_uarts; i++) {
-		struct uart_8250_port *up = &serial8250_ports[i];
+		if (up->port.dev)
+			continue;
 
 		up->port.dev = dev;
 
@@ -2859,9 +2878,6 @@ static struct console serial8250_console = {
 
 static int __init serial8250_console_init(void)
 {
-	if (nr_uarts > UART_NR)
-		nr_uarts = UART_NR;
-
 	serial8250_isa_init_ports();
 	register_console(&serial8250_console);
 	return 0;
@@ -2979,36 +2995,36 @@ void serial8250_resume_port(int line)
 static int __devinit serial8250_probe(struct platform_device *dev)
 {
 	struct plat_serial8250_port *p = dev->dev.platform_data;
-	struct uart_port port;
+	struct uart_8250_port uart;
 	int ret, i, irqflag = 0;
 
-	memset(&port, 0, sizeof(struct uart_port));
+	memset(&uart, 0, sizeof(uart));
 
 	if (share_irqs)
 		irqflag = IRQF_SHARED;
 
 	for (i = 0; p && p->flags != 0; p++, i++) {
-		port.iobase		= p->iobase;
-		port.membase		= p->membase;
-		port.irq		= p->irq;
-		port.irqflags		= p->irqflags;
-		port.uartclk		= p->uartclk;
-		port.regshift		= p->regshift;
-		port.iotype		= p->iotype;
-		port.flags		= p->flags;
-		port.mapbase		= p->mapbase;
-		port.hub6		= p->hub6;
-		port.private_data	= p->private_data;
-		port.type		= p->type;
-		port.serial_in		= p->serial_in;
-		port.serial_out		= p->serial_out;
-		port.handle_irq		= p->handle_irq;
-		port.handle_break	= p->handle_break;
-		port.set_termios	= p->set_termios;
-		port.pm			= p->pm;
-		port.dev		= &dev->dev;
-		port.irqflags		|= irqflag;
-		ret = serial8250_register_port(&port);
+		uart.port.iobase	= p->iobase;
+		uart.port.membase	= p->membase;
+		uart.port.irq		= p->irq;
+		uart.port.irqflags	= p->irqflags;
+		uart.port.uartclk	= p->uartclk;
+		uart.port.regshift	= p->regshift;
+		uart.port.iotype	= p->iotype;
+		uart.port.flags		= p->flags;
+		uart.port.mapbase	= p->mapbase;
+		uart.port.hub6		= p->hub6;
+		uart.port.private_data	= p->private_data;
+		uart.port.type		= p->type;
+		uart.port.serial_in	= p->serial_in;
+		uart.port.serial_out	= p->serial_out;
+		uart.port.handle_irq	= p->handle_irq;
+		uart.port.handle_break	= p->handle_break;
+		uart.port.set_termios	= p->set_termios;
+		uart.port.pm		= p->pm;
+		uart.port.dev		= &dev->dev;
+		uart.port.irqflags	|= irqflag;
+		ret = serial8250_register_8250_port(&uart);
 		if (ret < 0) {
 			dev_err(&dev->dev, "unable to register port at index %d "
 				"(IO%lx MEM%llx IRQ%d): %d\n", i,
@@ -3081,7 +3097,7 @@ static struct platform_driver serial8250_isa_driver = {
 static struct platform_device *serial8250_isa_devs;
 
 /*
- * serial8250_register_port and serial8250_unregister_port allows for
+ * serial8250_register_8250_port and serial8250_unregister_port allows for
  * 16x50 serial ports to be configured at run-time, to support PCMCIA
  * modems and PCI multiport cards.
  */
@@ -3143,8 +3159,9 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 	mutex_lock(&serial_mutex);
 
 	uart = serial8250_find_match_or_unused(&up->port);
-	if (uart) {
-		uart_remove_one_port(&serial8250_reg, &uart->port);
+	if (uart && uart->port.type != PORT_8250_CIR) {
+		if (uart->port.dev)
+			uart_remove_one_port(&serial8250_reg, &uart->port);
 
 		uart->port.iobase       = up->port.iobase;
 		uart->port.membase      = up->port.membase;
@@ -3155,6 +3172,7 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		uart->port.regshift     = up->port.regshift;
 		uart->port.iotype       = up->port.iotype;
 		uart->port.flags        = up->port.flags | UPF_BOOT_AUTOCONF;
+		uart->bugs		= up->bugs;
 		uart->port.mapbase      = up->port.mapbase;
 		uart->port.private_data = up->port.private_data;
 		if (up->port.dev)
@@ -3198,29 +3216,6 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 EXPORT_SYMBOL(serial8250_register_8250_port);
 
 /**
- *	serial8250_register_port - register a serial port
- *	@port: serial port template
- *
- *	Configure the serial port specified by the request. If the
- *	port exists and is in use, it is hung up and unregistered
- *	first.
- *
- *	The port is then probed and if necessary the IRQ is autodetected
- *	If this fails an error is returned.
- *
- *	On success the port is ready to use and the line number is returned.
- */
-int serial8250_register_port(struct uart_port *port)
-{
-	struct uart_8250_port up;
-
-	memset(&up, 0, sizeof(up));
-	memcpy(&up.port, port, sizeof(*port));
-	return serial8250_register_8250_port(&up);
-}
-EXPORT_SYMBOL(serial8250_register_port);
-
-/**
  *	serial8250_unregister_port - remove a 16x50 serial port at runtime
  *	@line: serial line number
  *
@@ -3250,8 +3245,7 @@ static int __init serial8250_init(void)
 {
 	int ret;
 
-	if (nr_uarts > UART_NR)
-		nr_uarts = UART_NR;
+	serial8250_isa_init_ports();
 
 	printk(KERN_INFO "Serial: 8250/16550 driver, "
 		"%d ports, IRQ sharing %sabled\n", nr_uarts,
@@ -3266,11 +3260,15 @@ static int __init serial8250_init(void)
 	if (ret)
 		goto out;
 
+	ret = serial8250_pnp_init();
+	if (ret)
+		goto unreg_uart_drv;
+
 	serial8250_isa_devs = platform_device_alloc("serial8250",
 						    PLAT8250_DEV_LEGACY);
 	if (!serial8250_isa_devs) {
 		ret = -ENOMEM;
-		goto unreg_uart_drv;
+		goto unreg_pnp;
 	}
 
 	ret = platform_device_add(serial8250_isa_devs);
@@ -3286,6 +3284,8 @@ static int __init serial8250_init(void)
 	platform_device_del(serial8250_isa_devs);
 put_dev:
 	platform_device_put(serial8250_isa_devs);
+unreg_pnp:
+	serial8250_pnp_exit();
 unreg_uart_drv:
 #ifdef CONFIG_SPARC
 	sunserial_unregister_minors(&serial8250_reg, UART_NR);
@@ -3310,6 +3310,8 @@ static void __exit serial8250_exit(void)
 	platform_driver_unregister(&serial8250_isa_driver);
 	platform_device_unregister(isa_dev);
 
+	serial8250_pnp_exit();
+
 #ifdef CONFIG_SPARC
 	sunserial_unregister_minors(&serial8250_reg, UART_NR);
 #else
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index f9719d167c8d..5a76f9c8d36b 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -13,36 +13,6 @@
 
 #include <linux/serial_8250.h>
 
-struct uart_8250_port {
-	struct uart_port	port;
-	struct timer_list	timer;		/* "no irq" timer */
-	struct list_head	list;		/* ports on this IRQ */
-	unsigned short		capabilities;	/* port capabilities */
-	unsigned short		bugs;		/* port bugs */
-	unsigned int		tx_loadsz;	/* transmit fifo load size */
-	unsigned char		acr;
-	unsigned char		ier;
-	unsigned char		lcr;
-	unsigned char		mcr;
-	unsigned char		mcr_mask;	/* mask of user bits */
-	unsigned char		mcr_force;	/* mask of forced bits */
-	unsigned char		cur_iotype;	/* Running I/O type */
-
-	/*
-	 * Some bits in registers are cleared on a read, so they must
-	 * be saved whenever the register is read but the bits will not
-	 * be immediately processed.
-	 */
-#define LSR_SAVE_FLAGS UART_LSR_BRK_ERROR_BITS
-	unsigned char		lsr_saved_flags;
-#define MSR_SAVE_FLAGS UART_MSR_ANY_DELTA
-	unsigned char		msr_saved_flags;
-
-	/* 8250 specific callbacks */
-	int			(*dl_read)(struct uart_8250_port *);
-	void			(*dl_write)(struct uart_8250_port *, int);
-};
-
 struct old_serial_port {
 	unsigned int uart;
 	unsigned int baud_base;
@@ -56,9 +26,6 @@ struct old_serial_port {
 	unsigned long irqflags;
 };
 
-/*
- * This replaces serial_uart_config in include/linux/serial.h
- */
 struct serial8250_config {
 	const char	*name;
 	unsigned short	fifo_size;
@@ -78,6 +45,7 @@ struct serial8250_config {
 #define UART_BUG_TXEN	(1 << 1)	/* UART has buggy TX IIR status */
 #define UART_BUG_NOMSR	(1 << 2)	/* UART has buggy MSR status bits (Au1x00) */
 #define UART_BUG_THRE	(1 << 3)	/* UART has buggy THRE reassertion */
+#define UART_BUG_PARITY	(1 << 4)	/* UART mishandles parity if FIFO enabled */
 
 #define PROBE_RSA	(1 << 0)
 #define PROBE_ANY	(~0)
@@ -129,3 +97,12 @@ static inline void serial_dl_write(struct uart_8250_port *up, int value)
 #else
 #define ALPHA_KLUDGE_MCR 0
 #endif
+
+#ifdef CONFIG_SERIAL_8250_PNP
+int serial8250_pnp_init(void);
+void serial8250_pnp_exit(void);
+#else
+static inline int serial8250_pnp_init(void) { return 0; }
+static inline void serial8250_pnp_exit(void) { }
+#endif
+
diff --git a/drivers/tty/serial/8250/8250_acorn.c b/drivers/tty/serial/8250/8250_acorn.c
index b0ce8c56f1a4..857498312a9a 100644
--- a/drivers/tty/serial/8250/8250_acorn.c
+++ b/drivers/tty/serial/8250/8250_acorn.c
@@ -43,7 +43,7 @@ serial_card_probe(struct expansion_card *ec, const struct ecard_id *id)
 {
 	struct serial_card_info *info;
 	struct serial_card_type *type = id->data;
-	struct uart_port port;
+	struct uart_8250_port uart;
 	unsigned long bus_addr;
 	unsigned int i;
 
@@ -62,19 +62,19 @@ serial_card_probe(struct expansion_card *ec, const struct ecard_id *id)
 
 	ecard_set_drvdata(ec, info);
 
-	memset(&port, 0, sizeof(struct uart_port));
-	port.irq	= ec->irq;
-	port.flags	= UPF_BOOT_AUTOCONF | UPF_SHARE_IRQ;
-	port.uartclk	= type->uartclk;
-	port.iotype	= UPIO_MEM;
-	port.regshift	= 2;
-	port.dev	= &ec->dev;
+	memset(&uart, 0, sizeof(struct uart_8250_port));
+	uart.port.irq	= ec->irq;
+	uart.port.flags	= UPF_BOOT_AUTOCONF | UPF_SHARE_IRQ;
+	uart.port.uartclk	= type->uartclk;
+	uart.port.iotype	= UPIO_MEM;
+	uart.port.regshift	= 2;
+	uart.port.dev	= &ec->dev;
 
 	for (i = 0; i < info->num_ports; i ++) {
-		port.membase = info->vaddr + type->offset[i];
-		port.mapbase = bus_addr + type->offset[i];
+		uart.port.membase = info->vaddr + type->offset[i];
+		uart.port.mapbase = bus_addr + type->offset[i];
 
-		info->ports[i] = serial8250_register_port(&port);
+		info->ports[i] = serial8250_register_8250_port(&uart);
 	}
 
 	return 0;
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index f574eef3075f..c3b2ec0c8c0b 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -89,7 +89,7 @@ static int dw8250_handle_irq(struct uart_port *p)
 
 static int __devinit dw8250_probe(struct platform_device *pdev)
 {
-	struct uart_port port = {};
+	struct uart_8250_port uart = {};
 	struct resource *regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct resource *irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	struct device_node *np = pdev->dev.of_node;
@@ -104,28 +104,28 @@ static int __devinit dw8250_probe(struct platform_device *pdev)
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
-	port.private_data = data;
-
-	spin_lock_init(&port.lock);
-	port.mapbase = regs->start;
-	port.irq = irq->start;
-	port.handle_irq = dw8250_handle_irq;
-	port.type = PORT_8250;
-	port.flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF | UPF_IOREMAP |
+	uart.port.private_data = data;
+
+	spin_lock_init(&uart.port.lock);
+	uart.port.mapbase = regs->start;
+	uart.port.irq = irq->start;
+	uart.port.handle_irq = dw8250_handle_irq;
+	uart.port.type = PORT_8250;
+	uart.port.flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF | UPF_IOREMAP |
 		UPF_FIXED_PORT | UPF_FIXED_TYPE;
-	port.dev = &pdev->dev;
+	uart.port.dev = &pdev->dev;
 
-	port.iotype = UPIO_MEM;
-	port.serial_in = dw8250_serial_in;
-	port.serial_out = dw8250_serial_out;
+	uart.port.iotype = UPIO_MEM;
+	uart.port.serial_in = dw8250_serial_in;
+	uart.port.serial_out = dw8250_serial_out;
 	if (!of_property_read_u32(np, "reg-io-width", &val)) {
 		switch (val) {
 		case 1:
 			break;
 		case 4:
-			port.iotype = UPIO_MEM32;
-			port.serial_in = dw8250_serial_in32;
-			port.serial_out = dw8250_serial_out32;
+			uart.port.iotype = UPIO_MEM32;
+			uart.port.serial_in = dw8250_serial_in32;
+			uart.port.serial_out = dw8250_serial_out32;
 			break;
 		default:
 			dev_err(&pdev->dev, "unsupported reg-io-width (%u)\n",
@@ -135,15 +135,15 @@ static int __devinit dw8250_probe(struct platform_device *pdev)
 	}
 
 	if (!of_property_read_u32(np, "reg-shift", &val))
-		port.regshift = val;
+		uart.port.regshift = val;
 
 	if (of_property_read_u32(np, "clock-frequency", &val)) {
 		dev_err(&pdev->dev, "no clock-frequency property set\n");
 		return -EINVAL;
 	}
-	port.uartclk = val;
+	uart.port.uartclk = val;
 
-	data->line = serial8250_register_port(&port);
+	data->line = serial8250_register_8250_port(&uart);
 	if (data->line < 0)
 		return data->line;
 
diff --git a/drivers/tty/serial/8250/8250_gsc.c b/drivers/tty/serial/8250/8250_gsc.c
index d8c0ffbfa6e3..097dff9c08ad 100644
--- a/drivers/tty/serial/8250/8250_gsc.c
+++ b/drivers/tty/serial/8250/8250_gsc.c
@@ -26,7 +26,7 @@
 
 static int __init serial_init_chip(struct parisc_device *dev)
 {
-	struct uart_port port;
+	struct uart_8250_port uart;
 	unsigned long address;
 	int err;
 
@@ -48,21 +48,21 @@ static int __init serial_init_chip(struct parisc_device *dev)
 	if (dev->id.sversion != 0x8d)
 		address += 0x800;
 
-	memset(&port, 0, sizeof(port));
-	port.iotype	= UPIO_MEM;
+	memset(&uart, 0, sizeof(uart));
+	uart.port.iotype	= UPIO_MEM;
 	/* 7.272727MHz on Lasi.  Assumed the same for Dino, Wax and Timi. */
-	port.uartclk	= 7272727;
-	port.mapbase	= address;
-	port.membase	= ioremap_nocache(address, 16);
-	port.irq	= dev->irq;
-	port.flags	= UPF_BOOT_AUTOCONF;
-	port.dev	= &dev->dev;
-
-	err = serial8250_register_port(&port);
+	uart.port.uartclk	= 7272727;
+	uart.port.mapbase	= address;
+	uart.port.membase	= ioremap_nocache(address, 16);
+	uart.port.irq	= dev->irq;
+	uart.port.flags	= UPF_BOOT_AUTOCONF;
+	uart.port.dev	= &dev->dev;
+
+	err = serial8250_register_8250_port(&uart);
 	if (err < 0) {
 		printk(KERN_WARNING
-			"serial8250_register_port returned error %d\n", err);
-		iounmap(port.membase);
+			"serial8250_register_8250_port returned error %d\n", err);
+		iounmap(uart.port.membase);
 		return err;
 	}
 
diff --git a/drivers/tty/serial/8250/8250_hp300.c b/drivers/tty/serial/8250/8250_hp300.c
index c13438c93012..8f1dd2cc00a8 100644
--- a/drivers/tty/serial/8250/8250_hp300.c
+++ b/drivers/tty/serial/8250/8250_hp300.c
@@ -171,7 +171,7 @@ static int __devinit hpdca_init_one(struct dio_dev *d,
 		return 0;
 	}
 #endif
-	memset(&port, 0, sizeof(struct uart_port));
+	memset(&uart, 0, sizeof(uart));
 
 	/* Memory mapped I/O */
 	port.iotype = UPIO_MEM;
@@ -182,7 +182,7 @@ static int __devinit hpdca_init_one(struct dio_dev *d,
 	port.membase = (char *)(port.mapbase + DIO_VIRADDRBASE);
 	port.regshift = 1;
 	port.dev = &d->dev;
-	line = serial8250_register_port(&port);
+	line = serial8250_register_8250_port(&uart);
 
 	if (line < 0) {
 		printk(KERN_NOTICE "8250_hp300: register_serial() DCA scode %d"
@@ -210,7 +210,7 @@ static int __init hp300_8250_init(void)
 #ifdef CONFIG_HPAPCI
 	int line;
 	unsigned long base;
-	struct uart_port uport;
+	struct uart_8250_port uart;
 	struct hp300_port *port;
 	int i;
 #endif
@@ -248,26 +248,26 @@ static int __init hp300_8250_init(void)
 		if (!port)
 			return -ENOMEM;
 
-		memset(&uport, 0, sizeof(struct uart_port));
+		memset(&uart, 0, sizeof(uart));
 
 		base = (FRODO_BASE + FRODO_APCI_OFFSET(i));
 
 		/* Memory mapped I/O */
-		uport.iotype = UPIO_MEM;
-		uport.flags = UPF_SKIP_TEST | UPF_SHARE_IRQ \
+		uart.port.iotype = UPIO_MEM;
+		uart.port.flags = UPF_SKIP_TEST | UPF_SHARE_IRQ \
 			      | UPF_BOOT_AUTOCONF;
 		/* XXX - no interrupt support yet */
-		uport.irq = 0;
-		uport.uartclk = HPAPCI_BAUD_BASE * 16;
-		uport.mapbase = base;
-		uport.membase = (char *)(base + DIO_VIRADDRBASE);
-		uport.regshift = 2;
+		uart.port.irq = 0;
+		uart.port.uartclk = HPAPCI_BAUD_BASE * 16;
+		uart.port.mapbase = base;
+		uart.port.membase = (char *)(base + DIO_VIRADDRBASE);
+		uart.port.regshift = 2;
 
-		line = serial8250_register_port(&uport);
+		line = serial8250_register_8250_port(&uart);
 
 		if (line < 0) {
 			printk(KERN_NOTICE "8250_hp300: register_serial() APCI"
-			       " %d irq %d failed\n", i, uport.irq);
+			       " %d irq %d failed\n", i, uart.port.irq);
 			kfree(port);
 			continue;
 		}
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index 28e7c7cce893..17b7d26abf41 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -44,7 +44,7 @@ struct pci_serial_quirk {
 	int	(*init)(struct pci_dev *dev);
 	int	(*setup)(struct serial_private *,
 			 const struct pciserial_board *,
-			 struct uart_port *, int);
+			 struct uart_8250_port *, int);
 	void	(*exit)(struct pci_dev *dev);
 };
 
@@ -59,7 +59,7 @@ struct serial_private {
 };
 
 static int pci_default_setup(struct serial_private*,
-	  const struct pciserial_board*, struct uart_port*, int);
+	  const struct pciserial_board*, struct uart_8250_port *, int);
 
 static void moan_device(const char *str, struct pci_dev *dev)
 {
@@ -74,7 +74,7 @@ static void moan_device(const char *str, struct pci_dev *dev)
 }
 
 static int
-setup_port(struct serial_private *priv, struct uart_port *port,
+setup_port(struct serial_private *priv, struct uart_8250_port *port,
 	   int bar, int offset, int regshift)
 {
 	struct pci_dev *dev = priv->dev;
@@ -93,17 +93,17 @@ setup_port(struct serial_private *priv, struct uart_port *port,
 		if (!priv->remapped_bar[bar])
 			return -ENOMEM;
 
-		port->iotype = UPIO_MEM;
-		port->iobase = 0;
-		port->mapbase = base + offset;
-		port->membase = priv->remapped_bar[bar] + offset;
-		port->regshift = regshift;
+		port->port.iotype = UPIO_MEM;
+		port->port.iobase = 0;
+		port->port.mapbase = base + offset;
+		port->port.membase = priv->remapped_bar[bar] + offset;
+		port->port.regshift = regshift;
 	} else {
-		port->iotype = UPIO_PORT;
-		port->iobase = base + offset;
-		port->mapbase = 0;
-		port->membase = NULL;
-		port->regshift = 0;
+		port->port.iotype = UPIO_PORT;
+		port->port.iobase = base + offset;
+		port->port.mapbase = 0;
+		port->port.membase = NULL;
+		port->port.regshift = 0;
 	}
 	return 0;
 }
@@ -113,7 +113,7 @@ setup_port(struct serial_private *priv, struct uart_port *port,
  */
 static int addidata_apci7800_setup(struct serial_private *priv,
 				const struct pciserial_board *board,
-				struct uart_port *port, int idx)
+				struct uart_8250_port *port, int idx)
 {
 	unsigned int bar = 0, offset = board->first_offset;
 	bar = FL_GET_BASE(board->flags);
@@ -140,7 +140,7 @@ static int addidata_apci7800_setup(struct serial_private *priv,
  */
 static int
 afavlab_setup(struct serial_private *priv, const struct pciserial_board *board,
-	      struct uart_port *port, int idx)
+	      struct uart_8250_port *port, int idx)
 {
 	unsigned int bar, offset = board->first_offset;
 
@@ -195,7 +195,7 @@ static int pci_hp_diva_init(struct pci_dev *dev)
 static int
 pci_hp_diva_setup(struct serial_private *priv,
 		const struct pciserial_board *board,
-		struct uart_port *port, int idx)
+		struct uart_8250_port *port, int idx)
 {
 	unsigned int offset = board->first_offset;
 	unsigned int bar = FL_GET_BASE(board->flags);
@@ -370,7 +370,7 @@ static void __devexit pci_ni8430_exit(struct pci_dev *dev)
 /* SBS Technologies Inc. PMC-OCTPRO and P-OCTAL cards */
 static int
 sbs_setup(struct serial_private *priv, const struct pciserial_board *board,
-		struct uart_port *port, int idx)
+		struct uart_8250_port *port, int idx)
 {
 	unsigned int bar, offset = board->first_offset;
 
@@ -525,7 +525,7 @@ static int pci_siig_init(struct pci_dev *dev)
 
 static int pci_siig_setup(struct serial_private *priv,
 			  const struct pciserial_board *board,
-			  struct uart_port *port, int idx)
+			  struct uart_8250_port *port, int idx)
 {
 	unsigned int bar = FL_GET_BASE(board->flags) + idx, offset = 0;
 
@@ -619,7 +619,7 @@ static int pci_timedia_init(struct pci_dev *dev)
 static int
 pci_timedia_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
-		  struct uart_port *port, int idx)
+		  struct uart_8250_port *port, int idx)
 {
 	unsigned int bar = 0, offset = board->first_offset;
 
@@ -653,7 +653,7 @@ pci_timedia_setup(struct serial_private *priv,
 static int
 titan_400l_800l_setup(struct serial_private *priv,
 		      const struct pciserial_board *board,
-		      struct uart_port *port, int idx)
+		      struct uart_8250_port *port, int idx)
 {
 	unsigned int bar, offset = board->first_offset;
 
@@ -754,7 +754,7 @@ static int pci_ni8430_init(struct pci_dev *dev)
 static int
 pci_ni8430_setup(struct serial_private *priv,
 		 const struct pciserial_board *board,
-		 struct uart_port *port, int idx)
+		 struct uart_8250_port *port, int idx)
 {
 	void __iomem *p;
 	unsigned long base, len;
@@ -781,7 +781,7 @@ pci_ni8430_setup(struct serial_private *priv,
 
 static int pci_netmos_9900_setup(struct serial_private *priv,
 				const struct pciserial_board *board,
-				struct uart_port *port, int idx)
+				struct uart_8250_port *port, int idx)
 {
 	unsigned int bar;
 
@@ -1032,10 +1032,17 @@ static int pci_oxsemi_tornado_init(struct pci_dev *dev)
 	return number_uarts;
 }
 
-static int
-pci_default_setup(struct serial_private *priv,
+static int pci_asix_setup(struct serial_private *priv,
+		  const struct pciserial_board *board,
+		  struct uart_8250_port *port, int idx)
+{
+	port->bugs |= UART_BUG_PARITY;
+	return pci_default_setup(priv, board, port, idx);
+}
+
+static int pci_default_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
-		  struct uart_port *port, int idx)
+		  struct uart_8250_port *port, int idx)
 {
 	unsigned int bar, offset = board->first_offset, maxnr;
 
@@ -1057,15 +1064,15 @@ pci_default_setup(struct serial_private *priv,
 static int
 ce4100_serial_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
-		  struct uart_port *port, int idx)
+		  struct uart_8250_port *port, int idx)
 {
 	int ret;
 
 	ret = setup_port(priv, port, 0, 0, board->reg_shift);
-	port->iotype = UPIO_MEM32;
-	port->type = PORT_XSCALE;
-	port->flags = (port->flags | UPF_FIXED_PORT | UPF_FIXED_TYPE);
-	port->regshift = 2;
+	port->port.iotype = UPIO_MEM32;
+	port->port.type = PORT_XSCALE;
+	port->port.flags = (port->port.flags | UPF_FIXED_PORT | UPF_FIXED_TYPE);
+	port->port.regshift = 2;
 
 	return ret;
 }
@@ -1073,16 +1080,16 @@ ce4100_serial_setup(struct serial_private *priv,
 static int
 pci_omegapci_setup(struct serial_private *priv,
 		      const struct pciserial_board *board,
-		      struct uart_port *port, int idx)
+		      struct uart_8250_port *port, int idx)
 {
 	return setup_port(priv, port, 2, idx * 8, 0);
 }
 
 static int skip_tx_en_setup(struct serial_private *priv,
 			const struct pciserial_board *board,
-			struct uart_port *port, int idx)
+			struct uart_8250_port *port, int idx)
 {
-	port->flags |= UPF_NO_TXEN_TEST;
+	port->port.flags |= UPF_NO_TXEN_TEST;
 	printk(KERN_DEBUG "serial8250: skipping TxEn test for device "
 			  "[%04x:%04x] subsystem [%04x:%04x]\n",
 			  priv->dev->vendor,
@@ -1131,11 +1138,11 @@ static unsigned int kt_serial_in(struct uart_port *p, int offset)
 
 static int kt_serial_setup(struct serial_private *priv,
 			   const struct pciserial_board *board,
-			   struct uart_port *port, int idx)
+			   struct uart_8250_port *port, int idx)
 {
-	port->flags |= UPF_BUG_THRE;
-	port->serial_in = kt_serial_in;
-	port->handle_break = kt_handle_break;
+	port->port.flags |= UPF_BUG_THRE;
+	port->port.serial_in = kt_serial_in;
+	port->port.handle_break = kt_handle_break;
 	return skip_tx_en_setup(priv, board, port, idx);
 }
 
@@ -1151,9 +1158,19 @@ static int pci_eg20t_init(struct pci_dev *dev)
 static int
 pci_xr17c154_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
-		  struct uart_port *port, int idx)
+		  struct uart_8250_port *port, int idx)
 {
-	port->flags |= UPF_EXAR_EFR;
+	port->port.flags |= UPF_EXAR_EFR;
+	return pci_default_setup(priv, board, port, idx);
+}
+
+static int
+pci_wch_ch353_setup(struct serial_private *priv,
+                    const struct pciserial_board *board,
+                    struct uart_8250_port *port, int idx)
+{
+	port->port.flags |= UPF_FIXED_TYPE;
+	port->port.type = PORT_16550A;
 	return pci_default_setup(priv, board, port, idx);
 }
 
@@ -1164,6 +1181,8 @@ pci_xr17c154_setup(struct serial_private *priv,
 #define PCI_SUBDEVICE_ID_OCTPRO422	0x0208
 #define PCI_SUBDEVICE_ID_POCTAL232	0x0308
 #define PCI_SUBDEVICE_ID_POCTAL422	0x0408
+#define PCI_SUBDEVICE_ID_SIIG_DUAL_00	0x2500
+#define PCI_SUBDEVICE_ID_SIIG_DUAL_30	0x2530
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe
 #define PCI_DEVICE_ID_INTEL_CE4100_UART 0x2e66
 #define PCI_DEVICE_ID_ADVANTECH_PCI3620	0x3620
@@ -1187,6 +1206,13 @@ pci_xr17c154_setup(struct serial_private *priv,
 #define PCIE_DEVICE_ID_NEO_2_OX_IBM	0x00F6
 #define PCI_DEVICE_ID_PLX_CRONYX_OMEGA	0xc001
 #define PCI_DEVICE_ID_INTEL_PATSBURG_KT 0x1d3d
+#define PCI_VENDOR_ID_WCH		0x4348
+#define PCI_DEVICE_ID_WCH_CH353_4S	0x3453
+#define PCI_DEVICE_ID_WCH_CH353_2S1PF	0x5046
+#define PCI_DEVICE_ID_WCH_CH353_2S1P	0x7053
+#define PCI_VENDOR_ID_AGESTAR		0x5372
+#define PCI_DEVICE_ID_AGESTAR_9375	0x6872
+#define PCI_VENDOR_ID_ASIX		0x9710
 
 /* Unknown vendors/cards - this should not be in linux/pci_ids.h */
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1584	0x1584
@@ -1726,7 +1752,41 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= PCI_ANY_ID,
 		.setup		= pci_omegapci_setup,
-	 },
+	},
+	/* WCH CH353 2S1P card (16550 clone) */
+	{
+		.vendor         = PCI_VENDOR_ID_WCH,
+		.device         = PCI_DEVICE_ID_WCH_CH353_2S1P,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch353_setup,
+	},
+	/* WCH CH353 4S card (16550 clone) */
+	{
+		.vendor         = PCI_VENDOR_ID_WCH,
+		.device         = PCI_DEVICE_ID_WCH_CH353_4S,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch353_setup,
+	},
+	/* WCH CH353 2S1PF card (16550 clone) */
+	{
+		.vendor         = PCI_VENDOR_ID_WCH,
+		.device         = PCI_DEVICE_ID_WCH_CH353_2S1PF,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch353_setup,
+	},
+	/*
+	 * ASIX devices with FIFO bug
+	 */
+	{
+		.vendor		= PCI_VENDOR_ID_ASIX,
+		.device		= PCI_ANY_ID,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= pci_asix_setup,
+	},
 	/*
 	 * Default "match everything" terminator entry
 	 */
@@ -1887,7 +1947,6 @@ enum pci_board_num_t {
 	pbn_panacom,
 	pbn_panacom2,
 	pbn_panacom4,
-	pbn_exsys_4055,
 	pbn_plx_romulus,
 	pbn_oxsemi,
 	pbn_oxsemi_1_4000000,
@@ -2393,13 +2452,6 @@ static struct pciserial_board pci_boards[] __devinitdata = {
 		.reg_shift	= 7,
 	},
 
-	[pbn_exsys_4055] = {
-		.flags		= FL_BASE2,
-		.num_ports	= 4,
-		.base_baud	= 115200,
-		.uart_offset	= 8,
-	},
-
 	/* I think this entry is broken - the first_offset looks wrong --rmk */
 	[pbn_plx_romulus] = {
 		.flags		= FL_BASE2,
@@ -2624,10 +2676,14 @@ static struct pciserial_board pci_boards[] __devinitdata = {
 	},
 };
 
-static const struct pci_device_id softmodem_blacklist[] = {
+static const struct pci_device_id blacklist[] = {
+	/* softmodems */
 	{ PCI_VDEVICE(AL, 0x5457), }, /* ALi Corporation M5457 AC'97 Modem */
 	{ PCI_VDEVICE(MOTOROLA, 0x3052), }, /* Motorola Si3052-based modem */
 	{ PCI_DEVICE(0x1543, 0x3052), }, /* Si3052-based modem, default IDs */
+
+	/* multi-io cards handled by parport_serial */
+	{ PCI_DEVICE(0x4348, 0x7053), }, /* WCH CH353 2S1P */
 };
 
 /*
@@ -2638,7 +2694,7 @@ static const struct pci_device_id softmodem_blacklist[] = {
 static int __devinit
 serial_pci_guess_board(struct pci_dev *dev, struct pciserial_board *board)
 {
-	const struct pci_device_id *blacklist;
+	const struct pci_device_id *bldev;
 	int num_iomem, num_port, first_port = -1, i;
 
 	/*
@@ -2655,13 +2711,13 @@ serial_pci_guess_board(struct pci_dev *dev, struct pciserial_board *board)
 
 	/*
 	 * Do not access blacklisted devices that are known not to
-	 * feature serial ports.
+	 * feature serial ports or are handled by other modules.
 	 */
-	for (blacklist = softmodem_blacklist;
-	     blacklist < softmodem_blacklist + ARRAY_SIZE(softmodem_blacklist);
-	     blacklist++) {
-		if (dev->vendor == blacklist->vendor &&
-		    dev->device == blacklist->device)
+	for (bldev = blacklist;
+	     bldev < blacklist + ARRAY_SIZE(blacklist);
+	     bldev++) {
+		if (dev->vendor == bldev->vendor &&
+		    dev->device == bldev->device)
 			return -ENODEV;
 	}
 
@@ -2728,7 +2784,7 @@ serial_pci_matches(const struct pciserial_board *board,
 struct serial_private *
 pciserial_init_ports(struct pci_dev *dev, const struct pciserial_board *board)
 {
-	struct uart_port serial_port;
+	struct uart_8250_port uart;
 	struct serial_private *priv;
 	struct pci_serial_quirk *quirk;
 	int rc, nr_ports, i;
@@ -2768,22 +2824,22 @@ pciserial_init_ports(struct pci_dev *dev, const struct pciserial_board *board)
 	priv->dev = dev;
 	priv->quirk = quirk;
 
-	memset(&serial_port, 0, sizeof(struct uart_port));
-	serial_port.flags = UPF_SKIP_TEST | UPF_BOOT_AUTOCONF | UPF_SHARE_IRQ;
-	serial_port.uartclk = board->base_baud * 16;
-	serial_port.irq = get_pci_irq(dev, board);
-	serial_port.dev = &dev->dev;
+	memset(&uart, 0, sizeof(uart));
+	uart.port.flags = UPF_SKIP_TEST | UPF_BOOT_AUTOCONF | UPF_SHARE_IRQ;
+	uart.port.uartclk = board->base_baud * 16;
+	uart.port.irq = get_pci_irq(dev, board);
+	uart.port.dev = &dev->dev;
 
 	for (i = 0; i < nr_ports; i++) {
-		if (quirk->setup(priv, board, &serial_port, i))
+		if (quirk->setup(priv, board, &uart, i))
 			break;
 
 #ifdef SERIAL_DEBUG_PCI
 		printk(KERN_DEBUG "Setup PCI port: port %lx, irq %d, type %d\n",
-		       serial_port.iobase, serial_port.irq, serial_port.iotype);
+		       uart.port.iobase, uart.port.irq, uart.port.iotype);
 #endif
 
-		priv->line[i] = serial8250_register_port(&serial_port);
+		priv->line[i] = serial8250_register_8250_port(&uart);
 		if (priv->line[i] < 0) {
 			printk(KERN_WARNING "Couldn't register serial port %s: %d\n", pci_name(dev), priv->line[i]);
 			break;
@@ -3193,7 +3249,7 @@ static struct pci_device_id serial_pci_tbl[] = {
 	{	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050,
 		PCI_SUBVENDOR_ID_EXSYS,
 		PCI_SUBDEVICE_ID_EXSYS_4055, 0, 0,
-		pbn_exsys_4055 },
+		pbn_b2_4_115200 },
 	/*
 	 * Megawolf Romulus PCI Serial Card, from Mike Hudson
 	 * (Exoray@isys.ca)
@@ -3232,8 +3288,11 @@ static struct pci_device_id serial_pci_tbl[] = {
 		 * For now just used the hex ID 0x950a.
 		 */
 	{	PCI_VENDOR_ID_OXSEMI, 0x950a,
-		PCI_SUBVENDOR_ID_SIIG, PCI_SUBDEVICE_ID_SIIG_DUAL_SERIAL, 0, 0,
-		pbn_b0_2_115200 },
+		PCI_SUBVENDOR_ID_SIIG, PCI_SUBDEVICE_ID_SIIG_DUAL_00,
+		0, 0, pbn_b0_2_115200 },
+	{	PCI_VENDOR_ID_OXSEMI, 0x950a,
+		PCI_SUBVENDOR_ID_SIIG, PCI_SUBDEVICE_ID_SIIG_DUAL_30,
+		0, 0, pbn_b0_2_115200 },
 	{	PCI_VENDOR_ID_OXSEMI, 0x950a,
 		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
 		pbn_b0_2_1130000 },
@@ -4179,6 +4238,25 @@ static struct pci_device_id serial_pci_tbl[] = {
 		pbn_omegapci },
 
 	/*
+	 * AgeStar as-prs2-009
+	 */
+	{	PCI_VENDOR_ID_AGESTAR, PCI_DEVICE_ID_AGESTAR_9375,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, pbn_b0_bt_2_115200 },
+
+	/*
+	 * WCH CH353 series devices: The 2S1P is handled by parport_serial
+	 * so not listed here.
+	 */
+	{	PCI_VENDOR_ID_WCH, PCI_DEVICE_ID_WCH_CH353_4S,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, pbn_b0_bt_4_115200 },
+
+	{	PCI_VENDOR_ID_WCH, PCI_DEVICE_ID_WCH_CH353_2S1PF,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, pbn_b0_bt_2_115200 },
+
+	/*
 	 * These entries match devices with class COMMUNICATION_SERIAL,
 	 * COMMUNICATION_MODEM or COMMUNICATION_MULTISERIAL
 	 */
@@ -4236,7 +4314,7 @@ static void serial8250_io_resume(struct pci_dev *dev)
 		pciserial_resume_ports(priv);
 }
 
-static struct pci_error_handlers serial8250_err_handler = {
+static const struct pci_error_handlers serial8250_err_handler = {
 	.error_detected = serial8250_io_error_detected,
 	.slot_reset = serial8250_io_slot_reset,
 	.resume = serial8250_io_resume,
diff --git a/drivers/tty/serial/8250/8250_pnp.c b/drivers/tty/serial/8250/8250_pnp.c
index a2f236510ff1..f8ee25001dd0 100644
--- a/drivers/tty/serial/8250/8250_pnp.c
+++ b/drivers/tty/serial/8250/8250_pnp.c
@@ -1,5 +1,5 @@
 /*
- *  Probe module for 8250/16550-type ISAPNP serial ports.
+ *  Probe for 8250/16550-type ISAPNP serial ports.
  *
  *  Based on drivers/char/serial.c, by Linus Torvalds, Theodore Ts'o.
  *
@@ -25,7 +25,7 @@
 #include "8250.h"
 
 #define UNKNOWN_DEV 0x3000
-
+#define CIR_PORT	0x0800
 
 static const struct pnp_device_id pnp_dev_table[] = {
 	/* Archtek America Corp. */
@@ -362,6 +362,9 @@ static const struct pnp_device_id pnp_dev_table[] = {
 	{	"PNPCXXX",		UNKNOWN_DEV	},
 	/* More unknown PnP modems */
 	{	"PNPDXXX",		UNKNOWN_DEV	},
+	/* Winbond CIR port, should not be probed. We should keep track
+	   of it to prevent the legacy serial driver from probing it */
+	{	"WEC1022",		CIR_PORT	},
 	{	"",			0	}
 };
 
@@ -409,7 +412,7 @@ static int __devinit check_resources(struct pnp_dev *dev)
  * PnP modems, alternatively we must hardcode all modems in pnp_devices[]
  * table.
  */
-static int __devinit serial_pnp_guess_board(struct pnp_dev *dev, int *flags)
+static int __devinit serial_pnp_guess_board(struct pnp_dev *dev)
 {
 	if (!(check_name(pnp_dev_name(dev)) ||
 		(dev->card && check_name(dev->card->name))))
@@ -424,42 +427,49 @@ static int __devinit serial_pnp_guess_board(struct pnp_dev *dev, int *flags)
 static int __devinit
 serial_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
 {
-	struct uart_port port;
+	struct uart_8250_port uart;
 	int ret, line, flags = dev_id->driver_data;
 
 	if (flags & UNKNOWN_DEV) {
-		ret = serial_pnp_guess_board(dev, &flags);
+		ret = serial_pnp_guess_board(dev);
 		if (ret < 0)
 			return ret;
 	}
 
-	memset(&port, 0, sizeof(struct uart_port));
+	memset(&uart, 0, sizeof(uart));
 	if (pnp_irq_valid(dev, 0))
-		port.irq = pnp_irq(dev, 0);
-	if (pnp_port_valid(dev, 0)) {
-		port.iobase = pnp_port_start(dev, 0);
-		port.iotype = UPIO_PORT;
+		uart.port.irq = pnp_irq(dev, 0);
+	if ((flags & CIR_PORT) && pnp_port_valid(dev, 2)) {
+		uart.port.iobase = pnp_port_start(dev, 2);
+		uart.port.iotype = UPIO_PORT;
+	} else if (pnp_port_valid(dev, 0)) {
+		uart.port.iobase = pnp_port_start(dev, 0);
+		uart.port.iotype = UPIO_PORT;
 	} else if (pnp_mem_valid(dev, 0)) {
-		port.mapbase = pnp_mem_start(dev, 0);
-		port.iotype = UPIO_MEM;
-		port.flags = UPF_IOREMAP;
+		uart.port.mapbase = pnp_mem_start(dev, 0);
+		uart.port.iotype = UPIO_MEM;
+		uart.port.flags = UPF_IOREMAP;
 	} else
 		return -ENODEV;
 
 #ifdef SERIAL_DEBUG_PNP
 	printk(KERN_DEBUG
 		"Setup PNP port: port %x, mem 0x%lx, irq %d, type %d\n",
-		       port.iobase, port.mapbase, port.irq, port.iotype);
+		       uart.port.iobase, uart.port.mapbase, uart.port.irq, uart.port.iotype);
 #endif
+	if (flags & CIR_PORT) {
+		uart.port.flags |= UPF_FIXED_PORT | UPF_FIXED_TYPE;
+		uart.port.type = PORT_8250_CIR;
+	}
 
-	port.flags |= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF;
+	uart.port.flags |= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF;
 	if (pnp_irq_flags(dev, 0) & IORESOURCE_IRQ_SHAREABLE)
-		port.flags |= UPF_SHARE_IRQ;
-	port.uartclk = 1843200;
-	port.dev = &dev->dev;
+		uart.port.flags |= UPF_SHARE_IRQ;
+	uart.port.uartclk = 1843200;
+	uart.port.dev = &dev->dev;
 
-	line = serial8250_register_port(&port);
-	if (line < 0)
+	line = serial8250_register_8250_port(&uart);
+	if (line < 0 || (flags & CIR_PORT))
 		return -ENODEV;
 
 	pnp_set_drvdata(dev, (void *)((long)line + 1));
@@ -507,18 +517,13 @@ static struct pnp_driver serial_pnp_driver = {
 	.id_table	= pnp_dev_table,
 };
 
-static int __init serial8250_pnp_init(void)
+int serial8250_pnp_init(void)
 {
 	return pnp_register_driver(&serial_pnp_driver);
 }
 
-static void __exit serial8250_pnp_exit(void)
+void serial8250_pnp_exit(void)
 {
 	pnp_unregister_driver(&serial_pnp_driver);
 }
 
-module_init(serial8250_pnp_init);
-module_exit(serial8250_pnp_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Generic 8250/16x50 PnP serial driver");
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index a27dd0569bd7..f3d283f2e3aa 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -33,6 +33,14 @@ config SERIAL_8250
 	  Most people will say Y or M here, so that they can use serial mice,
 	  modems and similar devices connecting to the standard serial ports.
 
+config SERIAL_8250_PNP
+	bool "8250/16550 PNP device support" if EXPERT
+	depends on SERIAL_8250 && PNP
+	default y
+	---help---
+	  This builds standard PNP serial support. You may be able to
+	  disable this feature if you only need legacy serial support.
+
 config SERIAL_8250_CONSOLE
 	bool "Console on 8250/16550 and compatible serial port"
 	depends on SERIAL_8250=y
@@ -85,14 +93,6 @@ config SERIAL_8250_PCI
 	  disable this feature if you only need legacy serial support.
 	  Saves about 9K.
 
-config SERIAL_8250_PNP
-	tristate "8250/16550 PNP device support" if EXPERT
-	depends on SERIAL_8250 && PNP
-	default SERIAL_8250
-	help
-	  This builds standard PNP serial support. You may be able to
-	  disable this feature if you only need legacy serial support.
-
 config SERIAL_8250_HP300
 	tristate
 	depends on SERIAL_8250 && HP300
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index d7533c7d2c1a..108fe7fe13e2 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -2,8 +2,9 @@
 # Makefile for the 8250 serial device drivers.
 #
 
-obj-$(CONFIG_SERIAL_8250)		+= 8250.o
-obj-$(CONFIG_SERIAL_8250_PNP)		+= 8250_pnp.o
+obj-$(CONFIG_SERIAL_8250)		+= 8250_core.o
+8250_core-y				:= 8250.o
+8250_core-$(CONFIG_SERIAL_8250_PNP)	+= 8250_pnp.o
 obj-$(CONFIG_SERIAL_8250_GSC)		+= 8250_gsc.o
 obj-$(CONFIG_SERIAL_8250_PCI)		+= 8250_pci.o
 obj-$(CONFIG_SERIAL_8250_HP300)		+= 8250_hp300.o
diff --git a/drivers/tty/serial/8250/serial_cs.c b/drivers/tty/serial/8250/serial_cs.c
index 29b695d041ec..b7d48b346393 100644
--- a/drivers/tty/serial/8250/serial_cs.c
+++ b/drivers/tty/serial/8250/serial_cs.c
@@ -73,7 +73,7 @@ struct serial_quirk {
 	unsigned int prodid;
 	int multi;		/* 1 = multifunction, > 1 = # ports */
 	void (*config)(struct pcmcia_device *);
-	void (*setup)(struct pcmcia_device *, struct uart_port *);
+	void (*setup)(struct pcmcia_device *, struct uart_8250_port *);
 	void (*wakeup)(struct pcmcia_device *);
 	int (*post)(struct pcmcia_device *);
 };
@@ -105,9 +105,9 @@ struct serial_cfg_mem {
  * Elan VPU16551 UART with 14.7456MHz oscillator
  * manfid 0x015D, 0x4C45
  */
-static void quirk_setup_brainboxes_0104(struct pcmcia_device *link, struct uart_port *port)
+static void quirk_setup_brainboxes_0104(struct pcmcia_device *link, struct uart_8250_port *uart)
 {
-	port->uartclk = 14745600;
+	uart->port.uartclk = 14745600;
 }
 
 static int quirk_post_ibm(struct pcmcia_device *link)
@@ -343,25 +343,25 @@ static void serial_detach(struct pcmcia_device *link)
 static int setup_serial(struct pcmcia_device *handle, struct serial_info * info,
 			unsigned int iobase, int irq)
 {
-	struct uart_port port;
+	struct uart_8250_port uart;
 	int line;
 
-	memset(&port, 0, sizeof (struct uart_port));
-	port.iobase = iobase;
-	port.irq = irq;
-	port.flags = UPF_BOOT_AUTOCONF | UPF_SKIP_TEST | UPF_SHARE_IRQ;
-	port.uartclk = 1843200;
-	port.dev = &handle->dev;
+	memset(&uart, 0, sizeof(uart));
+	uart.port.iobase = iobase;
+	uart.port.irq = irq;
+	uart.port.flags = UPF_BOOT_AUTOCONF | UPF_SKIP_TEST | UPF_SHARE_IRQ;
+	uart.port.uartclk = 1843200;
+	uart.port.dev = &handle->dev;
 	if (buggy_uart)
-		port.flags |= UPF_BUGGY_UART;
+		uart.port.flags |= UPF_BUGGY_UART;
 
 	if (info->quirk && info->quirk->setup)
-		info->quirk->setup(handle, &port);
+		info->quirk->setup(handle, &uart);
 
-	line = serial8250_register_port(&port);
+	line = serial8250_register_8250_port(&uart);
 	if (line < 0) {
-		printk(KERN_NOTICE "serial_cs: serial8250_register_port() at "
-		       "0x%04lx, irq %d failed\n", (u_long)iobase, irq);
+		pr_err("serial_cs: serial8250_register_8250_port() at 0x%04lx, irq %d failed\n",
+							(unsigned long)iobase, irq);
 		return -EINVAL;
 	}
 
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 4720b4ba096a..233fbaaf2559 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -141,6 +141,25 @@ config SERIAL_ATMEL_TTYAT
 
 	  Say Y if you have an external 8250/16C550 UART.  If unsure, say N.
 
+config SERIAL_KGDB_NMI
+	bool "Serial console over KGDB NMI debugger port"
+	depends on KGDB_SERIAL_CONSOLE
+	help
+	  This special driver allows you to temporary use NMI debugger port
+	  as a normal console (assuming that the port is attached to KGDB).
+
+	  Unlike KDB's disable_nmi command, with this driver you are always
+	  able to go back to the debugger using KGDB escape sequence ($3#33).
+	  This is because this console driver processes the input in NMI
+	  context, and thus is able to intercept the magic sequence.
+
+	  Note that since the console interprets input and uses polling
+	  communication methods, for things like PPP you still must fully
+	  detach debugger port from the KGDB NMI (i.e. disable_nmi), and
+	  use raw console.
+
+	  If unsure, say N.
+
 config SERIAL_KS8695
 	bool "Micrel KS8695 (Centaur) serial port support"
 	depends on ARCH_KS8695
@@ -257,12 +276,19 @@ config SERIAL_MAX3100
 	help
 	  MAX3100 chip support
 
-config SERIAL_MAX3107
-	tristate "MAX3107 support"
+config SERIAL_MAX310X
+	bool "MAX310X support"
 	depends on SPI
 	select SERIAL_CORE
+	select REGMAP_SPI if SPI
+	default n
 	help
-	  MAX3107 chip support
+	  This selects support for an advanced UART from Maxim (Dallas).
+	  Supported ICs are MAX3107, MAX3108.
+	  Each IC contains 128 words each of receive and transmit FIFO
+	  that can be controlled through I2C or high-speed SPI.
+
+	  Say Y here if you want to support this ICs.
 
 config SERIAL_DZ
 	bool "DECstation DZ serial driver"
@@ -686,7 +712,7 @@ config SERIAL_SH_SCI_CONSOLE
 
 config SERIAL_SH_SCI_DMA
 	bool "DMA support"
-	depends on SERIAL_SH_SCI && SH_DMAE && EXPERIMENTAL
+	depends on SERIAL_SH_SCI && SH_DMAE
 
 config SERIAL_PNX8XXX
 	bool "Enable PNX8XXX SoCs' UART Support"
@@ -704,6 +730,25 @@ config SERIAL_PNX8XXX_CONSOLE
 	  If you have a MIPS-based Philips SoC such as PNX8550 or PNX8330
 	  and you want to use serial console, say Y. Otherwise, say N.
 
+config SERIAL_HS_LPC32XX
+	tristate "LPC32XX high speed serial port support"
+	depends on ARCH_LPC32XX && OF
+	select SERIAL_CORE
+	help
+	  Support for the LPC32XX high speed serial ports (up to 900kbps).
+	  Those are UARTs completely different from the Standard UARTs on the
+	  LPC32XX SoC.
+	  Choose M or Y here to build this driver.
+
+config SERIAL_HS_LPC32XX_CONSOLE
+	bool "Enable LPC32XX high speed UART serial console"
+	depends on SERIAL_HS_LPC32XX
+	select SERIAL_CORE_CONSOLE
+	help
+	  If you would like to be able to use one of the high speed serial
+	  ports on the LPC32XX as the console, you can do so by answering
+	  Y to this option.
+
 config SERIAL_CORE
 	tristate
 
@@ -1104,6 +1149,24 @@ config SERIAL_SC26XX_CONSOLE
 	help
 	  Support for Console on SC2681/SC2692 serial ports.
 
+config SERIAL_SCCNXP
+	bool "SCCNXP serial port support"
+	depends on !SERIAL_SC26XX
+	select SERIAL_CORE
+	default n
+	help
+	  This selects support for an advanced UART from NXP (Philips).
+	  Supported ICs are SCC2681, SCC2691, SCC2692, SC28L91, SC28L92,
+	  SC28L202, SCC68681 and SCC68692.
+	  Positioned as a replacement for the driver SC26XX.
+
+config SERIAL_SCCNXP_CONSOLE
+	bool "Console on SCCNXP serial port"
+	depends on SERIAL_SCCNXP
+	select SERIAL_CORE_CONSOLE
+	help
+	  Support for console on SCCNXP serial ports.
+
 config SERIAL_BFIN_SPORT
 	tristate "Blackfin SPORT emulate UART"
 	depends on BLACKFIN
@@ -1260,7 +1323,7 @@ config SERIAL_ALTERA_UART_CONSOLE
 
 config SERIAL_IFX6X60
         tristate "SPI protocol driver for Infineon 6x60 modem (EXPERIMENTAL)"
-	depends on GPIOLIB && SPI && EXPERIMENTAL
+	depends on GPIOLIB && SPI
 	help
 	  Support for the IFX6x60 modem devices on Intel MID platforms.
 
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 7257c5d898ae..4f694dafa719 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -28,12 +28,13 @@ obj-$(CONFIG_SERIAL_BFIN) += bfin_uart.o
 obj-$(CONFIG_SERIAL_BFIN_SPORT) += bfin_sport_uart.o
 obj-$(CONFIG_SERIAL_SAMSUNG) += samsung.o
 obj-$(CONFIG_SERIAL_MAX3100) += max3100.o
-obj-$(CONFIG_SERIAL_MAX3107) += max3107.o
+obj-$(CONFIG_SERIAL_MAX310X) += max310x.o
 obj-$(CONFIG_SERIAL_IP22_ZILOG) += ip22zilog.o
 obj-$(CONFIG_SERIAL_MUX) += mux.o
 obj-$(CONFIG_SERIAL_68328) += 68328serial.o
 obj-$(CONFIG_SERIAL_MCF) += mcf.o
 obj-$(CONFIG_SERIAL_PMACZILOG) += pmac_zilog.o
+obj-$(CONFIG_SERIAL_HS_LPC32XX) += lpc32xx_hs.o
 obj-$(CONFIG_SERIAL_DZ) += dz.o
 obj-$(CONFIG_SERIAL_ZS) += zs.o
 obj-$(CONFIG_SERIAL_SH_SCI) += sh-sci.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_SERIAL_MPSC) += mpsc.o
 obj-$(CONFIG_SERIAL_SB1250_DUART) += sb1250-duart.o
 obj-$(CONFIG_ETRAX_SERIAL) += crisv10.o
 obj-$(CONFIG_SERIAL_SC26XX) += sc26xx.o
+obj-$(CONFIG_SERIAL_SCCNXP) += sccnxp.o
 obj-$(CONFIG_SERIAL_JSM) += jsm/
 obj-$(CONFIG_SERIAL_TXX9) += serial_txx9.o
 obj-$(CONFIG_SERIAL_VR41XX) += vr41xx_siu.o
@@ -59,6 +61,7 @@ obj-$(CONFIG_SERIAL_MSM_HS) += msm_serial_hs.o
 obj-$(CONFIG_SERIAL_NETX) += netx-serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM) += of_serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL) += nwpserial.o
+obj-$(CONFIG_SERIAL_KGDB_NMI) += kgdb_nmi.o
 obj-$(CONFIG_SERIAL_KS8695) += serial_ks8695.o
 obj-$(CONFIG_SERIAL_OMAP) += omap-serial.o
 obj-$(CONFIG_SERIAL_ALTERA_UART) += altera_uart.o
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index 1f0330915d5a..15d80b9fb303 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -591,7 +591,7 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
 	port->ops = &altera_uart_ops;
 	port->flags = UPF_BOOT_AUTOCONF;
 
-	dev_set_drvdata(&pdev->dev, port);
+	platform_set_drvdata(pdev, port);
 
 	uart_add_one_port(&altera_uart_driver, port);
 
@@ -600,11 +600,11 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
 
 static int __devexit altera_uart_remove(struct platform_device *pdev)
 {
-	struct uart_port *port = dev_get_drvdata(&pdev->dev);
+	struct uart_port *port = platform_get_drvdata(pdev);
 
 	if (port) {
 		uart_remove_one_port(&altera_uart_driver, port);
-		dev_set_drvdata(&pdev->dev, NULL);
+		platform_set_drvdata(pdev, NULL);
 		port->mapbase = 0;
 	}
 
diff --git a/drivers/tty/serial/amba-pl010.c b/drivers/tty/serial/amba-pl010.c
index 0d91a540bf11..22317dd16474 100644
--- a/drivers/tty/serial/amba-pl010.c
+++ b/drivers/tty/serial/amba-pl010.c
@@ -312,16 +312,12 @@ static int pl010_startup(struct uart_port *port)
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
 	int retval;
 
-	retval = clk_prepare(uap->clk);
-	if (retval)
-		goto out;
-
 	/*
 	 * Try to enable the clock producer.
 	 */
-	retval = clk_enable(uap->clk);
+	retval = clk_prepare_enable(uap->clk);
 	if (retval)
-		goto clk_unprep;
+		goto out;
 
 	uap->port.uartclk = clk_get_rate(uap->clk);
 
@@ -346,9 +342,7 @@ static int pl010_startup(struct uart_port *port)
 	return 0;
 
  clk_dis:
-	clk_disable(uap->clk);
- clk_unprep:
-	clk_unprepare(uap->clk);
+	clk_disable_unprepare(uap->clk);
  out:
 	return retval;
 }
@@ -375,8 +369,7 @@ static void pl010_shutdown(struct uart_port *port)
 	/*
 	 * Shut down the clock producer
 	 */
-	clk_disable(uap->clk);
-	clk_unprepare(uap->clk);
+	clk_disable_unprepare(uap->clk);
 }
 
 static void
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index d3553b5d3fca..d7e1edec50b5 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -52,6 +52,8 @@
 #include <linux/scatterlist.h>
 #include <linux/delay.h>
 #include <linux/types.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/sizes.h>
 
@@ -75,7 +77,6 @@ struct vendor_data {
 	unsigned int		lcrh_tx;
 	unsigned int		lcrh_rx;
 	bool			oversampling;
-	bool			interrupt_may_hang;   /* vendor-specific */
 	bool			dma_threshold;
 	bool			cts_event_workaround;
 };
@@ -96,7 +97,6 @@ static struct vendor_data vendor_st = {
 	.lcrh_tx		= ST_UART011_LCRH_TX,
 	.lcrh_rx		= ST_UART011_LCRH_RX,
 	.oversampling		= true,
-	.interrupt_may_hang	= true,
 	.dma_threshold		= true,
 	.cts_event_workaround	= true,
 };
@@ -147,7 +147,6 @@ struct uart_amba_port {
 	unsigned int		old_cr;		/* state during shutdown */
 	bool			autorts;
 	char			type[12];
-	bool			interrupt_may_hang; /* vendor-specific */
 #ifdef CONFIG_DMA_ENGINE
 	/* DMA stuff */
 	bool			using_tx_dma;
@@ -1215,14 +1214,14 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 	return IRQ_RETVAL(handled);
 }
 
-static unsigned int pl01x_tx_empty(struct uart_port *port)
+static unsigned int pl011_tx_empty(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
 	unsigned int status = readw(uap->port.membase + UART01x_FR);
 	return status & (UART01x_FR_BUSY|UART01x_FR_TXFF) ? 0 : TIOCSER_TEMT;
 }
 
-static unsigned int pl01x_get_mctrl(struct uart_port *port)
+static unsigned int pl011_get_mctrl(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
 	unsigned int result = 0;
@@ -1285,11 +1284,40 @@ static void pl011_break_ctl(struct uart_port *port, int break_state)
 }
 
 #ifdef CONFIG_CONSOLE_POLL
-static int pl010_get_poll_char(struct uart_port *port)
+
+static void pl011_quiesce_irqs(struct uart_port *port)
+{
+	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	unsigned char __iomem *regs = uap->port.membase;
+
+	writew(readw(regs + UART011_MIS), regs + UART011_ICR);
+	/*
+	 * There is no way to clear TXIM as this is "ready to transmit IRQ", so
+	 * we simply mask it. start_tx() will unmask it.
+	 *
+	 * Note we can race with start_tx(), and if the race happens, the
+	 * polling user might get another interrupt just after we clear it.
+	 * But it should be OK and can happen even w/o the race, e.g.
+	 * controller immediately got some new data and raised the IRQ.
+	 *
+	 * And whoever uses polling routines assumes that it manages the device
+	 * (including tx queue), so we're also fine with start_tx()'s caller
+	 * side.
+	 */
+	writew(readw(regs + UART011_IMSC) & ~UART011_TXIM, regs + UART011_IMSC);
+}
+
+static int pl011_get_poll_char(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
 	unsigned int status;
 
+	/*
+	 * The caller might need IRQs lowered, e.g. if used with KDB NMI
+	 * debugger.
+	 */
+	pl011_quiesce_irqs(port);
+
 	status = readw(uap->port.membase + UART01x_FR);
 	if (status & UART01x_FR_RXFE)
 		return NO_POLL_CHAR;
@@ -1297,7 +1325,7 @@ static int pl010_get_poll_char(struct uart_port *port)
 	return readw(uap->port.membase + UART01x_DR);
 }
 
-static void pl010_put_poll_char(struct uart_port *port,
+static void pl011_put_poll_char(struct uart_port *port,
 			 unsigned char ch)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
@@ -1310,10 +1338,9 @@ static void pl010_put_poll_char(struct uart_port *port,
 
 #endif /* CONFIG_CONSOLE_POLL */
 
-static int pl011_startup(struct uart_port *port)
+static int pl011_hwinit(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
-	unsigned int cr;
 	int retval;
 
 	/* Optionaly enable pins to be muxed in and configured */
@@ -1324,16 +1351,12 @@ static int pl011_startup(struct uart_port *port)
 				"could not set default pins\n");
 	}
 
-	retval = clk_prepare(uap->clk);
-	if (retval)
-		goto out;
-
 	/*
 	 * Try to enable the clock producer.
 	 */
-	retval = clk_enable(uap->clk);
+	retval = clk_prepare_enable(uap->clk);
 	if (retval)
-		goto clk_unprep;
+		goto out;
 
 	uap->port.uartclk = clk_get_rate(uap->clk);
 
@@ -1342,6 +1365,37 @@ static int pl011_startup(struct uart_port *port)
 	       UART011_RTIS | UART011_RXIS, uap->port.membase + UART011_ICR);
 
 	/*
+	 * Save interrupts enable mask, and enable RX interrupts in case if
+	 * the interrupt is used for NMI entry.
+	 */
+	uap->im = readw(uap->port.membase + UART011_IMSC);
+	writew(UART011_RTIM | UART011_RXIM, uap->port.membase + UART011_IMSC);
+
+	if (uap->port.dev->platform_data) {
+		struct amba_pl011_data *plat;
+
+		plat = uap->port.dev->platform_data;
+		if (plat->init)
+			plat->init();
+	}
+	return 0;
+ out:
+	return retval;
+}
+
+static int pl011_startup(struct uart_port *port)
+{
+	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	unsigned int cr;
+	int retval;
+
+	retval = pl011_hwinit(port);
+	if (retval)
+		goto clk_dis;
+
+	writew(uap->im, uap->port.membase + UART011_IMSC);
+
+	/*
 	 * Allocate the IRQ
 	 */
 	retval = request_irq(uap->port.irq, pl011_int, 0, "uart-pl011", uap);
@@ -1400,21 +1454,10 @@ static int pl011_startup(struct uart_port *port)
 	writew(uap->im, uap->port.membase + UART011_IMSC);
 	spin_unlock_irq(&uap->port.lock);
 
-	if (uap->port.dev->platform_data) {
-		struct amba_pl011_data *plat;
-
-		plat = uap->port.dev->platform_data;
-		if (plat->init)
-			plat->init();
-	}
-
 	return 0;
 
  clk_dis:
-	clk_disable(uap->clk);
- clk_unprep:
-	clk_unprepare(uap->clk);
- out:
+	clk_disable_unprepare(uap->clk);
 	return retval;
 }
 
@@ -1473,8 +1516,7 @@ static void pl011_shutdown(struct uart_port *port)
 	/*
 	 * Shut down the clock producer
 	 */
-	clk_disable(uap->clk);
-	clk_unprepare(uap->clk);
+	clk_disable_unprepare(uap->clk);
 	/* Optionally let pins go into sleep states */
 	if (!IS_ERR(uap->pins_sleep)) {
 		retval = pinctrl_select_state(uap->pinctrl, uap->pins_sleep);
@@ -1603,13 +1645,26 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 			old_cr &= ~ST_UART011_CR_OVSFACT;
 	}
 
+	/*
+	 * Workaround for the ST Micro oversampling variants to
+	 * increase the bitrate slightly, by lowering the divisor,
+	 * to avoid delayed sampling of start bit at high speeds,
+	 * else we see data corruption.
+	 */
+	if (uap->vendor->oversampling) {
+		if ((baud >= 3000000) && (baud < 3250000) && (quot > 1))
+			quot -= 1;
+		else if ((baud > 3250000) && (quot > 2))
+			quot -= 2;
+	}
 	/* Set baud rate */
 	writew(quot & 0x3f, port->membase + UART011_FBRD);
 	writew(quot >> 6, port->membase + UART011_IBRD);
 
 	/*
 	 * ----------v----------v----------v----------v-----
-	 * NOTE: MUST BE WRITTEN AFTER UARTLCR_M & UARTLCR_L
+	 * NOTE: lcrh_tx and lcrh_rx MUST BE WRITTEN AFTER
+	 * UART011_FBRD & UART011_IBRD.
 	 * ----------^----------^----------^----------^-----
 	 */
 	writew(lcr_h, port->membase + uap->lcrh_rx);
@@ -1637,7 +1692,7 @@ static const char *pl011_type(struct uart_port *port)
 /*
  * Release the memory region(s) being used by 'port'
  */
-static void pl010_release_port(struct uart_port *port)
+static void pl011_release_port(struct uart_port *port)
 {
 	release_mem_region(port->mapbase, SZ_4K);
 }
@@ -1645,7 +1700,7 @@ static void pl010_release_port(struct uart_port *port)
 /*
  * Request the memory region(s) being used by 'port'
  */
-static int pl010_request_port(struct uart_port *port)
+static int pl011_request_port(struct uart_port *port)
 {
 	return request_mem_region(port->mapbase, SZ_4K, "uart-pl011")
 			!= NULL ? 0 : -EBUSY;
@@ -1654,18 +1709,18 @@ static int pl010_request_port(struct uart_port *port)
 /*
  * Configure/autoconfigure the port.
  */
-static void pl010_config_port(struct uart_port *port, int flags)
+static void pl011_config_port(struct uart_port *port, int flags)
 {
 	if (flags & UART_CONFIG_TYPE) {
 		port->type = PORT_AMBA;
-		pl010_request_port(port);
+		pl011_request_port(port);
 	}
 }
 
 /*
  * verify the new serial_struct (for TIOCSSERIAL).
  */
-static int pl010_verify_port(struct uart_port *port, struct serial_struct *ser)
+static int pl011_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
 	int ret = 0;
 	if (ser->type != PORT_UNKNOWN && ser->type != PORT_AMBA)
@@ -1678,9 +1733,9 @@ static int pl010_verify_port(struct uart_port *port, struct serial_struct *ser)
 }
 
 static struct uart_ops amba_pl011_pops = {
-	.tx_empty	= pl01x_tx_empty,
+	.tx_empty	= pl011_tx_empty,
 	.set_mctrl	= pl011_set_mctrl,
-	.get_mctrl	= pl01x_get_mctrl,
+	.get_mctrl	= pl011_get_mctrl,
 	.stop_tx	= pl011_stop_tx,
 	.start_tx	= pl011_start_tx,
 	.stop_rx	= pl011_stop_rx,
@@ -1691,13 +1746,14 @@ static struct uart_ops amba_pl011_pops = {
 	.flush_buffer	= pl011_dma_flush_buffer,
 	.set_termios	= pl011_set_termios,
 	.type		= pl011_type,
-	.release_port	= pl010_release_port,
-	.request_port	= pl010_request_port,
-	.config_port	= pl010_config_port,
-	.verify_port	= pl010_verify_port,
+	.release_port	= pl011_release_port,
+	.request_port	= pl011_request_port,
+	.config_port	= pl011_config_port,
+	.verify_port	= pl011_verify_port,
 #ifdef CONFIG_CONSOLE_POLL
-	.poll_get_char = pl010_get_poll_char,
-	.poll_put_char = pl010_put_poll_char,
+	.poll_init     = pl011_hwinit,
+	.poll_get_char = pl011_get_poll_char,
+	.poll_put_char = pl011_put_poll_char,
 #endif
 };
 
@@ -1869,6 +1925,38 @@ static struct uart_driver amba_reg = {
 	.cons			= AMBA_CONSOLE,
 };
 
+static int pl011_probe_dt_alias(int index, struct device *dev)
+{
+	struct device_node *np;
+	static bool seen_dev_with_alias = false;
+	static bool seen_dev_without_alias = false;
+	int ret = index;
+
+	if (!IS_ENABLED(CONFIG_OF))
+		return ret;
+
+	np = dev->of_node;
+	if (!np)
+		return ret;
+
+	ret = of_alias_get_id(np, "serial");
+	if (IS_ERR_VALUE(ret)) {
+		seen_dev_without_alias = true;
+		ret = index;
+	} else {
+		seen_dev_with_alias = true;
+		if (ret >= ARRAY_SIZE(amba_ports) || amba_ports[ret] != NULL) {
+			dev_warn(dev, "requested serial port %d  not available.\n", ret);
+			ret = index;
+		}
+	}
+
+	if (seen_dev_with_alias && seen_dev_without_alias)
+		dev_warn(dev, "aliased and non-aliased serial devices found in device tree. Serial port enumeration may be unpredictable.\n");
+
+	return ret;
+}
+
 static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct uart_amba_port *uap;
@@ -1891,6 +1979,8 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 		goto out;
 	}
 
+	i = pl011_probe_dt_alias(i, &dev->dev);
+
 	base = ioremap(dev->res.start, resource_size(&dev->res));
 	if (!base) {
 		ret = -ENOMEM;
@@ -1923,7 +2013,6 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	uap->lcrh_tx = vendor->lcrh_tx;
 	uap->old_cr = 0;
 	uap->fifosize = vendor->fifosize;
-	uap->interrupt_may_hang = vendor->interrupt_may_hang;
 	uap->port.dev = &dev->dev;
 	uap->port.mapbase = dev->res.start;
 	uap->port.membase = base;
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index bd97db23985b..9242d56ba267 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -182,7 +182,7 @@ static void bfin_serial_start_tx(struct uart_port *port)
 	 * To avoid losting RX interrupt, we reset IR function
 	 * before sending data.
 	 */
-	if (tty->termios->c_line == N_IRDA)
+	if (tty->termios.c_line == N_IRDA)
 		bfin_serial_reset_irda(port);
 
 #ifdef CONFIG_SERIAL_BFIN_DMA
diff --git a/drivers/tty/serial/cpm_uart/cpm_uart_core.c b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
index b418947b7107..d0dd9194cecc 100644
--- a/drivers/tty/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
@@ -71,7 +71,7 @@ static void cpm_uart_initbd(struct uart_cpm_port *pinfo);
 
 /**************************************************************/
 
-#define HW_BUF_SPD_THRESHOLD    9600
+#define HW_BUF_SPD_THRESHOLD    2400
 
 /*
  * Check, if transmit buffers are processed
@@ -417,6 +417,7 @@ static int cpm_uart_startup(struct uart_port *port)
 			clrbits32(&pinfo->sccp->scc_gsmrl, SCC_GSMRL_ENR);
 			clrbits16(&pinfo->sccp->scc_sccm, UART_SCCM_RX);
 		}
+		cpm_uart_initbd(pinfo);
 		cpm_line_cr_cmd(pinfo, CPM_CR_INIT_TRX);
 	}
 	/* Install interrupt handler. */
@@ -500,16 +501,28 @@ static void cpm_uart_set_termios(struct uart_port *port,
 	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
 	smc_t __iomem *smcp = pinfo->smcp;
 	scc_t __iomem *sccp = pinfo->sccp;
+	int maxidl;
 
 	pr_debug("CPM uart[%d]:set_termios\n", port->line);
 
 	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk / 16);
-	if (baud <= HW_BUF_SPD_THRESHOLD ||
+	if (baud < HW_BUF_SPD_THRESHOLD ||
 	    (pinfo->port.state && pinfo->port.state->port.tty->low_latency))
 		pinfo->rx_fifosize = 1;
 	else
 		pinfo->rx_fifosize = RX_BUF_SIZE;
 
+	/* MAXIDL is the timeout after which a receive buffer is closed
+	 * when not full if no more characters are received.
+	 * We calculate it from the baudrate so that the duration is
+	 * always the same at standard rates: about 4ms.
+	 */
+	maxidl = baud / 2400;
+	if (maxidl < 1)
+		maxidl = 1;
+	if (maxidl > 0x10)
+		maxidl = 0x10;
+
 	/* Character length programmed into the mode register is the
 	 * sum of: 1 start bit, number of data bits, 0 or 1 parity bit,
 	 * 1 or 2 stop bits, minus 1.
@@ -610,6 +623,7 @@ static void cpm_uart_set_termios(struct uart_port *port,
 		 * SMC/SCC receiver is disabled.
 		 */
 		out_be16(&pinfo->smcup->smc_mrblr, pinfo->rx_fifosize);
+		out_be16(&pinfo->smcup->smc_maxidl, maxidl);
 
 		/* Set the mode register.  We want to keep a copy of the
 		 * enables, because we want to put them back if they were
@@ -622,6 +636,7 @@ static void cpm_uart_set_termios(struct uart_port *port,
 		    SMCMR_SM_UART | prev_mode);
 	} else {
 		out_be16(&pinfo->sccup->scc_genscc.scc_mrblr, pinfo->rx_fifosize);
+		out_be16(&pinfo->sccup->scc_maxidl, maxidl);
 		out_be16(&sccp->scc_psmr, (sbits << 12) | scval);
 	}
 
@@ -798,7 +813,7 @@ static void cpm_uart_init_scc(struct uart_cpm_port *pinfo)
 	cpm_set_scc_fcr(sup);
 
 	out_be16(&sup->scc_genscc.scc_mrblr, pinfo->rx_fifosize);
-	out_be16(&sup->scc_maxidl, pinfo->rx_fifosize);
+	out_be16(&sup->scc_maxidl, 0x10);
 	out_be16(&sup->scc_brkcr, 1);
 	out_be16(&sup->scc_parec, 0);
 	out_be16(&sup->scc_frmec, 0);
@@ -872,7 +887,7 @@ static void cpm_uart_init_smc(struct uart_cpm_port *pinfo)
 
 	/* Using idle character time requires some additional tuning.  */
 	out_be16(&up->smc_mrblr, pinfo->rx_fifosize);
-	out_be16(&up->smc_maxidl, pinfo->rx_fifosize);
+	out_be16(&up->smc_maxidl, 0x10);
 	out_be16(&up->smc_brklen, 0);
 	out_be16(&up->smc_brkec, 0);
 	out_be16(&up->smc_brkcr, 1);
diff --git a/drivers/tty/serial/crisv10.c b/drivers/tty/serial/crisv10.c
index 80b6b1b1f725..35ee6a2c6877 100644
--- a/drivers/tty/serial/crisv10.c
+++ b/drivers/tty/serial/crisv10.c
@@ -955,7 +955,7 @@ static const struct control_pins e100_modem_pins[NR_PORTS] =
 /* Calculate the chartime depending on baudrate, numbor of bits etc. */
 static void update_char_time(struct e100_serial * info)
 {
-	tcflag_t cflags = info->port.tty->termios->c_cflag;
+	tcflag_t cflags = info->port.tty->termios.c_cflag;
 	int bits;
 
 	/* calc. number of bits / data byte */
@@ -1473,7 +1473,7 @@ rs_stop(struct tty_struct *tty)
 		xoff = IO_FIELD(R_SERIAL0_XOFF, xoff_char,
 				STOP_CHAR(info->port.tty));
 		xoff |= IO_STATE(R_SERIAL0_XOFF, tx_stop, stop);
-		if (tty->termios->c_iflag & IXON ) {
+		if (tty->termios.c_iflag & IXON ) {
 			xoff |= IO_STATE(R_SERIAL0_XOFF, auto_xoff, enable);
 		}
 
@@ -1496,7 +1496,7 @@ rs_start(struct tty_struct *tty)
 					 info->xmit.tail,SERIAL_XMIT_SIZE)));
 		xoff = IO_FIELD(R_SERIAL0_XOFF, xoff_char, STOP_CHAR(tty));
 		xoff |= IO_STATE(R_SERIAL0_XOFF, tx_stop, enable);
-		if (tty->termios->c_iflag & IXON ) {
+		if (tty->termios.c_iflag & IXON ) {
 			xoff |= IO_STATE(R_SERIAL0_XOFF, auto_xoff, enable);
 		}
 
@@ -2929,7 +2929,7 @@ shutdown(struct e100_serial * info)
 			descr[i].buf = 0;
 		}
 
-	if (!info->port.tty || (info->port.tty->termios->c_cflag & HUPCL)) {
+	if (!info->port.tty || (info->port.tty->termios.c_cflag & HUPCL)) {
 		/* hang up DTR and RTS if HUPCL is enabled */
 		e100_dtr(info, 0);
 		e100_rts(info, 0); /* could check CRTSCTS before doing this */
@@ -2953,12 +2953,12 @@ change_speed(struct e100_serial *info)
 	unsigned long flags;
 	/* first some safety checks */
 
-	if (!info->port.tty || !info->port.tty->termios)
+	if (!info->port.tty)
 		return;
 	if (!info->ioport)
 		return;
 
-	cflag = info->port.tty->termios->c_cflag;
+	cflag = info->port.tty->termios.c_cflag;
 
 	/* possibly, the tx/rx should be disabled first to do this safely */
 
@@ -3088,7 +3088,7 @@ change_speed(struct e100_serial *info)
 	info->ioport[REG_REC_CTRL] = info->rx_ctrl;
 	xoff = IO_FIELD(R_SERIAL0_XOFF, xoff_char, STOP_CHAR(info->port.tty));
 	xoff |= IO_STATE(R_SERIAL0_XOFF, tx_stop, enable);
-	if (info->port.tty->termios->c_iflag & IXON ) {
+	if (info->port.tty->termios.c_iflag & IXON ) {
 		DFLOW(DEBUG_LOG(info->line, "FLOW XOFF enabled 0x%02X\n",
 				STOP_CHAR(info->port.tty)));
 		xoff |= IO_STATE(R_SERIAL0_XOFF, auto_xoff, enable);
@@ -3355,7 +3355,7 @@ rs_throttle(struct tty_struct * tty)
 	DFLOW(DEBUG_LOG(info->line,"rs_throttle %lu\n", tty->ldisc.chars_in_buffer(tty)));
 
 	/* Do RTS before XOFF since XOFF might take some time */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		/* Turn off RTS line */
 		e100_rts(info, 0);
 	}
@@ -3377,7 +3377,7 @@ rs_unthrottle(struct tty_struct * tty)
 	DFLOW(DEBUG_LOG(info->line,"rs_unthrottle ldisc %d\n", tty->ldisc.chars_in_buffer(tty)));
 	DFLOW(DEBUG_LOG(info->line,"rs_unthrottle flip.count: %i\n", tty->flip.count));
 	/* Do RTS before XOFF since XOFF might take some time */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		/* Assert RTS line  */
 		e100_rts(info, 1);
 	}
@@ -3748,7 +3748,7 @@ rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle turning off CRTSCTS */
 	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		rs_start(tty);
 	}
@@ -3815,7 +3815,7 @@ rs_close(struct tty_struct *tty, struct file * filp)
 	 * separate termios for callout and dialin.
 	 */
 	if (info->flags & ASYNC_NORMAL_ACTIVE)
-		info->normal_termios = *tty->termios;
+		info->normal_termios = tty->termios;
 	/*
 	 * Now we wait for the transmit buffer to clear; and we notify
 	 * the line discipline to only process XON/XOFF characters.
@@ -3976,7 +3976,7 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 	 */
 	if (tty_hung_up_p(filp) ||
 	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->close_wait,
+		wait_event_interruptible_tty(tty, info->close_wait,
 			!(info->flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
 		if (info->flags & ASYNC_HUP_NOTIFY)
@@ -3998,7 +3998,7 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 		return 0;
 	}
 
-	if (tty->termios->c_cflag & CLOCAL) {
+	if (tty->termios.c_cflag & CLOCAL) {
 			do_clocal = 1;
 	}
 
@@ -4052,9 +4052,9 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 		printk("block_til_ready blocking: ttyS%d, count = %d\n",
 		       info->line, info->count);
 #endif
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	set_current_state(TASK_RUNNING);
 	remove_wait_queue(&info->open_wait, &wait);
@@ -4115,7 +4115,7 @@ rs_open(struct tty_struct *tty, struct file * filp)
 	 */
 	if (tty_hung_up_p(filp) ||
 	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->close_wait,
+		wait_event_interruptible_tty(tty, info->close_wait,
 			!(info->flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
 		return ((info->flags & ASYNC_HUP_NOTIFY) ?
@@ -4219,7 +4219,7 @@ rs_open(struct tty_struct *tty, struct file * filp)
 	}
 
 	if ((info->count == 1) && (info->flags & ASYNC_SPLIT_TERMIOS)) {
-		*tty->termios = info->normal_termios;
+		tty->termios = info->normal_termios;
 		change_speed(info);
 	}
 
@@ -4443,14 +4443,12 @@ static int __init rs_init(void)
 		B115200 | CS8 | CREAD | HUPCL | CLOCAL; /* is normally B9600 default... */
 	driver->init_termios.c_ispeed = 115200;
 	driver->init_termios.c_ospeed = 115200;
-	driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	driver->flags = TTY_DRIVER_REAL_RAW;
 
 	tty_set_operations(driver, &rs_ops);
         serial_driver = driver;
-	if (tty_register_driver(driver))
-		panic("Couldn't register serial driver\n");
-	/* do some initializing for the separate ports */
 
+	/* do some initializing for the separate ports */
 	for (i = 0, info = rs_table; i < NR_PORTS; i++,info++) {
 		if (info->enabled) {
 			if (cris_request_io_interface(info->io_if,
@@ -4502,7 +4500,12 @@ static int __init rs_init(void)
 			printk(KERN_INFO "%s%d at %p is a builtin UART with DMA\n",
 			       serial_driver->name, info->line, info->ioport);
 		}
+		tty_port_link_device(&info->port, driver, i);
 	}
+
+	if (tty_register_driver(driver))
+		panic("Couldn't register serial driver\n");
+
 #ifdef CONFIG_ETRAX_FAST_TIMER
 #ifdef CONFIG_ETRAX_SERIAL_FAST_TIMER
 	memset(fast_timers, 0, sizeof(fast_timers));
diff --git a/drivers/tty/serial/ifx6x60.c b/drivers/tty/serial/ifx6x60.c
index 3ad079ffd049..5b9bc19ed134 100644
--- a/drivers/tty/serial/ifx6x60.c
+++ b/drivers/tty/serial/ifx6x60.c
@@ -800,8 +800,8 @@ static int ifx_spi_create_port(struct ifx_spi_device *ifx_dev)
 	tty_port_init(pport);
 	pport->ops = &ifx_tty_port_ops;
 	ifx_dev->minor = IFX_SPI_TTY_ID;
-	ifx_dev->tty_dev = tty_register_device(tty_drv, ifx_dev->minor,
-					       &ifx_dev->spi_dev->dev);
+	ifx_dev->tty_dev = tty_port_register_device(pport, tty_drv,
+			ifx_dev->minor, &ifx_dev->spi_dev->dev);
 	if (IS_ERR(ifx_dev->tty_dev)) {
 		dev_dbg(&ifx_dev->spi_dev->dev,
 			"%s: registering tty device failed", __func__);
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index e309e8b0aaba..efeb8becfa43 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -207,7 +207,7 @@ struct imx_port {
 	unsigned short		trcv_delay; /* transceiver delay */
 	struct clk		*clk_ipg;
 	struct clk		*clk_per;
-	struct imx_uart_data	*devdata;
+	const struct imx_uart_data *devdata;
 };
 
 struct imx_port_ucrs {
@@ -1373,8 +1373,7 @@ static int serial_imx_suspend(struct platform_device *dev, pm_message_t state)
 	val |= UCR3_AWAKEN;
 	writel(val, sport->port.membase + UCR3);
 
-	if (sport)
-		uart_suspend_port(&imx_reg, &sport->port);
+	uart_suspend_port(&imx_reg, &sport->port);
 
 	return 0;
 }
@@ -1389,8 +1388,7 @@ static int serial_imx_resume(struct platform_device *dev)
 	val &= ~UCR3_AWAKEN;
 	writel(val, sport->port.membase + UCR3);
 
-	if (sport)
-		uart_resume_port(&imx_reg, &sport->port);
+	uart_resume_port(&imx_reg, &sport->port);
 
 	return 0;
 }
@@ -1505,18 +1503,21 @@ static int serial_imx_probe(struct platform_device *pdev)
 	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
 	if (IS_ERR(pinctrl)) {
 		ret = PTR_ERR(pinctrl);
+		dev_err(&pdev->dev, "failed to get default pinctrl: %d\n", ret);
 		goto unmap;
 	}
 
 	sport->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
 	if (IS_ERR(sport->clk_ipg)) {
 		ret = PTR_ERR(sport->clk_ipg);
+		dev_err(&pdev->dev, "failed to get ipg clk: %d\n", ret);
 		goto unmap;
 	}
 
 	sport->clk_per = devm_clk_get(&pdev->dev, "per");
 	if (IS_ERR(sport->clk_per)) {
 		ret = PTR_ERR(sport->clk_per);
+		dev_err(&pdev->dev, "failed to get per clk: %d\n", ret);
 		goto unmap;
 	}
 
@@ -1537,7 +1538,7 @@ static int serial_imx_probe(struct platform_device *pdev)
 	ret = uart_add_one_port(&imx_reg, &sport->port);
 	if (ret)
 		goto deinit;
-	platform_set_drvdata(pdev, &sport->port);
+	platform_set_drvdata(pdev, sport);
 
 	return 0;
 deinit:
diff --git a/drivers/tty/serial/ioc3_serial.c b/drivers/tty/serial/ioc3_serial.c
index 758ff310f7f8..5ac52898a0bb 100644
--- a/drivers/tty/serial/ioc3_serial.c
+++ b/drivers/tty/serial/ioc3_serial.c
@@ -1120,13 +1120,14 @@ static inline int do_read(struct uart_port *the_port, char *buf, int len)
 	struct ioc3_port *port = get_ioc3_port(the_port);
 	struct ring *inring;
 	struct ring_entry *entry;
-	struct port_hooks *hooks = port->ip_hooks;
+	struct port_hooks *hooks;
 	int byte_num;
 	char *sc;
 	int loop_counter;
 
 	BUG_ON(!(len >= 0));
 	BUG_ON(!port);
+	hooks = port->ip_hooks;
 
 	/* There is a nasty timing issue in the IOC3. When the rx_timer
 	 * expires or the rx_high condition arises, we take an interrupt.
diff --git a/drivers/tty/serial/ioc4_serial.c b/drivers/tty/serial/ioc4_serial.c
index e16894fb2ca3..3e7da10cebba 100644
--- a/drivers/tty/serial/ioc4_serial.c
+++ b/drivers/tty/serial/ioc4_serial.c
@@ -1803,7 +1803,7 @@ static inline int ic4_startup_local(struct uart_port *the_port)
 	ioc4_set_proto(port, the_port->mapbase);
 
 	/* set the speed of the serial port */
-	ioc4_change_speed(the_port, state->port.tty->termios,
+	ioc4_change_speed(the_port, &state->port.tty->termios,
 			  (struct ktermios *)0);
 
 	return 0;
@@ -2069,13 +2069,14 @@ static inline int do_read(struct uart_port *the_port, unsigned char *buf,
 	struct ioc4_port *port = get_ioc4_port(the_port, 0);
 	struct ring *inring;
 	struct ring_entry *entry;
-	struct hooks *hooks = port->ip_hooks;
+	struct hooks *hooks;
 	int byte_num;
 	char *sc;
 	int loop_counter;
 
 	BUG_ON(!(len >= 0));
 	BUG_ON(!port);
+	hooks = port->ip_hooks;
 
 	/* There is a nasty timing issue in the IOC4. When the rx_timer
 	 * expires or the rx_high condition arises, we take an interrupt.
diff --git a/drivers/tty/serial/jsm/jsm_driver.c b/drivers/tty/serial/jsm/jsm_driver.c
index 7545fe1b9925..5ab3c3b595e4 100644
--- a/drivers/tty/serial/jsm/jsm_driver.c
+++ b/drivers/tty/serial/jsm/jsm_driver.c
@@ -54,7 +54,7 @@ static pci_ers_result_t jsm_io_error_detected(struct pci_dev *pdev,
 static pci_ers_result_t jsm_io_slot_reset(struct pci_dev *pdev);
 static void jsm_io_resume(struct pci_dev *pdev);
 
-static struct pci_error_handlers jsm_err_handler = {
+static const struct pci_error_handlers jsm_err_handler = {
 	.error_detected = jsm_io_error_detected,
 	.slot_reset = jsm_io_slot_reset,
 	.resume = jsm_io_resume,
diff --git a/drivers/tty/serial/jsm/jsm_tty.c b/drivers/tty/serial/jsm/jsm_tty.c
index 434bd881fcae..71397961773c 100644
--- a/drivers/tty/serial/jsm/jsm_tty.c
+++ b/drivers/tty/serial/jsm/jsm_tty.c
@@ -161,7 +161,7 @@ static void jsm_tty_send_xchar(struct uart_port *port, char ch)
 	struct ktermios *termios;
 
 	spin_lock_irqsave(&port->lock, lock_flags);
-	termios = port->state->port.tty->termios;
+	termios = &port->state->port.tty->termios;
 	if (ch == termios->c_cc[VSTART])
 		channel->ch_bd->bd_ops->send_start_character(channel);
 
@@ -250,7 +250,7 @@ static int jsm_tty_open(struct uart_port *port)
 	channel->ch_cached_lsr = 0;
 	channel->ch_stops_sent = 0;
 
-	termios = port->state->port.tty->termios;
+	termios = &port->state->port.tty->termios;
 	channel->ch_c_cflag	= termios->c_cflag;
 	channel->ch_c_iflag	= termios->c_iflag;
 	channel->ch_c_oflag	= termios->c_oflag;
@@ -283,7 +283,7 @@ static void jsm_tty_close(struct uart_port *port)
 	jsm_printk(CLOSE, INFO, &channel->ch_bd->pci_dev, "start\n");
 
 	bd = channel->ch_bd;
-	ts = port->state->port.tty->termios;
+	ts = &port->state->port.tty->termios;
 
 	channel->ch_flags &= ~(CH_STOPI);
 
@@ -567,7 +567,7 @@ void jsm_input(struct jsm_channel *ch)
 	 *input data and return immediately.
 	 */
 	if (!tp ||
-		!(tp->termios->c_cflag & CREAD) ) {
+		!(tp->termios.c_cflag & CREAD) ) {
 
 		jsm_printk(READ, INFO, &ch->ch_bd->pci_dev,
 			"input. dropping %d bytes on port %d...\n", data_len, ch->ch_portnum);
diff --git a/drivers/tty/serial/kgdb_nmi.c b/drivers/tty/serial/kgdb_nmi.c
new file mode 100644
index 000000000000..d185247ba1aa
--- /dev/null
+++ b/drivers/tty/serial/kgdb_nmi.c
@@ -0,0 +1,402 @@
+/*
+ * KGDB NMI serial console
+ *
+ * Copyright 2010 Google, Inc.
+ *		  Arve Hjønnevåg <arve@android.com>
+ *		  Colin Cross <ccross@android.com>
+ * Copyright 2012 Linaro Ltd.
+ *		  Anton Vorontsov <anton.vorontsov@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/compiler.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/atomic.h>
+#include <linux/console.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/tick.h>
+#include <linux/kfifo.h>
+#include <linux/kgdb.h>
+#include <linux/kdb.h>
+
+static int kgdb_nmi_knock = 1;
+module_param_named(knock, kgdb_nmi_knock, int, 0600);
+MODULE_PARM_DESC(knock, "if set to 1 (default), the special '$3#33' command " \
+			"must be used to enter the debugger; when set to 0, " \
+			"hitting return key is enough to enter the debugger; " \
+			"when set to -1, the debugger is entered immediately " \
+			"upon NMI");
+
+static char *kgdb_nmi_magic = "$3#33";
+module_param_named(magic, kgdb_nmi_magic, charp, 0600);
+MODULE_PARM_DESC(magic, "magic sequence to enter NMI debugger (default $3#33)");
+
+static bool kgdb_nmi_tty_enabled;
+
+static void kgdb_nmi_console_write(struct console *co, const char *s, uint c)
+{
+	int i;
+
+	if (!kgdb_nmi_tty_enabled || atomic_read(&kgdb_active) >= 0)
+		return;
+
+	for (i = 0; i < c; i++)
+		dbg_io_ops->write_char(s[i]);
+}
+
+static struct tty_driver *kgdb_nmi_tty_driver;
+
+static struct tty_driver *kgdb_nmi_console_device(struct console *co, int *idx)
+{
+	*idx = co->index;
+	return kgdb_nmi_tty_driver;
+}
+
+static struct console kgdb_nmi_console = {
+	.name	= "ttyNMI",
+	.write	= kgdb_nmi_console_write,
+	.device	= kgdb_nmi_console_device,
+	.flags	= CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
+	.index	= -1,
+};
+
+/*
+ * This is usually the maximum rate on debug ports. We make fifo large enough
+ * to make copy-pasting to the terminal usable.
+ */
+#define KGDB_NMI_BAUD		115200
+#define KGDB_NMI_FIFO_SIZE	roundup_pow_of_two(KGDB_NMI_BAUD / 8 / HZ)
+
+struct kgdb_nmi_tty_priv {
+	struct tty_port port;
+	struct tasklet_struct tlet;
+	STRUCT_KFIFO(char, KGDB_NMI_FIFO_SIZE) fifo;
+};
+
+static struct kgdb_nmi_tty_priv *kgdb_nmi_port_to_priv(struct tty_port *port)
+{
+	return container_of(port, struct kgdb_nmi_tty_priv, port);
+}
+
+/*
+ * Our debugging console is polled in a tasklet, so we'll check for input
+ * every tick. In HZ-less mode, we should program the next tick.  We have
+ * to use the lowlevel stuff as no locks should be grabbed.
+ */
+#ifdef CONFIG_HIGH_RES_TIMERS
+static void kgdb_tty_poke(void)
+{
+	tick_program_event(ktime_get(), 0);
+}
+#else
+static inline void kgdb_tty_poke(void) {}
+#endif
+
+static struct tty_port *kgdb_nmi_port;
+
+static void kgdb_tty_recv(int ch)
+{
+	struct kgdb_nmi_tty_priv *priv;
+	char c = ch;
+
+	if (!kgdb_nmi_port || ch < 0)
+		return;
+	/*
+	 * Can't use port->tty->driver_data as tty might be not there. Tasklet
+	 * will check for tty and will get the ref, but here we don't have to
+	 * do that, and actually, we can't: we're in NMI context, no locks are
+	 * possible.
+	 */
+	priv = kgdb_nmi_port_to_priv(kgdb_nmi_port);
+	kfifo_in(&priv->fifo, &c, 1);
+	kgdb_tty_poke();
+}
+
+static int kgdb_nmi_poll_one_knock(void)
+{
+	static int n;
+	int c = -1;
+	const char *magic = kgdb_nmi_magic;
+	size_t m = strlen(magic);
+	bool printch = 0;
+
+	c = dbg_io_ops->read_char();
+	if (c == NO_POLL_CHAR)
+		return c;
+
+	if (!kgdb_nmi_knock && (c == '\r' || c == '\n')) {
+		return 1;
+	} else if (c == magic[n]) {
+		n = (n + 1) % m;
+		if (!n)
+			return 1;
+		printch = 1;
+	} else {
+		n = 0;
+	}
+
+	if (kgdb_nmi_tty_enabled) {
+		kgdb_tty_recv(c);
+		return 0;
+	}
+
+	if (printch) {
+		kdb_printf("%c", c);
+		return 0;
+	}
+
+	kdb_printf("\r%s %s to enter the debugger> %*s",
+		   kgdb_nmi_knock ? "Type" : "Hit",
+		   kgdb_nmi_knock ? magic  : "<return>", (int)m, "");
+	while (m--)
+		kdb_printf("\b");
+	return 0;
+}
+
+/**
+ * kgdb_nmi_poll_knock - Check if it is time to enter the debugger
+ *
+ * "Serial ports are often noisy, especially when muxed over another port (we
+ * often use serial over the headset connector). Noise on the async command
+ * line just causes characters that are ignored, on a command line that blocked
+ * execution noise would be catastrophic." -- Colin Cross
+ *
+ * So, this function implements KGDB/KDB knocking on the serial line: we won't
+ * enter the debugger until we receive a known magic phrase (which is actually
+ * "$3#33", known as "escape to KDB" command. There is also a relaxed variant
+ * of knocking, i.e. just pressing the return key is enough to enter the
+ * debugger. And if knocking is disabled, the function always returns 1.
+ */
+bool kgdb_nmi_poll_knock(void)
+{
+	if (kgdb_nmi_knock < 0)
+		return 1;
+
+	while (1) {
+		int ret;
+
+		ret = kgdb_nmi_poll_one_knock();
+		if (ret == NO_POLL_CHAR)
+			return 0;
+		else if (ret == 1)
+			break;
+	}
+	return 1;
+}
+
+/*
+ * The tasklet is cheap, it does not cause wakeups when reschedules itself,
+ * instead it waits for the next tick.
+ */
+static void kgdb_nmi_tty_receiver(unsigned long data)
+{
+	struct kgdb_nmi_tty_priv *priv = (void *)data;
+	struct tty_struct *tty;
+	char ch;
+
+	tasklet_schedule(&priv->tlet);
+
+	if (likely(!kgdb_nmi_tty_enabled || !kfifo_len(&priv->fifo)))
+		return;
+
+	/* Port is there, but tty might be hung up, check. */
+	tty = tty_port_tty_get(kgdb_nmi_port);
+	if (!tty)
+		return;
+
+	while (kfifo_out(&priv->fifo, &ch, 1))
+		tty_insert_flip_char(priv->port.tty, ch, TTY_NORMAL);
+	tty_flip_buffer_push(priv->port.tty);
+
+	tty_kref_put(tty);
+}
+
+static int kgdb_nmi_tty_activate(struct tty_port *port, struct tty_struct *tty)
+{
+	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+
+	kgdb_nmi_port = port;
+	tasklet_schedule(&priv->tlet);
+	return 0;
+}
+
+static void kgdb_nmi_tty_shutdown(struct tty_port *port)
+{
+	struct kgdb_nmi_tty_priv *priv = port->tty->driver_data;
+
+	tasklet_kill(&priv->tlet);
+	kgdb_nmi_port = NULL;
+}
+
+static const struct tty_port_operations kgdb_nmi_tty_port_ops = {
+	.activate	= kgdb_nmi_tty_activate,
+	.shutdown	= kgdb_nmi_tty_shutdown,
+};
+
+static int kgdb_nmi_tty_install(struct tty_driver *drv, struct tty_struct *tty)
+{
+	struct kgdb_nmi_tty_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	INIT_KFIFO(priv->fifo);
+	tasklet_init(&priv->tlet, kgdb_nmi_tty_receiver, (unsigned long)priv);
+	tty_port_init(&priv->port);
+	priv->port.ops = &kgdb_nmi_tty_port_ops;
+	tty->driver_data = priv;
+
+	ret = tty_port_install(&priv->port, drv, tty);
+	if (ret) {
+		pr_err("%s: can't install tty port: %d\n", __func__, ret);
+		goto err;
+	}
+	return 0;
+err:
+	kfree(priv);
+	return ret;
+}
+
+static void kgdb_nmi_tty_cleanup(struct tty_struct *tty)
+{
+	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+
+	tty->driver_data = NULL;
+	kfree(priv);
+}
+
+static int kgdb_nmi_tty_open(struct tty_struct *tty, struct file *file)
+{
+	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+
+	return tty_port_open(&priv->port, tty, file);
+}
+
+static void kgdb_nmi_tty_close(struct tty_struct *tty, struct file *file)
+{
+	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+
+	tty_port_close(&priv->port, tty, file);
+}
+
+static void kgdb_nmi_tty_hangup(struct tty_struct *tty)
+{
+	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+
+	tty_port_hangup(&priv->port);
+}
+
+static int kgdb_nmi_tty_write_room(struct tty_struct *tty)
+{
+	/* Actually, we can handle any amount as we use polled writes. */
+	return 2048;
+}
+
+static int kgdb_nmi_tty_write(struct tty_struct *tty, const unchar *buf, int c)
+{
+	int i;
+
+	for (i = 0; i < c; i++)
+		dbg_io_ops->write_char(buf[i]);
+	return c;
+}
+
+static const struct tty_operations kgdb_nmi_tty_ops = {
+	.open		= kgdb_nmi_tty_open,
+	.close		= kgdb_nmi_tty_close,
+	.install	= kgdb_nmi_tty_install,
+	.cleanup	= kgdb_nmi_tty_cleanup,
+	.hangup		= kgdb_nmi_tty_hangup,
+	.write_room	= kgdb_nmi_tty_write_room,
+	.write		= kgdb_nmi_tty_write,
+};
+
+static int kgdb_nmi_enable_console(int argc, const char *argv[])
+{
+	kgdb_nmi_tty_enabled = !(argc == 1 && !strcmp(argv[1], "off"));
+	return 0;
+}
+
+int kgdb_register_nmi_console(void)
+{
+	int ret;
+
+	if (!arch_kgdb_ops.enable_nmi)
+		return 0;
+
+	kgdb_nmi_tty_driver = alloc_tty_driver(1);
+	if (!kgdb_nmi_tty_driver) {
+		pr_err("%s: cannot allocate tty\n", __func__);
+		return -ENOMEM;
+	}
+	kgdb_nmi_tty_driver->driver_name	= "ttyNMI";
+	kgdb_nmi_tty_driver->name		= "ttyNMI";
+	kgdb_nmi_tty_driver->num		= 1;
+	kgdb_nmi_tty_driver->type		= TTY_DRIVER_TYPE_SERIAL;
+	kgdb_nmi_tty_driver->subtype		= SERIAL_TYPE_NORMAL;
+	kgdb_nmi_tty_driver->flags		= TTY_DRIVER_REAL_RAW;
+	kgdb_nmi_tty_driver->init_termios	= tty_std_termios;
+	tty_termios_encode_baud_rate(&kgdb_nmi_tty_driver->init_termios,
+				     KGDB_NMI_BAUD, KGDB_NMI_BAUD);
+	tty_set_operations(kgdb_nmi_tty_driver, &kgdb_nmi_tty_ops);
+
+	ret = tty_register_driver(kgdb_nmi_tty_driver);
+	if (ret) {
+		pr_err("%s: can't register tty driver: %d\n", __func__, ret);
+		goto err_drv_reg;
+	}
+
+	ret = kdb_register("nmi_console", kgdb_nmi_enable_console, "[off]",
+			   "switch to Linux NMI console", 0);
+	if (ret) {
+		pr_err("%s: can't register kdb command: %d\n", __func__, ret);
+		goto err_kdb_reg;
+	}
+
+	register_console(&kgdb_nmi_console);
+	arch_kgdb_ops.enable_nmi(1);
+
+	return 0;
+err_kdb_reg:
+	tty_unregister_driver(kgdb_nmi_tty_driver);
+err_drv_reg:
+	put_tty_driver(kgdb_nmi_tty_driver);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(kgdb_register_nmi_console);
+
+int kgdb_unregister_nmi_console(void)
+{
+	int ret;
+
+	if (!arch_kgdb_ops.enable_nmi)
+		return 0;
+	arch_kgdb_ops.enable_nmi(0);
+
+	kdb_unregister("nmi_console");
+
+	ret = unregister_console(&kgdb_nmi_console);
+	if (ret)
+		return ret;
+
+	ret = tty_unregister_driver(kgdb_nmi_tty_driver);
+	if (ret)
+		return ret;
+	put_tty_driver(kgdb_nmi_tty_driver);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(kgdb_unregister_nmi_console);
diff --git a/drivers/tty/serial/kgdboc.c b/drivers/tty/serial/kgdboc.c
index 2b42a01a81c6..3f63d834cbc9 100644
--- a/drivers/tty/serial/kgdboc.c
+++ b/drivers/tty/serial/kgdboc.c
@@ -145,6 +145,8 @@ __setup("kgdboc=", kgdboc_option_setup);
 
 static void cleanup_kgdboc(void)
 {
+	if (kgdb_unregister_nmi_console())
+		return;
 	kgdboc_unregister_kbd();
 	if (configured == 1)
 		kgdb_unregister_io_module(&kgdboc_io_ops);
@@ -198,11 +200,18 @@ do_register:
 	if (err)
 		goto noconfig;
 
+	err = kgdb_register_nmi_console();
+	if (err)
+		goto nmi_con_failed;
+
 	configured = 1;
 
 	return 0;
 
+nmi_con_failed:
+	kgdb_unregister_io_module(&kgdboc_io_ops);
 noconfig:
+	kgdboc_unregister_kbd();
 	config[0] = 0;
 	configured = 0;
 	cleanup_kgdboc();
diff --git a/drivers/tty/serial/lpc32xx_hs.c b/drivers/tty/serial/lpc32xx_hs.c
new file mode 100644
index 000000000000..ba3af3bf6d43
--- /dev/null
+++ b/drivers/tty/serial/lpc32xx_hs.c
@@ -0,0 +1,823 @@
+/*
+ * High Speed Serial Ports on NXP LPC32xx SoC
+ *
+ * Authors: Kevin Wells <kevin.wells@nxp.com>
+ *          Roland Stigge <stigge@antcom.de>
+ *
+ * Copyright (C) 2010 NXP Semiconductors
+ * Copyright (C) 2012 Roland Stigge
+ *
+ * This program is free software; you can redistribute 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/ioport.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/sysrq.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/nmi.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <mach/platform.h>
+#include <mach/hardware.h>
+
+/*
+ * High Speed UART register offsets
+ */
+#define LPC32XX_HSUART_FIFO(x)			((x) + 0x00)
+#define LPC32XX_HSUART_LEVEL(x)			((x) + 0x04)
+#define LPC32XX_HSUART_IIR(x)			((x) + 0x08)
+#define LPC32XX_HSUART_CTRL(x)			((x) + 0x0C)
+#define LPC32XX_HSUART_RATE(x)			((x) + 0x10)
+
+#define LPC32XX_HSU_BREAK_DATA			(1 << 10)
+#define LPC32XX_HSU_ERROR_DATA			(1 << 9)
+#define LPC32XX_HSU_RX_EMPTY			(1 << 8)
+
+#define LPC32XX_HSU_TX_LEV(n)			(((n) >> 8) & 0xFF)
+#define LPC32XX_HSU_RX_LEV(n)			((n) & 0xFF)
+
+#define LPC32XX_HSU_TX_INT_SET			(1 << 6)
+#define LPC32XX_HSU_RX_OE_INT			(1 << 5)
+#define LPC32XX_HSU_BRK_INT			(1 << 4)
+#define LPC32XX_HSU_FE_INT			(1 << 3)
+#define LPC32XX_HSU_RX_TIMEOUT_INT		(1 << 2)
+#define LPC32XX_HSU_RX_TRIG_INT			(1 << 1)
+#define LPC32XX_HSU_TX_INT			(1 << 0)
+
+#define LPC32XX_HSU_HRTS_INV			(1 << 21)
+#define LPC32XX_HSU_HRTS_TRIG_8B		(0x0 << 19)
+#define LPC32XX_HSU_HRTS_TRIG_16B		(0x1 << 19)
+#define LPC32XX_HSU_HRTS_TRIG_32B		(0x2 << 19)
+#define LPC32XX_HSU_HRTS_TRIG_48B		(0x3 << 19)
+#define LPC32XX_HSU_HRTS_EN			(1 << 18)
+#define LPC32XX_HSU_TMO_DISABLED		(0x0 << 16)
+#define LPC32XX_HSU_TMO_INACT_4B		(0x1 << 16)
+#define LPC32XX_HSU_TMO_INACT_8B		(0x2 << 16)
+#define LPC32XX_HSU_TMO_INACT_16B		(0x3 << 16)
+#define LPC32XX_HSU_HCTS_INV			(1 << 15)
+#define LPC32XX_HSU_HCTS_EN			(1 << 14)
+#define LPC32XX_HSU_OFFSET(n)			((n) << 9)
+#define LPC32XX_HSU_BREAK			(1 << 8)
+#define LPC32XX_HSU_ERR_INT_EN			(1 << 7)
+#define LPC32XX_HSU_RX_INT_EN			(1 << 6)
+#define LPC32XX_HSU_TX_INT_EN			(1 << 5)
+#define LPC32XX_HSU_RX_TL1B			(0x0 << 2)
+#define LPC32XX_HSU_RX_TL4B			(0x1 << 2)
+#define LPC32XX_HSU_RX_TL8B			(0x2 << 2)
+#define LPC32XX_HSU_RX_TL16B			(0x3 << 2)
+#define LPC32XX_HSU_RX_TL32B			(0x4 << 2)
+#define LPC32XX_HSU_RX_TL48B			(0x5 << 2)
+#define LPC32XX_HSU_TX_TLEMPTY			(0x0 << 0)
+#define LPC32XX_HSU_TX_TL0B			(0x0 << 0)
+#define LPC32XX_HSU_TX_TL4B			(0x1 << 0)
+#define LPC32XX_HSU_TX_TL8B			(0x2 << 0)
+#define LPC32XX_HSU_TX_TL16B			(0x3 << 0)
+
+#define MODNAME "lpc32xx_hsuart"
+
+struct lpc32xx_hsuart_port {
+	struct uart_port port;
+};
+
+#define FIFO_READ_LIMIT 128
+#define MAX_PORTS 3
+#define LPC32XX_TTY_NAME "ttyTX"
+static struct lpc32xx_hsuart_port lpc32xx_hs_ports[MAX_PORTS];
+
+#ifdef CONFIG_SERIAL_HS_LPC32XX_CONSOLE
+static void wait_for_xmit_empty(struct uart_port *port)
+{
+	unsigned int timeout = 10000;
+
+	do {
+		if (LPC32XX_HSU_TX_LEV(readl(LPC32XX_HSUART_LEVEL(
+							port->membase))) == 0)
+			break;
+		if (--timeout == 0)
+			break;
+		udelay(1);
+	} while (1);
+}
+
+static void wait_for_xmit_ready(struct uart_port *port)
+{
+	unsigned int timeout = 10000;
+
+	while (1) {
+		if (LPC32XX_HSU_TX_LEV(readl(LPC32XX_HSUART_LEVEL(
+							port->membase))) < 32)
+			break;
+		if (--timeout == 0)
+			break;
+		udelay(1);
+	}
+}
+
+static void lpc32xx_hsuart_console_putchar(struct uart_port *port, int ch)
+{
+	wait_for_xmit_ready(port);
+	writel((u32)ch, LPC32XX_HSUART_FIFO(port->membase));
+}
+
+static void lpc32xx_hsuart_console_write(struct console *co, const char *s,
+					 unsigned int count)
+{
+	struct lpc32xx_hsuart_port *up = &lpc32xx_hs_ports[co->index];
+	unsigned long flags;
+	int locked = 1;
+
+	touch_nmi_watchdog();
+	local_irq_save(flags);
+	if (up->port.sysrq)
+		locked = 0;
+	else if (oops_in_progress)
+		locked = spin_trylock(&up->port.lock);
+	else
+		spin_lock(&up->port.lock);
+
+	uart_console_write(&up->port, s, count, lpc32xx_hsuart_console_putchar);
+	wait_for_xmit_empty(&up->port);
+
+	if (locked)
+		spin_unlock(&up->port.lock);
+	local_irq_restore(flags);
+}
+
+static int __init lpc32xx_hsuart_console_setup(struct console *co,
+					       char *options)
+{
+	struct uart_port *port;
+	int baud = 115200;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	if (co->index >= MAX_PORTS)
+		co->index = 0;
+
+	port = &lpc32xx_hs_ports[co->index].port;
+	if (!port->membase)
+		return -ENODEV;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static struct uart_driver lpc32xx_hsuart_reg;
+static struct console lpc32xx_hsuart_console = {
+	.name		= LPC32XX_TTY_NAME,
+	.write		= lpc32xx_hsuart_console_write,
+	.device		= uart_console_device,
+	.setup		= lpc32xx_hsuart_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.data		= &lpc32xx_hsuart_reg,
+};
+
+static int __init lpc32xx_hsuart_console_init(void)
+{
+	register_console(&lpc32xx_hsuart_console);
+	return 0;
+}
+console_initcall(lpc32xx_hsuart_console_init);
+
+#define LPC32XX_HSUART_CONSOLE (&lpc32xx_hsuart_console)
+#else
+#define LPC32XX_HSUART_CONSOLE NULL
+#endif
+
+static struct uart_driver lpc32xx_hs_reg = {
+	.owner		= THIS_MODULE,
+	.driver_name	= MODNAME,
+	.dev_name	= LPC32XX_TTY_NAME,
+	.nr		= MAX_PORTS,
+	.cons		= LPC32XX_HSUART_CONSOLE,
+};
+static int uarts_registered;
+
+static unsigned int __serial_get_clock_div(unsigned long uartclk,
+					   unsigned long rate)
+{
+	u32 div, goodrate, hsu_rate, l_hsu_rate, comprate;
+	u32 rate_diff;
+
+	/* Find the closest divider to get the desired clock rate */
+	div = uartclk / rate;
+	goodrate = hsu_rate = (div / 14) - 1;
+	if (hsu_rate != 0)
+		hsu_rate--;
+
+	/* Tweak divider */
+	l_hsu_rate = hsu_rate + 3;
+	rate_diff = 0xFFFFFFFF;
+
+	while (hsu_rate < l_hsu_rate) {
+		comprate = uartclk / ((hsu_rate + 1) * 14);
+		if (abs(comprate - rate) < rate_diff) {
+			goodrate = hsu_rate;
+			rate_diff = abs(comprate - rate);
+		}
+
+		hsu_rate++;
+	}
+	if (hsu_rate > 0xFF)
+		hsu_rate = 0xFF;
+
+	return goodrate;
+}
+
+static void __serial_uart_flush(struct uart_port *port)
+{
+	u32 tmp;
+	int cnt = 0;
+
+	while ((readl(LPC32XX_HSUART_LEVEL(port->membase)) > 0) &&
+	       (cnt++ < FIFO_READ_LIMIT))
+		tmp = readl(LPC32XX_HSUART_FIFO(port->membase));
+}
+
+static void __serial_lpc32xx_rx(struct uart_port *port)
+{
+	unsigned int tmp, flag;
+	struct tty_struct *tty = tty_port_tty_get(&port->state->port);
+
+	if (!tty) {
+		/* Discard data: no tty available */
+		while (!(readl(LPC32XX_HSUART_FIFO(port->membase)) &
+			 LPC32XX_HSU_RX_EMPTY))
+			;
+
+		return;
+	}
+
+	/* Read data from FIFO and push into terminal */
+	tmp = readl(LPC32XX_HSUART_FIFO(port->membase));
+	while (!(tmp & LPC32XX_HSU_RX_EMPTY)) {
+		flag = TTY_NORMAL;
+		port->icount.rx++;
+
+		if (tmp & LPC32XX_HSU_ERROR_DATA) {
+			/* Framing error */
+			writel(LPC32XX_HSU_FE_INT,
+			       LPC32XX_HSUART_IIR(port->membase));
+			port->icount.frame++;
+			flag = TTY_FRAME;
+			tty_insert_flip_char(tty, 0, TTY_FRAME);
+		}
+
+		tty_insert_flip_char(tty, (tmp & 0xFF), flag);
+
+		tmp = readl(LPC32XX_HSUART_FIFO(port->membase));
+	}
+	tty_flip_buffer_push(tty);
+	tty_kref_put(tty);
+}
+
+static void __serial_lpc32xx_tx(struct uart_port *port)
+{
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned int tmp;
+
+	if (port->x_char) {
+		writel((u32)port->x_char, LPC32XX_HSUART_FIFO(port->membase));
+		port->icount.tx++;
+		port->x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port))
+		goto exit_tx;
+
+	/* Transfer data */
+	while (LPC32XX_HSU_TX_LEV(readl(
+		LPC32XX_HSUART_LEVEL(port->membase))) < 64) {
+		writel((u32) xmit->buf[xmit->tail],
+		       LPC32XX_HSUART_FIFO(port->membase));
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		port->icount.tx++;
+		if (uart_circ_empty(xmit))
+			break;
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+
+exit_tx:
+	if (uart_circ_empty(xmit)) {
+		tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+		tmp &= ~LPC32XX_HSU_TX_INT_EN;
+		writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+	}
+}
+
+static irqreturn_t serial_lpc32xx_interrupt(int irq, void *dev_id)
+{
+	struct uart_port *port = dev_id;
+	struct tty_struct *tty = tty_port_tty_get(&port->state->port);
+	u32 status;
+
+	spin_lock(&port->lock);
+
+	/* Read UART status and clear latched interrupts */
+	status = readl(LPC32XX_HSUART_IIR(port->membase));
+
+	if (status & LPC32XX_HSU_BRK_INT) {
+		/* Break received */
+		writel(LPC32XX_HSU_BRK_INT, LPC32XX_HSUART_IIR(port->membase));
+		port->icount.brk++;
+		uart_handle_break(port);
+	}
+
+	/* Framing error */
+	if (status & LPC32XX_HSU_FE_INT)
+		writel(LPC32XX_HSU_FE_INT, LPC32XX_HSUART_IIR(port->membase));
+
+	if (status & LPC32XX_HSU_RX_OE_INT) {
+		/* Receive FIFO overrun */
+		writel(LPC32XX_HSU_RX_OE_INT,
+		       LPC32XX_HSUART_IIR(port->membase));
+		port->icount.overrun++;
+		if (tty) {
+			tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+			tty_schedule_flip(tty);
+		}
+	}
+
+	/* Data received? */
+	if (status & (LPC32XX_HSU_RX_TIMEOUT_INT | LPC32XX_HSU_RX_TRIG_INT)) {
+		__serial_lpc32xx_rx(port);
+		if (tty)
+			tty_flip_buffer_push(tty);
+	}
+
+	/* Transmit data request? */
+	if ((status & LPC32XX_HSU_TX_INT) && (!uart_tx_stopped(port))) {
+		writel(LPC32XX_HSU_TX_INT, LPC32XX_HSUART_IIR(port->membase));
+		__serial_lpc32xx_tx(port);
+	}
+
+	spin_unlock(&port->lock);
+	tty_kref_put(tty);
+
+	return IRQ_HANDLED;
+}
+
+/* port->lock is not held.  */
+static unsigned int serial_lpc32xx_tx_empty(struct uart_port *port)
+{
+	unsigned int ret = 0;
+
+	if (LPC32XX_HSU_TX_LEV(readl(LPC32XX_HSUART_LEVEL(port->membase))) == 0)
+		ret = TIOCSER_TEMT;
+
+	return ret;
+}
+
+/* port->lock held by caller.  */
+static void serial_lpc32xx_set_mctrl(struct uart_port *port,
+				     unsigned int mctrl)
+{
+	/* No signals are supported on HS UARTs */
+}
+
+/* port->lock is held by caller and interrupts are disabled.  */
+static unsigned int serial_lpc32xx_get_mctrl(struct uart_port *port)
+{
+	/* No signals are supported on HS UARTs */
+	return TIOCM_CAR | TIOCM_DSR | TIOCM_CTS;
+}
+
+/* port->lock held by caller.  */
+static void serial_lpc32xx_stop_tx(struct uart_port *port)
+{
+	u32 tmp;
+
+	tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+	tmp &= ~LPC32XX_HSU_TX_INT_EN;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+}
+
+/* port->lock held by caller.  */
+static void serial_lpc32xx_start_tx(struct uart_port *port)
+{
+	u32 tmp;
+
+	__serial_lpc32xx_tx(port);
+	tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+	tmp |= LPC32XX_HSU_TX_INT_EN;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+}
+
+/* port->lock held by caller.  */
+static void serial_lpc32xx_stop_rx(struct uart_port *port)
+{
+	u32 tmp;
+
+	tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+	tmp &= ~(LPC32XX_HSU_RX_INT_EN | LPC32XX_HSU_ERR_INT_EN);
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+
+	writel((LPC32XX_HSU_BRK_INT | LPC32XX_HSU_RX_OE_INT |
+		LPC32XX_HSU_FE_INT), LPC32XX_HSUART_IIR(port->membase));
+}
+
+/* port->lock held by caller.  */
+static void serial_lpc32xx_enable_ms(struct uart_port *port)
+{
+	/* Modem status is not supported */
+}
+
+/* port->lock is not held.  */
+static void serial_lpc32xx_break_ctl(struct uart_port *port,
+				     int break_state)
+{
+	unsigned long flags;
+	u32 tmp;
+
+	spin_lock_irqsave(&port->lock, flags);
+	tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+	if (break_state != 0)
+		tmp |= LPC32XX_HSU_BREAK;
+	else
+		tmp &= ~LPC32XX_HSU_BREAK;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/* LPC3250 Errata HSUART.1: Hang workaround via loopback mode on inactivity */
+static void lpc32xx_loopback_set(resource_size_t mapbase, int state)
+{
+	int bit;
+	u32 tmp;
+
+	switch (mapbase) {
+	case LPC32XX_HS_UART1_BASE:
+		bit = 0;
+		break;
+	case LPC32XX_HS_UART2_BASE:
+		bit = 1;
+		break;
+	case LPC32XX_HS_UART7_BASE:
+		bit = 6;
+		break;
+	default:
+		WARN(1, "lpc32xx_hs: Warning: Unknown port at %08x\n", mapbase);
+		return;
+	}
+
+	tmp = readl(LPC32XX_UARTCTL_CLOOP);
+	if (state)
+		tmp |= (1 << bit);
+	else
+		tmp &= ~(1 << bit);
+	writel(tmp, LPC32XX_UARTCTL_CLOOP);
+}
+
+/* port->lock is not held.  */
+static int serial_lpc32xx_startup(struct uart_port *port)
+{
+	int retval;
+	unsigned long flags;
+	u32 tmp;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	__serial_uart_flush(port);
+
+	writel((LPC32XX_HSU_TX_INT | LPC32XX_HSU_FE_INT |
+		LPC32XX_HSU_BRK_INT | LPC32XX_HSU_RX_OE_INT),
+	       LPC32XX_HSUART_IIR(port->membase));
+
+	writel(0xFF, LPC32XX_HSUART_RATE(port->membase));
+
+	/*
+	 * Set receiver timeout, HSU offset of 20, no break, no interrupts,
+	 * and default FIFO trigger levels
+	 */
+	tmp = LPC32XX_HSU_TX_TL8B | LPC32XX_HSU_RX_TL32B |
+		LPC32XX_HSU_OFFSET(20) | LPC32XX_HSU_TMO_INACT_4B;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+
+	lpc32xx_loopback_set(port->mapbase, 0); /* get out of loopback mode */
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	retval = request_irq(port->irq, serial_lpc32xx_interrupt,
+			     0, MODNAME, port);
+	if (!retval)
+		writel((tmp | LPC32XX_HSU_RX_INT_EN | LPC32XX_HSU_ERR_INT_EN),
+		       LPC32XX_HSUART_CTRL(port->membase));
+
+	return retval;
+}
+
+/* port->lock is not held.  */
+static void serial_lpc32xx_shutdown(struct uart_port *port)
+{
+	u32 tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	tmp = LPC32XX_HSU_TX_TL8B | LPC32XX_HSU_RX_TL32B |
+		LPC32XX_HSU_OFFSET(20) | LPC32XX_HSU_TMO_INACT_4B;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+
+	lpc32xx_loopback_set(port->mapbase, 1); /* go to loopback mode */
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	free_irq(port->irq, port);
+}
+
+/* port->lock is not held.  */
+static void serial_lpc32xx_set_termios(struct uart_port *port,
+				       struct ktermios *termios,
+				       struct ktermios *old)
+{
+	unsigned long flags;
+	unsigned int baud, quot;
+	u32 tmp;
+
+	/* Always 8-bit, no parity, 1 stop bit */
+	termios->c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
+	termios->c_cflag |= CS8;
+
+	termios->c_cflag &= ~(HUPCL | CMSPAR | CLOCAL | CRTSCTS);
+
+	baud = uart_get_baud_rate(port, termios, old, 0,
+				  port->uartclk / 14);
+
+	quot = __serial_get_clock_div(port->uartclk, baud);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	/* Ignore characters? */
+	tmp = readl(LPC32XX_HSUART_CTRL(port->membase));
+	if ((termios->c_cflag & CREAD) == 0)
+		tmp &= ~(LPC32XX_HSU_RX_INT_EN | LPC32XX_HSU_ERR_INT_EN);
+	else
+		tmp |= LPC32XX_HSU_RX_INT_EN | LPC32XX_HSU_ERR_INT_EN;
+	writel(tmp, LPC32XX_HSUART_CTRL(port->membase));
+
+	writel(quot, LPC32XX_HSUART_RATE(port->membase));
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	/* Don't rewrite B0 */
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+}
+
+static const char *serial_lpc32xx_type(struct uart_port *port)
+{
+	return MODNAME;
+}
+
+static void serial_lpc32xx_release_port(struct uart_port *port)
+{
+	if ((port->iotype == UPIO_MEM32) && (port->mapbase)) {
+		if (port->flags & UPF_IOREMAP) {
+			iounmap(port->membase);
+			port->membase = NULL;
+		}
+
+		release_mem_region(port->mapbase, SZ_4K);
+	}
+}
+
+static int serial_lpc32xx_request_port(struct uart_port *port)
+{
+	int ret = -ENODEV;
+
+	if ((port->iotype == UPIO_MEM32) && (port->mapbase)) {
+		ret = 0;
+
+		if (!request_mem_region(port->mapbase, SZ_4K, MODNAME))
+			ret = -EBUSY;
+		else if (port->flags & UPF_IOREMAP) {
+			port->membase = ioremap(port->mapbase, SZ_4K);
+			if (!port->membase) {
+				release_mem_region(port->mapbase, SZ_4K);
+				ret = -ENOMEM;
+			}
+		}
+	}
+
+	return ret;
+}
+
+static void serial_lpc32xx_config_port(struct uart_port *port, int uflags)
+{
+	int ret;
+
+	ret = serial_lpc32xx_request_port(port);
+	if (ret < 0)
+		return;
+	port->type = PORT_UART00;
+	port->fifosize = 64;
+
+	__serial_uart_flush(port);
+
+	writel((LPC32XX_HSU_TX_INT | LPC32XX_HSU_FE_INT |
+		LPC32XX_HSU_BRK_INT | LPC32XX_HSU_RX_OE_INT),
+	       LPC32XX_HSUART_IIR(port->membase));
+
+	writel(0xFF, LPC32XX_HSUART_RATE(port->membase));
+
+	/* Set receiver timeout, HSU offset of 20, no break, no interrupts,
+	   and default FIFO trigger levels */
+	writel(LPC32XX_HSU_TX_TL8B | LPC32XX_HSU_RX_TL32B |
+	       LPC32XX_HSU_OFFSET(20) | LPC32XX_HSU_TMO_INACT_4B,
+	       LPC32XX_HSUART_CTRL(port->membase));
+}
+
+static int serial_lpc32xx_verify_port(struct uart_port *port,
+				      struct serial_struct *ser)
+{
+	int ret = 0;
+
+	if (ser->type != PORT_UART00)
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static struct uart_ops serial_lpc32xx_pops = {
+	.tx_empty	= serial_lpc32xx_tx_empty,
+	.set_mctrl	= serial_lpc32xx_set_mctrl,
+	.get_mctrl	= serial_lpc32xx_get_mctrl,
+	.stop_tx	= serial_lpc32xx_stop_tx,
+	.start_tx	= serial_lpc32xx_start_tx,
+	.stop_rx	= serial_lpc32xx_stop_rx,
+	.enable_ms	= serial_lpc32xx_enable_ms,
+	.break_ctl	= serial_lpc32xx_break_ctl,
+	.startup	= serial_lpc32xx_startup,
+	.shutdown	= serial_lpc32xx_shutdown,
+	.set_termios	= serial_lpc32xx_set_termios,
+	.type		= serial_lpc32xx_type,
+	.release_port	= serial_lpc32xx_release_port,
+	.request_port	= serial_lpc32xx_request_port,
+	.config_port	= serial_lpc32xx_config_port,
+	.verify_port	= serial_lpc32xx_verify_port,
+};
+
+/*
+ * Register a set of serial devices attached to a platform device
+ */
+static int __devinit serial_hs_lpc32xx_probe(struct platform_device *pdev)
+{
+	struct lpc32xx_hsuart_port *p = &lpc32xx_hs_ports[uarts_registered];
+	int ret = 0;
+	struct resource *res;
+
+	if (uarts_registered >= MAX_PORTS) {
+		dev_err(&pdev->dev,
+			"Error: Number of possible ports exceeded (%d)!\n",
+			uarts_registered + 1);
+		return -ENXIO;
+	}
+
+	memset(p, 0, sizeof(*p));
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev,
+			"Error getting mem resource for HS UART port %d\n",
+			uarts_registered);
+		return -ENXIO;
+	}
+	p->port.mapbase = res->start;
+	p->port.membase = NULL;
+
+	p->port.irq = platform_get_irq(pdev, 0);
+	if (p->port.irq < 0) {
+		dev_err(&pdev->dev, "Error getting irq for HS UART port %d\n",
+			uarts_registered);
+		return p->port.irq;
+	}
+
+	p->port.iotype = UPIO_MEM32;
+	p->port.uartclk = LPC32XX_MAIN_OSC_FREQ;
+	p->port.regshift = 2;
+	p->port.flags = UPF_BOOT_AUTOCONF | UPF_FIXED_PORT | UPF_IOREMAP;
+	p->port.dev = &pdev->dev;
+	p->port.ops = &serial_lpc32xx_pops;
+	p->port.line = uarts_registered++;
+	spin_lock_init(&p->port.lock);
+
+	/* send port to loopback mode by default */
+	lpc32xx_loopback_set(p->port.mapbase, 1);
+
+	ret = uart_add_one_port(&lpc32xx_hs_reg, &p->port);
+
+	platform_set_drvdata(pdev, p);
+
+	return ret;
+}
+
+/*
+ * Remove serial ports registered against a platform device.
+ */
+static int __devexit serial_hs_lpc32xx_remove(struct platform_device *pdev)
+{
+	struct lpc32xx_hsuart_port *p = platform_get_drvdata(pdev);
+
+	uart_remove_one_port(&lpc32xx_hs_reg, &p->port);
+
+	return 0;
+}
+
+
+#ifdef CONFIG_PM
+static int serial_hs_lpc32xx_suspend(struct platform_device *pdev,
+				     pm_message_t state)
+{
+	struct lpc32xx_hsuart_port *p = platform_get_drvdata(pdev);
+
+	uart_suspend_port(&lpc32xx_hs_reg, &p->port);
+
+	return 0;
+}
+
+static int serial_hs_lpc32xx_resume(struct platform_device *pdev)
+{
+	struct lpc32xx_hsuart_port *p = platform_get_drvdata(pdev);
+
+	uart_resume_port(&lpc32xx_hs_reg, &p->port);
+
+	return 0;
+}
+#else
+#define serial_hs_lpc32xx_suspend	NULL
+#define serial_hs_lpc32xx_resume	NULL
+#endif
+
+static const struct of_device_id serial_hs_lpc32xx_dt_ids[] = {
+	{ .compatible = "nxp,lpc3220-hsuart" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, serial_hs_lpc32xx_dt_ids);
+
+static struct platform_driver serial_hs_lpc32xx_driver = {
+	.probe		= serial_hs_lpc32xx_probe,
+	.remove		= __devexit_p(serial_hs_lpc32xx_remove),
+	.suspend	= serial_hs_lpc32xx_suspend,
+	.resume		= serial_hs_lpc32xx_resume,
+	.driver		= {
+		.name	= MODNAME,
+		.owner	= THIS_MODULE,
+		.of_match_table	= serial_hs_lpc32xx_dt_ids,
+	},
+};
+
+static int __init lpc32xx_hsuart_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&lpc32xx_hs_reg);
+	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&serial_hs_lpc32xx_driver);
+	if (ret)
+		uart_unregister_driver(&lpc32xx_hs_reg);
+
+	return ret;
+}
+
+static void __exit lpc32xx_hsuart_exit(void)
+{
+	platform_driver_unregister(&serial_hs_lpc32xx_driver);
+	uart_unregister_driver(&lpc32xx_hs_reg);
+}
+
+module_init(lpc32xx_hsuart_init);
+module_exit(lpc32xx_hsuart_exit);
+
+MODULE_AUTHOR("Kevin Wells <kevin.wells@nxp.com>");
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("NXP LPC32XX High Speed UART driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/m32r_sio.c b/drivers/tty/serial/m32r_sio.c
index a0703624d5e5..b13949ad3408 100644
--- a/drivers/tty/serial/m32r_sio.c
+++ b/drivers/tty/serial/m32r_sio.c
@@ -44,8 +44,6 @@
 #include <asm/io.h>
 #include <asm/irq.h>
 
-#define PORT_M32R_BASE	PORT_M32R_SIO
-#define PORT_INDEX(x)	(x - PORT_M32R_BASE + 1)
 #define BAUD_RATE	115200
 
 #include <linux/serial_core.h>
@@ -132,22 +130,6 @@ struct irq_info {
 
 static struct irq_info irq_lists[NR_IRQS];
 
-/*
- * Here we define the default xmit fifo size used for each type of UART.
- */
-static const struct serial_uart_config uart_config[] = {
-	[PORT_UNKNOWN] = {
-		.name			= "unknown",
-		.dfl_xmit_fifo_size	= 1,
-		.flags			= 0,
-	},
-	[PORT_INDEX(PORT_M32R_SIO)] = {
-		.name			= "M32RSIO",
-		.dfl_xmit_fifo_size	= 1,
-		.flags			= 0,
-	},
-};
-
 #ifdef CONFIG_SERIAL_M32R_PLDSIO
 
 #define __sio_in(x) inw((unsigned long)(x))
@@ -907,8 +889,7 @@ static void m32r_sio_config_port(struct uart_port *port, int unused)
 
 	spin_lock_irqsave(&up->port.lock, flags);
 
-	up->port.type = (PORT_M32R_SIO - PORT_M32R_BASE + 1);
-	up->port.fifosize = uart_config[up->port.type].dfl_xmit_fifo_size;
+	up->port.fifosize = 1;
 
 	spin_unlock_irqrestore(&up->port.lock, flags);
 }
@@ -916,23 +897,11 @@ static void m32r_sio_config_port(struct uart_port *port, int unused)
 static int
 m32r_sio_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
-	if (ser->irq >= nr_irqs || ser->irq < 0 ||
-	    ser->baud_base < 9600 || ser->type < PORT_UNKNOWN ||
-	    ser->type >= ARRAY_SIZE(uart_config))
+	if (ser->irq >= nr_irqs || ser->irq < 0 || ser->baud_base < 9600)
 		return -EINVAL;
 	return 0;
 }
 
-static const char *
-m32r_sio_type(struct uart_port *port)
-{
-	int type = port->type;
-
-	if (type >= ARRAY_SIZE(uart_config))
-		type = 0;
-	return uart_config[type].name;
-}
-
 static struct uart_ops m32r_sio_pops = {
 	.tx_empty	= m32r_sio_tx_empty,
 	.set_mctrl	= m32r_sio_set_mctrl,
@@ -946,7 +915,6 @@ static struct uart_ops m32r_sio_pops = {
 	.shutdown	= m32r_sio_shutdown,
 	.set_termios	= m32r_sio_set_termios,
 	.pm		= m32r_sio_pm,
-	.type		= m32r_sio_type,
 	.release_port	= m32r_sio_release_port,
 	.request_port	= m32r_sio_request_port,
 	.config_port	= m32r_sio_config_port,
diff --git a/drivers/tty/serial/max3100.c b/drivers/tty/serial/max3100.c
index b4902b99cfd2..0f24486be532 100644
--- a/drivers/tty/serial/max3100.c
+++ b/drivers/tty/serial/max3100.c
@@ -827,14 +827,16 @@ static int __devexit max3100_remove(struct spi_device *spi)
 
 	/* find out the index for the chip we are removing */
 	for (i = 0; i < MAX_MAX3100; i++)
-		if (max3100s[i] == s)
+		if (max3100s[i] == s) {
+			dev_dbg(&spi->dev, "%s: removing port %d\n", __func__, i);
+			uart_remove_one_port(&max3100_uart_driver, &max3100s[i]->port);
+			kfree(max3100s[i]);
+			max3100s[i] = NULL;
 			break;
+		}
 
-	dev_dbg(&spi->dev, "%s: removing port %d\n", __func__, i);
-	uart_remove_one_port(&max3100_uart_driver, &max3100s[i]->port);
-	kfree(max3100s[i]);
-	max3100s[i] = NULL;
-
+	WARN_ON(i == MAX_MAX3100);
+	
 	/* check if this is the last chip we have */
 	for (i = 0; i < MAX_MAX3100; i++)
 		if (max3100s[i]) {
@@ -910,17 +912,7 @@ static struct spi_driver max3100_driver = {
 	.resume		= max3100_resume,
 };
 
-static int __init max3100_init(void)
-{
-	return spi_register_driver(&max3100_driver);
-}
-module_init(max3100_init);
-
-static void __exit max3100_exit(void)
-{
-	spi_unregister_driver(&max3100_driver);
-}
-module_exit(max3100_exit);
+module_spi_driver(max3100_driver);
 
 MODULE_DESCRIPTION("MAX3100 driver");
 MODULE_AUTHOR("Christian Pellegrin <chripell@evolware.org>");
diff --git a/drivers/tty/serial/max3107.c b/drivers/tty/serial/max3107.c
deleted file mode 100644
index 17c7ba805d98..000000000000
--- a/drivers/tty/serial/max3107.c
+++ /dev/null
@@ -1,1215 +0,0 @@
-/*
- *  max3107.c - spi uart protocol driver for Maxim 3107
- *  Based on max3100.c
- *	by Christian Pellegrin <chripell@evolware.org>
- *  and	max3110.c
- *	by Feng Tang <feng.tang@intel.com>
- *
- *  Copyright (C) Aavamobile 2009
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute 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/delay.h>
-#include <linux/device.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/gpio.h>
-#include <linux/spi/spi.h>
-#include <linux/freezer.h>
-#include <linux/module.h>
-#include "max3107.h"
-
-static const struct baud_table brg26_ext[] = {
-	{ 300,    MAX3107_BRG26_B300 },
-	{ 600,    MAX3107_BRG26_B600 },
-	{ 1200,   MAX3107_BRG26_B1200 },
-	{ 2400,   MAX3107_BRG26_B2400 },
-	{ 4800,   MAX3107_BRG26_B4800 },
-	{ 9600,   MAX3107_BRG26_B9600 },
-	{ 19200,  MAX3107_BRG26_B19200 },
-	{ 57600,  MAX3107_BRG26_B57600 },
-	{ 115200, MAX3107_BRG26_B115200 },
-	{ 230400, MAX3107_BRG26_B230400 },
-	{ 460800, MAX3107_BRG26_B460800 },
-	{ 921600, MAX3107_BRG26_B921600 },
-	{ 0, 0 }
-};
-
-static const struct baud_table brg13_int[] = {
-	{ 300,    MAX3107_BRG13_IB300 },
-	{ 600,    MAX3107_BRG13_IB600 },
-	{ 1200,   MAX3107_BRG13_IB1200 },
-	{ 2400,   MAX3107_BRG13_IB2400 },
-	{ 4800,   MAX3107_BRG13_IB4800 },
-	{ 9600,   MAX3107_BRG13_IB9600 },
-	{ 19200,  MAX3107_BRG13_IB19200 },
-	{ 57600,  MAX3107_BRG13_IB57600 },
-	{ 115200, MAX3107_BRG13_IB115200 },
-	{ 230400, MAX3107_BRG13_IB230400 },
-	{ 460800, MAX3107_BRG13_IB460800 },
-	{ 921600, MAX3107_BRG13_IB921600 },
-	{ 0, 0 }
-};
-
-static u32 get_new_brg(int baud, struct max3107_port *s)
-{
-	int i;
-	const struct baud_table *baud_tbl = s->baud_tbl;
-
-	for (i = 0; i < 13; i++) {
-		if (baud == baud_tbl[i].baud)
-			return baud_tbl[i].new_brg;
-	}
-
-	return 0;
-}
-
-/* Perform SPI transfer for write/read of device register(s) */
-int max3107_rw(struct max3107_port *s, u8 *tx, u8 *rx, int len)
-{
-	struct spi_message spi_msg;
-	struct spi_transfer spi_xfer;
-
-	/* Initialize SPI ,message */
-	spi_message_init(&spi_msg);
-
-	/* Initialize SPI transfer */
-	memset(&spi_xfer, 0, sizeof spi_xfer);
-	spi_xfer.len = len;
-	spi_xfer.tx_buf = tx;
-	spi_xfer.rx_buf = rx;
-	spi_xfer.speed_hz = MAX3107_SPI_SPEED;
-
-	/* Add SPI transfer to SPI message */
-	spi_message_add_tail(&spi_xfer, &spi_msg);
-
-#ifdef DBG_TRACE_SPI_DATA
-	{
-		int i;
-		pr_info("tx len %d:\n", spi_xfer.len);
-		for (i = 0 ; i < spi_xfer.len && i < 32 ; i++)
-			pr_info(" %x", ((u8 *)spi_xfer.tx_buf)[i]);
-		pr_info("\n");
-	}
-#endif
-
-	/* Perform synchronous SPI transfer */
-	if (spi_sync(s->spi, &spi_msg)) {
-		dev_err(&s->spi->dev, "spi_sync failure\n");
-		return -EIO;
-	}
-
-#ifdef DBG_TRACE_SPI_DATA
-	if (spi_xfer.rx_buf) {
-		int i;
-		pr_info("rx len %d:\n", spi_xfer.len);
-		for (i = 0 ; i < spi_xfer.len && i < 32 ; i++)
-			pr_info(" %x", ((u8 *)spi_xfer.rx_buf)[i]);
-		pr_info("\n");
-	}
-#endif
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max3107_rw);
-
-/* Puts received data to circular buffer */
-static void put_data_to_circ_buf(struct max3107_port *s, unsigned char *data,
-					int len)
-{
-	struct uart_port *port = &s->port;
-	struct tty_struct *tty;
-
-	if (!port->state)
-		return;
-
-	tty = port->state->port.tty;
-	if (!tty)
-		return;
-
-	/* Insert received data */
-	tty_insert_flip_string(tty, data, len);
-	/* Update RX counter */
-	port->icount.rx += len;
-}
-
-/* Handle data receiving */
-static void max3107_handlerx(struct max3107_port *s, u16 rxlvl)
-{
-	int i;
-	int j;
-	int len;				/* SPI transfer buffer length */
-	u16 *buf;
-	u8 *valid_str;
-
-	if (!s->rx_enabled)
-		/* RX is disabled */
-		return;
-
-	if (rxlvl == 0) {
-		/* RX fifo is empty */
-		return;
-	} else if (rxlvl >= MAX3107_RX_FIFO_SIZE) {
-		dev_warn(&s->spi->dev, "Possible RX FIFO overrun %d\n", rxlvl);
-		/* Ensure sanity of RX level */
-		rxlvl = MAX3107_RX_FIFO_SIZE;
-	}
-	if ((s->rxbuf == 0) || (s->rxstr == 0)) {
-		dev_warn(&s->spi->dev, "Rx buffer/str isn't ready\n");
-		return;
-	}
-	buf = s->rxbuf;
-	valid_str = s->rxstr;
-	while (rxlvl) {
-		pr_debug("rxlvl %d\n", rxlvl);
-		/* Clear buffer */
-		memset(buf, 0, sizeof(u16) * (MAX3107_RX_FIFO_SIZE + 2));
-		len = 0;
-		if (s->irqen_reg & MAX3107_IRQ_RXFIFO_BIT) {
-			/* First disable RX FIFO interrupt */
-			pr_debug("Disabling RX INT\n");
-			buf[0] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG);
-			s->irqen_reg &= ~MAX3107_IRQ_RXFIFO_BIT;
-			buf[0] |= s->irqen_reg;
-			len++;
-		}
-		/* Just increase the length by amount of words in FIFO since
-		 * buffer was zeroed and SPI transfer of 0x0000 means reading
-		 * from RX FIFO
-		 */
-		len += rxlvl;
-		/* Append RX level query */
-		buf[len] = MAX3107_RXFIFOLVL_REG;
-		len++;
-
-		/* Perform the SPI transfer */
-		if (max3107_rw(s, (u8 *)buf, (u8 *)buf, len * 2)) {
-			dev_err(&s->spi->dev, "SPI transfer for RX h failed\n");
-			return;
-		}
-
-		/* Skip RX FIFO interrupt disabling word if it was added */
-		j = ((len - 1) - rxlvl);
-		/* Read received words */
-		for (i = 0; i < rxlvl; i++, j++)
-			valid_str[i] = (u8)buf[j];
-		put_data_to_circ_buf(s, valid_str, rxlvl);
-		/* Get new RX level */
-		rxlvl = (buf[len - 1] & MAX3107_SPI_RX_DATA_MASK);
-	}
-
-	if (s->rx_enabled) {
-		/* RX still enabled, re-enable RX FIFO interrupt */
-		pr_debug("Enabling RX INT\n");
-		buf[0] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG);
-		s->irqen_reg |= MAX3107_IRQ_RXFIFO_BIT;
-		buf[0] |= s->irqen_reg;
-		if (max3107_rw(s, (u8 *)buf, NULL, 2))
-			dev_err(&s->spi->dev, "RX FIFO INT enabling failed\n");
-	}
-
-	/* Push the received data to receivers */
-	if (s->port.state->port.tty)
-		tty_flip_buffer_push(s->port.state->port.tty);
-}
-
-
-/* Handle data sending */
-static void max3107_handletx(struct max3107_port *s)
-{
-	struct circ_buf *xmit = &s->port.state->xmit;
-	int i;
-	unsigned long flags;
-	int len;				/* SPI transfer buffer length */
-	u16 *buf;
-
-	if (!s->tx_fifo_empty)
-		/* Don't send more data before previous data is sent */
-		return;
-
-	if (uart_circ_empty(xmit) || uart_tx_stopped(&s->port))
-		/* No data to send or TX is stopped */
-		return;
-
-	if (!s->txbuf) {
-		dev_warn(&s->spi->dev, "Txbuf isn't ready\n");
-		return;
-	}
-	buf = s->txbuf;
-	/* Get length of data pending in circular buffer */
-	len = uart_circ_chars_pending(xmit);
-	if (len) {
-		/* Limit to size of TX FIFO */
-		if (len > MAX3107_TX_FIFO_SIZE)
-			len = MAX3107_TX_FIFO_SIZE;
-
-		pr_debug("txlen %d\n", len);
-
-		/* Update TX counter */
-		s->port.icount.tx += len;
-
-		/* TX FIFO will no longer be empty */
-		s->tx_fifo_empty = 0;
-
-		i = 0;
-		if (s->irqen_reg & MAX3107_IRQ_TXEMPTY_BIT) {
-			/* First disable TX empty interrupt */
-			pr_debug("Disabling TE INT\n");
-			buf[i] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG);
-			s->irqen_reg &= ~MAX3107_IRQ_TXEMPTY_BIT;
-			buf[i] |= s->irqen_reg;
-			i++;
-			len++;
-		}
-		/* Add data to send */
-		spin_lock_irqsave(&s->port.lock, flags);
-		for ( ; i < len ; i++) {
-			buf[i] = (MAX3107_WRITE_BIT | MAX3107_THR_REG);
-			buf[i] |= ((u16)xmit->buf[xmit->tail] &
-						MAX3107_SPI_TX_DATA_MASK);
-			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
-		}
-		spin_unlock_irqrestore(&s->port.lock, flags);
-		if (!(s->irqen_reg & MAX3107_IRQ_TXEMPTY_BIT)) {
-			/* Enable TX empty interrupt */
-			pr_debug("Enabling TE INT\n");
-			buf[i] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG);
-			s->irqen_reg |= MAX3107_IRQ_TXEMPTY_BIT;
-			buf[i] |= s->irqen_reg;
-			i++;
-			len++;
-		}
-		if (!s->tx_enabled) {
-			/* Enable TX */
-			pr_debug("Enable TX\n");
-			buf[i] = (MAX3107_WRITE_BIT | MAX3107_MODE1_REG);
-			spin_lock_irqsave(&s->data_lock, flags);
-			s->mode1_reg &= ~MAX3107_MODE1_TXDIS_BIT;
-			buf[i] |= s->mode1_reg;
-			spin_unlock_irqrestore(&s->data_lock, flags);
-			s->tx_enabled = 1;
-			i++;
-			len++;
-		}
-
-		/* Perform the SPI transfer */
-		if (max3107_rw(s, (u8 *)buf, NULL, len*2)) {
-			dev_err(&s->spi->dev,
-				"SPI transfer TX handling failed\n");
-			return;
-		}
-	}
-
-	/* Indicate wake up if circular buffer is getting low on data */
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
-		uart_write_wakeup(&s->port);
-
-}
-
-/* Handle interrupts
- * Also reads and returns current RX FIFO level
- */
-static u16 handle_interrupt(struct max3107_port *s)
-{
-	u16 buf[4];	/* Buffer for SPI transfers */
-	u8 irq_status;
-	u16 rx_level;
-	unsigned long flags;
-
-	/* Read IRQ status register */
-	buf[0] = MAX3107_IRQSTS_REG;
-	/* Read status IRQ status register */
-	buf[1] = MAX3107_STS_IRQSTS_REG;
-	/* Read LSR IRQ status register */
-	buf[2] = MAX3107_LSR_IRQSTS_REG;
-	/* Query RX level */
-	buf[3] = MAX3107_RXFIFOLVL_REG;
-
-	if (max3107_rw(s, (u8 *)buf, (u8 *)buf, 8)) {
-		dev_err(&s->spi->dev,
-			"SPI transfer for INTR handling failed\n");
-		return 0;
-	}
-
-	irq_status = (u8)buf[0];
-	pr_debug("IRQSTS %x\n", irq_status);
-	rx_level = (buf[3] & MAX3107_SPI_RX_DATA_MASK);
-
-	if (irq_status & MAX3107_IRQ_LSR_BIT) {
-		/* LSR interrupt */
-		if (buf[2] & MAX3107_LSR_RXTO_BIT)
-			/* RX timeout interrupt,
-			 * handled by normal RX handling
-			 */
-			pr_debug("RX TO INT\n");
-	}
-
-	if (irq_status & MAX3107_IRQ_TXEMPTY_BIT) {
-		/* Tx empty interrupt,
-		 * disable TX and set tx_fifo_empty flag
-		 */
-		pr_debug("TE INT, disabling TX\n");
-		buf[0] = (MAX3107_WRITE_BIT | MAX3107_MODE1_REG);
-		spin_lock_irqsave(&s->data_lock, flags);
-		s->mode1_reg |= MAX3107_MODE1_TXDIS_BIT;
-		buf[0] |= s->mode1_reg;
-		spin_unlock_irqrestore(&s->data_lock, flags);
-		if (max3107_rw(s, (u8 *)buf, NULL, 2))
-			dev_err(&s->spi->dev, "SPI transfer TX dis failed\n");
-		s->tx_enabled = 0;
-		s->tx_fifo_empty = 1;
-	}
-
-	if (irq_status & MAX3107_IRQ_RXFIFO_BIT)
-		/* RX FIFO interrupt,
-		 * handled by normal RX handling
-		 */
-		pr_debug("RFIFO INT\n");
-
-	/* Return RX level */
-	return rx_level;
-}
-
-/* Trigger work thread*/
-static void max3107_dowork(struct max3107_port *s)
-{
-	if (!work_pending(&s->work) && !freezing(current) && !s->suspended)
-		queue_work(s->workqueue, &s->work);
-	else
-		dev_warn(&s->spi->dev, "interrup isn't serviced normally!\n");
-}
-
-/* Work thread */
-static void max3107_work(struct work_struct *w)
-{
-	struct max3107_port *s = container_of(w, struct max3107_port, work);
-	u16 rxlvl = 0;
-	int len;	/* SPI transfer buffer length */
-	u16 buf[5];	/* Buffer for SPI transfers */
-	unsigned long flags;
-
-	/* Start by reading current RX FIFO level */
-	buf[0] = MAX3107_RXFIFOLVL_REG;
-	if (max3107_rw(s, (u8 *)buf, (u8 *)buf, 2)) {
-		dev_err(&s->spi->dev, "SPI transfer RX lev failed\n");
-		rxlvl = 0;
-	} else {
-		rxlvl = (buf[0] & MAX3107_SPI_RX_DATA_MASK);
-	}
-
-	do {
-		pr_debug("rxlvl %d\n", rxlvl);
-
-		/* Handle RX */
-		max3107_handlerx(s, rxlvl);
-		rxlvl = 0;
-
-		if (s->handle_irq) {
-			/* Handle pending interrupts
-			 * We also get new RX FIFO level since new data may
-			 * have been received while pushing received data to
-			 * receivers
-			 */
-			s->handle_irq = 0;
-			rxlvl = handle_interrupt(s);
-		}
-
-		/* Handle TX */
-		max3107_handletx(s);
-
-		/* Handle configuration changes */
-		len = 0;
-		spin_lock_irqsave(&s->data_lock, flags);
-		if (s->mode1_commit) {
-			pr_debug("mode1_commit\n");
-			buf[len] = (MAX3107_WRITE_BIT | MAX3107_MODE1_REG);
-			buf[len++] |= s->mode1_reg;
-			s->mode1_commit = 0;
-		}
-		if (s->lcr_commit) {
-			pr_debug("lcr_commit\n");
-			buf[len] = (MAX3107_WRITE_BIT | MAX3107_LCR_REG);
-			buf[len++] |= s->lcr_reg;
-			s->lcr_commit = 0;
-		}
-		if (s->brg_commit) {
-			pr_debug("brg_commit\n");
-			buf[len] = (MAX3107_WRITE_BIT | MAX3107_BRGDIVMSB_REG);
-			buf[len++] |= ((s->brg_cfg >> 16) &
-						MAX3107_SPI_TX_DATA_MASK);
-			buf[len] = (MAX3107_WRITE_BIT | MAX3107_BRGDIVLSB_REG);
-			buf[len++] |= ((s->brg_cfg >> 8) &
-						MAX3107_SPI_TX_DATA_MASK);
-			buf[len] = (MAX3107_WRITE_BIT | MAX3107_BRGCFG_REG);
-			buf[len++] |= ((s->brg_cfg) & 0xff);
-			s->brg_commit = 0;
-		}
-		spin_unlock_irqrestore(&s->data_lock, flags);
-
-		if (len > 0) {
-			if (max3107_rw(s, (u8 *)buf, NULL, len * 2))
-				dev_err(&s->spi->dev,
-					"SPI transfer config failed\n");
-		}
-
-		/* Reloop if interrupt handling indicated data in RX FIFO */
-	} while (rxlvl);
-
-}
-
-/* Set sleep mode */
-static void max3107_set_sleep(struct max3107_port *s, int mode)
-{
-	u16 buf[1];	/* Buffer for SPI transfer */
-	unsigned long flags;
-	pr_debug("enter, mode %d\n", mode);
-
-	buf[0] = (MAX3107_WRITE_BIT | MAX3107_MODE1_REG);
-	spin_lock_irqsave(&s->data_lock, flags);
-	switch (mode) {
-	case MAX3107_DISABLE_FORCED_SLEEP:
-			s->mode1_reg &= ~MAX3107_MODE1_FORCESLEEP_BIT;
-			break;
-	case MAX3107_ENABLE_FORCED_SLEEP:
-			s->mode1_reg |= MAX3107_MODE1_FORCESLEEP_BIT;
-			break;
-	case MAX3107_DISABLE_AUTOSLEEP:
-			s->mode1_reg &= ~MAX3107_MODE1_AUTOSLEEP_BIT;
-			break;
-	case MAX3107_ENABLE_AUTOSLEEP:
-			s->mode1_reg |= MAX3107_MODE1_AUTOSLEEP_BIT;
-			break;
-	default:
-		spin_unlock_irqrestore(&s->data_lock, flags);
-		dev_warn(&s->spi->dev, "invalid sleep mode\n");
-		return;
-	}
-	buf[0] |= s->mode1_reg;
-	spin_unlock_irqrestore(&s->data_lock, flags);
-
-	if (max3107_rw(s, (u8 *)buf, NULL, 2))
-		dev_err(&s->spi->dev, "SPI transfer sleep mode failed\n");
-
-	if (mode == MAX3107_DISABLE_AUTOSLEEP ||
-			mode == MAX3107_DISABLE_FORCED_SLEEP)
-		msleep(MAX3107_WAKEUP_DELAY);
-}
-
-/* Perform full register initialization */
-static void max3107_register_init(struct max3107_port *s)
-{
-	u16 buf[11];	/* Buffer for SPI transfers */
-
-	/* 1. Configure baud rate, 9600 as default */
-	s->baud = 9600;
-	/* the below is default*/
-	if (s->ext_clk) {
-		s->brg_cfg = MAX3107_BRG26_B9600;
-		s->baud_tbl = (struct baud_table *)brg26_ext;
-	} else {
-		s->brg_cfg = MAX3107_BRG13_IB9600;
-		s->baud_tbl = (struct baud_table *)brg13_int;
-	}
-
-	if (s->pdata->init)
-		s->pdata->init(s);
-
-	buf[0] = (MAX3107_WRITE_BIT | MAX3107_BRGDIVMSB_REG)
-		| ((s->brg_cfg >> 16) & MAX3107_SPI_TX_DATA_MASK);
-	buf[1] = (MAX3107_WRITE_BIT | MAX3107_BRGDIVLSB_REG)
-		| ((s->brg_cfg >> 8) & MAX3107_SPI_TX_DATA_MASK);
-	buf[2] = (MAX3107_WRITE_BIT | MAX3107_BRGCFG_REG)
-		| ((s->brg_cfg) & 0xff);
-
-	/* 2. Configure LCR register, 8N1 mode by default */
-	s->lcr_reg = MAX3107_LCR_WORD_LEN_8;
-	buf[3] = (MAX3107_WRITE_BIT | MAX3107_LCR_REG)
-		| s->lcr_reg;
-
-	/* 3. Configure MODE 1 register */
-	s->mode1_reg = 0;
-	/* Enable IRQ pin */
-	s->mode1_reg |= MAX3107_MODE1_IRQSEL_BIT;
-	/* Disable TX */
-	s->mode1_reg |= MAX3107_MODE1_TXDIS_BIT;
-	s->tx_enabled = 0;
-	/* RX is enabled */
-	s->rx_enabled = 1;
-	buf[4] = (MAX3107_WRITE_BIT | MAX3107_MODE1_REG)
-		| s->mode1_reg;
-
-	/* 4. Configure MODE 2 register */
-	buf[5] = (MAX3107_WRITE_BIT | MAX3107_MODE2_REG);
-	if (s->loopback) {
-		/* Enable loopback */
-		buf[5] |= MAX3107_MODE2_LOOPBACK_BIT;
-	}
-	/* Reset FIFOs */
-	buf[5] |= MAX3107_MODE2_FIFORST_BIT;
-	s->tx_fifo_empty = 1;
-
-	/* 5. Configure FIFO trigger level register */
-	buf[6] = (MAX3107_WRITE_BIT | MAX3107_FIFOTRIGLVL_REG);
-	/* RX FIFO trigger for 16 words, TX FIFO trigger not used */
-	buf[6] |= (MAX3107_FIFOTRIGLVL_RX(16) | MAX3107_FIFOTRIGLVL_TX(0));
-
-	/* 6. Configure flow control levels */
-	buf[7] = (MAX3107_WRITE_BIT | MAX3107_FLOWLVL_REG);
-	/* Flow control halt level 96, resume level 48 */
-	buf[7] |= (MAX3107_FLOWLVL_RES(48) | MAX3107_FLOWLVL_HALT(96));
-
-	/* 7. Configure flow control */
-	buf[8] = (MAX3107_WRITE_BIT | MAX3107_FLOWCTRL_REG);
-	/* Enable auto CTS and auto RTS flow control */
-	buf[8] |= (MAX3107_FLOWCTRL_AUTOCTS_BIT | MAX3107_FLOWCTRL_AUTORTS_BIT);
-
-	/* 8. Configure RX timeout register */
-	buf[9] = (MAX3107_WRITE_BIT | MAX3107_RXTO_REG);
-	/* Timeout after 48 character intervals */
-	buf[9] |= 0x0030;
-
-	/* 9. Configure LSR interrupt enable register */
-	buf[10] = (MAX3107_WRITE_BIT | MAX3107_LSR_IRQEN_REG);
-	/* Enable RX timeout interrupt */
-	buf[10] |= MAX3107_LSR_RXTO_BIT;
-
-	/* Perform SPI transfer */
-	if (max3107_rw(s, (u8 *)buf, NULL, 22))
-		dev_err(&s->spi->dev, "SPI transfer for init failed\n");
-
-	/* 10. Clear IRQ status register by reading it */
-	buf[0] = MAX3107_IRQSTS_REG;
-
-	/* 11. Configure interrupt enable register */
-	/* Enable LSR interrupt */
-	s->irqen_reg = MAX3107_IRQ_LSR_BIT;
-	/* Enable RX FIFO interrupt */
-	s->irqen_reg |= MAX3107_IRQ_RXFIFO_BIT;
-	buf[1] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG)
-		| s->irqen_reg;
-
-	/* 12. Clear FIFO reset that was set in step 6 */
-	buf[2] = (MAX3107_WRITE_BIT | MAX3107_MODE2_REG);
-	if (s->loopback) {
-		/* Keep loopback enabled */
-		buf[2] |= MAX3107_MODE2_LOOPBACK_BIT;
-	}
-
-	/* Perform SPI transfer */
-	if (max3107_rw(s, (u8 *)buf, (u8 *)buf, 6))
-		dev_err(&s->spi->dev, "SPI transfer for init failed\n");
-
-}
-
-/* IRQ handler */
-static irqreturn_t max3107_irq(int irqno, void *dev_id)
-{
-	struct max3107_port *s = dev_id;
-
-	if (irqno != s->spi->irq) {
-		/* Unexpected IRQ */
-		return IRQ_NONE;
-	}
-
-	/* Indicate irq */
-	s->handle_irq = 1;
-
-	/* Trigger work thread */
-	max3107_dowork(s);
-
-	return IRQ_HANDLED;
-}
-
-/* HW suspension function
- *
- * Currently autosleep is used to decrease current consumption, alternative
- * approach would be to set the chip to reset mode if UART is not being
- * used but that would mess the GPIOs
- *
- */
-void max3107_hw_susp(struct max3107_port *s, int suspend)
-{
-	pr_debug("enter, suspend %d\n", suspend);
-
-	if (suspend) {
-		/* Suspend requested,
-		 * enable autosleep to decrease current consumption
-		 */
-		s->suspended = 1;
-		max3107_set_sleep(s, MAX3107_ENABLE_AUTOSLEEP);
-	} else {
-		/* Resume requested,
-		 * disable autosleep
-		 */
-		s->suspended = 0;
-		max3107_set_sleep(s, MAX3107_DISABLE_AUTOSLEEP);
-	}
-}
-EXPORT_SYMBOL_GPL(max3107_hw_susp);
-
-/* Modem status IRQ enabling */
-static void max3107_enable_ms(struct uart_port *port)
-{
-	/* Modem status not supported */
-}
-
-/* Data send function */
-static void max3107_start_tx(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-
-	/* Trigger work thread for sending data */
-	max3107_dowork(s);
-}
-
-/* Function for checking that there is no pending transfers */
-static unsigned int max3107_tx_empty(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-
-	pr_debug("returning %d\n",
-		  (s->tx_fifo_empty && uart_circ_empty(&s->port.state->xmit)));
-	return s->tx_fifo_empty && uart_circ_empty(&s->port.state->xmit);
-}
-
-/* Function for stopping RX */
-static void max3107_stop_rx(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-	unsigned long flags;
-
-	/* Set RX disabled in MODE 1 register */
-	spin_lock_irqsave(&s->data_lock, flags);
-	s->mode1_reg |= MAX3107_MODE1_RXDIS_BIT;
-	s->mode1_commit = 1;
-	spin_unlock_irqrestore(&s->data_lock, flags);
-	/* Set RX disabled */
-	s->rx_enabled = 0;
-	/* Trigger work thread for doing the actual configuration change */
-	max3107_dowork(s);
-}
-
-/* Function for returning control pin states */
-static unsigned int max3107_get_mctrl(struct uart_port *port)
-{
-	/* DCD and DSR are not wired and CTS/RTS is handled automatically
-	 * so just indicate DSR and CAR asserted
-	 */
-	return TIOCM_DSR | TIOCM_CAR;
-}
-
-/* Function for setting control pin states */
-static void max3107_set_mctrl(struct uart_port *port, unsigned int mctrl)
-{
-	/* DCD and DSR are not wired and CTS/RTS is hadnled automatically
-	 * so do nothing
-	 */
-}
-
-/* Function for configuring UART parameters */
-static void max3107_set_termios(struct uart_port *port,
-				struct ktermios *termios,
-				struct ktermios *old)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-	struct tty_struct *tty;
-	int baud;
-	u16 new_lcr = 0;
-	u32 new_brg = 0;
-	unsigned long flags;
-
-	if (!port->state)
-		return;
-
-	tty = port->state->port.tty;
-	if (!tty)
-		return;
-
-	/* Get new LCR register values */
-	/* Word size */
-	if ((termios->c_cflag & CSIZE) == CS7)
-		new_lcr |= MAX3107_LCR_WORD_LEN_7;
-	else
-		new_lcr |= MAX3107_LCR_WORD_LEN_8;
-
-	/* Parity */
-	if (termios->c_cflag & PARENB) {
-		new_lcr |= MAX3107_LCR_PARITY_BIT;
-		if (!(termios->c_cflag & PARODD))
-			new_lcr |= MAX3107_LCR_EVENPARITY_BIT;
-	}
-
-	/* Stop bits */
-	if (termios->c_cflag & CSTOPB) {
-		/* 2 stop bits */
-		new_lcr |= MAX3107_LCR_STOPLEN_BIT;
-	}
-
-	/* Mask termios capabilities we don't support */
-	termios->c_cflag &= ~CMSPAR;
-
-	/* Set status ignore mask */
-	s->port.ignore_status_mask = 0;
-	if (termios->c_iflag & IGNPAR)
-		s->port.ignore_status_mask |= MAX3107_ALL_ERRORS;
-
-	/* Set low latency to immediately handle pushed data */
-	s->port.state->port.tty->low_latency = 1;
-
-	/* Get new baud rate generator configuration */
-	baud = tty_get_baud_rate(tty);
-
-	spin_lock_irqsave(&s->data_lock, flags);
-	new_brg = get_new_brg(baud, s);
-	/* if can't find the corrent config, use previous */
-	if (!new_brg) {
-		baud = s->baud;
-		new_brg = s->brg_cfg;
-	}
-	spin_unlock_irqrestore(&s->data_lock, flags);
-	tty_termios_encode_baud_rate(termios, baud, baud);
-	s->baud = baud;
-
-	/* Update timeout according to new baud rate */
-	uart_update_timeout(port, termios->c_cflag, baud);
-
-	spin_lock_irqsave(&s->data_lock, flags);
-	if (s->lcr_reg != new_lcr) {
-		s->lcr_reg = new_lcr;
-		s->lcr_commit = 1;
-	}
-	if (s->brg_cfg != new_brg) {
-		s->brg_cfg = new_brg;
-		s->brg_commit = 1;
-	}
-	spin_unlock_irqrestore(&s->data_lock, flags);
-
-	/* Trigger work thread for doing the actual configuration change */
-	max3107_dowork(s);
-}
-
-/* Port shutdown function */
-static void max3107_shutdown(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-
-	if (s->suspended && s->pdata->hw_suspend)
-		s->pdata->hw_suspend(s, 0);
-
-	/* Free the interrupt */
-	free_irq(s->spi->irq, s);
-
-	if (s->workqueue) {
-		/* Flush and destroy work queue */
-		flush_workqueue(s->workqueue);
-		destroy_workqueue(s->workqueue);
-		s->workqueue = NULL;
-	}
-
-	/* Suspend HW */
-	if (s->pdata->hw_suspend)
-		s->pdata->hw_suspend(s, 1);
-}
-
-/* Port startup function */
-static int max3107_startup(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-
-	/* Initialize work queue */
-	s->workqueue = create_freezable_workqueue("max3107");
-	if (!s->workqueue) {
-		dev_err(&s->spi->dev, "Workqueue creation failed\n");
-		return -EBUSY;
-	}
-	INIT_WORK(&s->work, max3107_work);
-
-	/* Setup IRQ */
-	if (request_irq(s->spi->irq, max3107_irq, IRQF_TRIGGER_FALLING,
-			"max3107", s)) {
-		dev_err(&s->spi->dev, "IRQ reguest failed\n");
-		destroy_workqueue(s->workqueue);
-		s->workqueue = NULL;
-		return -EBUSY;
-	}
-
-	/* Resume HW */
-	if (s->pdata->hw_suspend)
-		s->pdata->hw_suspend(s, 0);
-
-	/* Init registers */
-	max3107_register_init(s);
-
-	return 0;
-}
-
-/* Port type function */
-static const char *max3107_type(struct uart_port *port)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-	return s->spi->modalias;
-}
-
-/* Port release function */
-static void max3107_release_port(struct uart_port *port)
-{
-	/* Do nothing */
-}
-
-/* Port request function */
-static int max3107_request_port(struct uart_port *port)
-{
-	/* Do nothing */
-	return 0;
-}
-
-/* Port config function */
-static void max3107_config_port(struct uart_port *port, int flags)
-{
-	struct max3107_port *s = container_of(port, struct max3107_port, port);
-	s->port.type = PORT_MAX3107;
-}
-
-/* Port verify function */
-static int max3107_verify_port(struct uart_port *port,
-				struct serial_struct *ser)
-{
-	if (ser->type == PORT_UNKNOWN || ser->type == PORT_MAX3107)
-		return 0;
-
-	return -EINVAL;
-}
-
-/* Port stop TX function */
-static void max3107_stop_tx(struct uart_port *port)
-{
-	/* Do nothing */
-}
-
-/* Port break control function */
-static void max3107_break_ctl(struct uart_port *port, int break_state)
-{
-	/* We don't support break control, do nothing */
-}
-
-
-/* Port functions */
-static struct uart_ops max3107_ops = {
-	.tx_empty       = max3107_tx_empty,
-	.set_mctrl      = max3107_set_mctrl,
-	.get_mctrl      = max3107_get_mctrl,
-	.stop_tx        = max3107_stop_tx,
-	.start_tx       = max3107_start_tx,
-	.stop_rx        = max3107_stop_rx,
-	.enable_ms      = max3107_enable_ms,
-	.break_ctl      = max3107_break_ctl,
-	.startup        = max3107_startup,
-	.shutdown       = max3107_shutdown,
-	.set_termios    = max3107_set_termios,
-	.type           = max3107_type,
-	.release_port   = max3107_release_port,
-	.request_port   = max3107_request_port,
-	.config_port    = max3107_config_port,
-	.verify_port    = max3107_verify_port,
-};
-
-/* UART driver data */
-static struct uart_driver max3107_uart_driver = {
-	.owner          = THIS_MODULE,
-	.driver_name    = "ttyMAX",
-	.dev_name       = "ttyMAX",
-	.nr             = 1,
-};
-
-static int driver_registered = 0;
-
-
-
-/* 'Generic' platform data */
-static struct max3107_plat generic_plat_data = {
-	.loopback               = 0,
-	.ext_clk                = 1,
-	.hw_suspend		= max3107_hw_susp,
-	.polled_mode            = 0,
-	.poll_time              = 0,
-};
-
-
-/*******************************************************************/
-
-/**
- *	max3107_probe		-	SPI bus probe entry point
- *	@spi: the spi device
- *
- *	SPI wants us to probe this device and if appropriate claim it.
- *	Perform any platform specific requirements and then initialise
- *	the device.
- */
-
-int max3107_probe(struct spi_device *spi, struct max3107_plat *pdata)
-{
-	struct max3107_port *s;
-	u16 buf[2];	/* Buffer for SPI transfers */
-	int retval;
-
-	pr_info("enter max3107 probe\n");
-
-	/* Allocate port structure */
-	s = kzalloc(sizeof(*s), GFP_KERNEL);
-	if (!s) {
-		pr_err("Allocating port structure failed\n");
-		return -ENOMEM;
-	}
-
-	s->pdata = pdata;
-
-	/* SPI Rx buffer
-	 * +2 for RX FIFO interrupt
-	 * disabling and RX level query
-	 */
-	s->rxbuf = kzalloc(sizeof(u16) * (MAX3107_RX_FIFO_SIZE+2), GFP_KERNEL);
-	if (!s->rxbuf) {
-		pr_err("Allocating RX buffer failed\n");
-		retval = -ENOMEM;
-		goto err_free4;
-	}
-	s->rxstr = kzalloc(sizeof(u8) * MAX3107_RX_FIFO_SIZE, GFP_KERNEL);
-	if (!s->rxstr) {
-		pr_err("Allocating RX buffer failed\n");
-		retval = -ENOMEM;
-		goto err_free3;
-	}
-	/* SPI Tx buffer
-	 * SPI transfer buffer
-	 * +3 for TX FIFO empty
-	 * interrupt disabling and
-	 * enabling and TX enabling
-	 */
-	s->txbuf = kzalloc(sizeof(u16) * MAX3107_TX_FIFO_SIZE + 3, GFP_KERNEL);
-	if (!s->txbuf) {
-		pr_err("Allocating TX buffer failed\n");
-		retval = -ENOMEM;
-		goto err_free2;
-	}
-	/* Initialize shared data lock */
-	spin_lock_init(&s->data_lock);
-
-	/* SPI intializations */
-	dev_set_drvdata(&spi->dev, s);
-	spi->mode = SPI_MODE_0;
-	spi->dev.platform_data = pdata;
-	spi->bits_per_word = 16;
-	s->ext_clk = pdata->ext_clk;
-	s->loopback = pdata->loopback;
-	spi_setup(spi);
-	s->spi = spi;
-
-	/* Check REV ID to ensure we are talking to what we expect */
-	buf[0] = MAX3107_REVID_REG;
-	if (max3107_rw(s, (u8 *)buf, (u8 *)buf, 2)) {
-		dev_err(&s->spi->dev, "SPI transfer for REVID read failed\n");
-		retval = -EIO;
-		goto err_free1;
-	}
-	if ((buf[0] & MAX3107_SPI_RX_DATA_MASK) != MAX3107_REVID1 &&
-		(buf[0] & MAX3107_SPI_RX_DATA_MASK) != MAX3107_REVID2) {
-		dev_err(&s->spi->dev, "REVID %x does not match\n",
-				(buf[0] & MAX3107_SPI_RX_DATA_MASK));
-		retval = -ENODEV;
-		goto err_free1;
-	}
-
-	/* Disable all interrupts */
-	buf[0] = (MAX3107_WRITE_BIT | MAX3107_IRQEN_REG | 0x0000);
-	buf[0] |= 0x0000;
-
-	/* Configure clock source */
-	buf[1] = (MAX3107_WRITE_BIT | MAX3107_CLKSRC_REG);
-	if (s->ext_clk) {
-		/* External clock */
-		buf[1] |= MAX3107_CLKSRC_EXTCLK_BIT;
-	}
-
-	/* PLL bypass ON */
-	buf[1] |= MAX3107_CLKSRC_PLLBYP_BIT;
-
-	/* Perform SPI transfer */
-	if (max3107_rw(s, (u8 *)buf, NULL, 4)) {
-		dev_err(&s->spi->dev, "SPI transfer for init failed\n");
-		retval = -EIO;
-		goto err_free1;
-	}
-
-	/* Register UART driver */
-	if (!driver_registered) {
-		retval = uart_register_driver(&max3107_uart_driver);
-		if (retval) {
-			dev_err(&s->spi->dev, "Registering UART driver failed\n");
-			goto err_free1;
-		}
-		driver_registered = 1;
-	}
-
-	/* Initialize UART port data */
-	s->port.fifosize = 128;
-	s->port.ops = &max3107_ops;
-	s->port.line = 0;
-	s->port.dev = &spi->dev;
-	s->port.uartclk = 9600;
-	s->port.flags = UPF_SKIP_TEST | UPF_BOOT_AUTOCONF;
-	s->port.irq = s->spi->irq;
-	s->port.type = PORT_MAX3107;
-
-	/* Add UART port */
-	retval = uart_add_one_port(&max3107_uart_driver, &s->port);
-	if (retval < 0) {
-		dev_err(&s->spi->dev, "Adding UART port failed\n");
-		goto err_free1;
-	}
-
-	if (pdata->configure) {
-		retval = pdata->configure(s);
-		if (retval < 0)
-			goto err_free1;
-	}
-
-	/* Go to suspend mode */
-	if (pdata->hw_suspend)
-		pdata->hw_suspend(s, 1);
-
-	return 0;
-
-err_free1:
-	kfree(s->txbuf);
-err_free2:
-	kfree(s->rxstr);
-err_free3:
-	kfree(s->rxbuf);
-err_free4:
-	kfree(s);
-	return retval;
-}
-EXPORT_SYMBOL_GPL(max3107_probe);
-
-/* Driver remove function */
-int max3107_remove(struct spi_device *spi)
-{
-	struct max3107_port *s = dev_get_drvdata(&spi->dev);
-
-	pr_info("enter max3107 remove\n");
-
-	/* Remove port */
-	if (uart_remove_one_port(&max3107_uart_driver, &s->port))
-		dev_warn(&s->spi->dev, "Removing UART port failed\n");
-
-
-	/* Free TxRx buffer */
-	kfree(s->rxbuf);
-	kfree(s->rxstr);
-	kfree(s->txbuf);
-
-	/* Free port structure */
-	kfree(s);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max3107_remove);
-
-/* Driver suspend function */
-int max3107_suspend(struct spi_device *spi, pm_message_t state)
-{
-#ifdef CONFIG_PM
-	struct max3107_port *s = dev_get_drvdata(&spi->dev);
-
-	pr_debug("enter suspend\n");
-
-	/* Suspend UART port */
-	uart_suspend_port(&max3107_uart_driver, &s->port);
-
-	/* Go to suspend mode */
-	if (s->pdata->hw_suspend)
-		s->pdata->hw_suspend(s, 1);
-#endif	/* CONFIG_PM */
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max3107_suspend);
-
-/* Driver resume function */
-int max3107_resume(struct spi_device *spi)
-{
-#ifdef CONFIG_PM
-	struct max3107_port *s = dev_get_drvdata(&spi->dev);
-
-	pr_debug("enter resume\n");
-
-	/* Resume from suspend */
-	if (s->pdata->hw_suspend)
-		s->pdata->hw_suspend(s, 0);
-
-	/* Resume UART port */
-	uart_resume_port(&max3107_uart_driver, &s->port);
-#endif	/* CONFIG_PM */
-	return 0;
-}
-EXPORT_SYMBOL_GPL(max3107_resume);
-
-static int max3107_probe_generic(struct spi_device *spi)
-{
-	return max3107_probe(spi, &generic_plat_data);
-}
-
-/* Spi driver data */
-static struct spi_driver max3107_driver = {
-	.driver = {
-		.name		= "max3107",
-		.owner		= THIS_MODULE,
-	},
-	.probe		= max3107_probe_generic,
-	.remove		= __devexit_p(max3107_remove),
-	.suspend	= max3107_suspend,
-	.resume		= max3107_resume,
-};
-
-/* Driver init function */
-static int __init max3107_init(void)
-{
-	pr_info("enter max3107 init\n");
-	return spi_register_driver(&max3107_driver);
-}
-
-/* Driver exit function */
-static void __exit max3107_exit(void)
-{
-	pr_info("enter max3107 exit\n");
-	/* Unregister UART driver */
-	if (driver_registered)
-		uart_unregister_driver(&max3107_uart_driver);
-	spi_unregister_driver(&max3107_driver);
-}
-
-module_init(max3107_init);
-module_exit(max3107_exit);
-
-MODULE_DESCRIPTION("MAX3107 driver");
-MODULE_AUTHOR("Aavamobile");
-MODULE_ALIAS("spi:max3107");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/max3107.h b/drivers/tty/serial/max3107.h
deleted file mode 100644
index 8415fc723b96..000000000000
--- a/drivers/tty/serial/max3107.h
+++ /dev/null
@@ -1,441 +0,0 @@
-/*
- * max3107.h - spi uart protocol driver header for Maxim 3107
- *
- * Copyright (C) Aavamobile 2009
- * Based on serial_max3100.h by Christian Pellegrin
- *
- * This program is free software; you can redistribute 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 _MAX3107_H
-#define _MAX3107_H
-
-/* Serial error status definitions */
-#define MAX3107_PARITY_ERROR	1
-#define MAX3107_FRAME_ERROR	2
-#define MAX3107_OVERRUN_ERROR	4
-#define MAX3107_ALL_ERRORS	(MAX3107_PARITY_ERROR | \
-				 MAX3107_FRAME_ERROR | \
-				 MAX3107_OVERRUN_ERROR)
-
-/* GPIO definitions */
-#define MAX3107_GPIO_BASE	88
-#define MAX3107_GPIO_COUNT	4
-
-
-/* GPIO connected to chip's reset pin */
-#define MAX3107_RESET_GPIO	87
-
-
-/* Chip reset delay */
-#define MAX3107_RESET_DELAY	10
-
-/* Chip wakeup delay */
-#define MAX3107_WAKEUP_DELAY	50
-
-
-/* Sleep mode definitions */
-#define MAX3107_DISABLE_FORCED_SLEEP	0
-#define MAX3107_ENABLE_FORCED_SLEEP	1
-#define MAX3107_DISABLE_AUTOSLEEP	2
-#define MAX3107_ENABLE_AUTOSLEEP	3
-
-
-/* Definitions for register access with SPI transfers
- *
- * SPI transfer format:
- *
- * Master to slave bits xzzzzzzzyyyyyyyy
- * Slave to master bits aaaaaaaabbbbbbbb
- *
- * where:
- * x = 0 for reads, 1 for writes
- * z = register address
- * y = new register value if write, 0 if read
- * a = unspecified
- * b = register value if read, unspecified if write
- */
-
-/* SPI speed */
-#define MAX3107_SPI_SPEED	(3125000 * 2)
-
-/* Write bit */
-#define MAX3107_WRITE_BIT	(1 << 15)
-
-/* SPI TX data mask */
-#define MAX3107_SPI_RX_DATA_MASK	(0x00ff)
-
-/* SPI RX data mask */
-#define MAX3107_SPI_TX_DATA_MASK	(0x00ff)
-
-/* Register access masks */
-#define MAX3107_RHR_REG			(0x0000) /* RX FIFO */
-#define MAX3107_THR_REG			(0x0000) /* TX FIFO */
-#define MAX3107_IRQEN_REG		(0x0100) /* IRQ enable */
-#define MAX3107_IRQSTS_REG		(0x0200) /* IRQ status */
-#define MAX3107_LSR_IRQEN_REG		(0x0300) /* LSR IRQ enable */
-#define MAX3107_LSR_IRQSTS_REG		(0x0400) /* LSR IRQ status */
-#define MAX3107_SPCHR_IRQEN_REG		(0x0500) /* Special char IRQ enable */
-#define MAX3107_SPCHR_IRQSTS_REG	(0x0600) /* Special char IRQ status */
-#define MAX3107_STS_IRQEN_REG		(0x0700) /* Status IRQ enable */
-#define MAX3107_STS_IRQSTS_REG		(0x0800) /* Status IRQ status */
-#define MAX3107_MODE1_REG		(0x0900) /* MODE1 */
-#define MAX3107_MODE2_REG		(0x0a00) /* MODE2 */
-#define MAX3107_LCR_REG			(0x0b00) /* LCR */
-#define MAX3107_RXTO_REG		(0x0c00) /* RX timeout */
-#define MAX3107_HDPIXDELAY_REG		(0x0d00) /* Auto transceiver delays */
-#define MAX3107_IRDA_REG		(0x0e00) /* IRDA settings */
-#define MAX3107_FLOWLVL_REG		(0x0f00) /* Flow control levels */
-#define MAX3107_FIFOTRIGLVL_REG		(0x1000) /* FIFO IRQ trigger levels */
-#define MAX3107_TXFIFOLVL_REG		(0x1100) /* TX FIFO level */
-#define MAX3107_RXFIFOLVL_REG		(0x1200) /* RX FIFO level */
-#define MAX3107_FLOWCTRL_REG		(0x1300) /* Flow control */
-#define MAX3107_XON1_REG		(0x1400) /* XON1 character */
-#define MAX3107_XON2_REG		(0x1500) /* XON2 character */
-#define MAX3107_XOFF1_REG		(0x1600) /* XOFF1 character */
-#define MAX3107_XOFF2_REG		(0x1700) /* XOFF2 character */
-#define MAX3107_GPIOCFG_REG		(0x1800) /* GPIO config */
-#define MAX3107_GPIODATA_REG		(0x1900) /* GPIO data */
-#define MAX3107_PLLCFG_REG		(0x1a00) /* PLL config */
-#define MAX3107_BRGCFG_REG		(0x1b00) /* Baud rate generator conf */
-#define MAX3107_BRGDIVLSB_REG		(0x1c00) /* Baud rate divisor LSB */
-#define MAX3107_BRGDIVMSB_REG		(0x1d00) /* Baud rate divisor MSB */
-#define MAX3107_CLKSRC_REG		(0x1e00) /* Clock source */
-#define MAX3107_REVID_REG		(0x1f00) /* Revision identification */
-
-/* IRQ register bits */
-#define MAX3107_IRQ_LSR_BIT	(1 << 0) /* LSR interrupt */
-#define MAX3107_IRQ_SPCHR_BIT	(1 << 1) /* Special char interrupt */
-#define MAX3107_IRQ_STS_BIT	(1 << 2) /* Status interrupt */
-#define MAX3107_IRQ_RXFIFO_BIT	(1 << 3) /* RX FIFO interrupt */
-#define MAX3107_IRQ_TXFIFO_BIT	(1 << 4) /* TX FIFO interrupt */
-#define MAX3107_IRQ_TXEMPTY_BIT	(1 << 5) /* TX FIFO empty interrupt */
-#define MAX3107_IRQ_RXEMPTY_BIT	(1 << 6) /* RX FIFO empty interrupt */
-#define MAX3107_IRQ_CTS_BIT	(1 << 7) /* CTS interrupt */
-
-/* LSR register bits */
-#define MAX3107_LSR_RXTO_BIT	(1 << 0) /* RX timeout */
-#define MAX3107_LSR_RXOVR_BIT	(1 << 1) /* RX overrun */
-#define MAX3107_LSR_RXPAR_BIT	(1 << 2) /* RX parity error */
-#define MAX3107_LSR_FRERR_BIT	(1 << 3) /* Frame error */
-#define MAX3107_LSR_RXBRK_BIT	(1 << 4) /* RX break */
-#define MAX3107_LSR_RXNOISE_BIT	(1 << 5) /* RX noise */
-#define MAX3107_LSR_UNDEF6_BIT	(1 << 6) /* Undefined/not used */
-#define MAX3107_LSR_CTS_BIT	(1 << 7) /* CTS pin state */
-
-/* Special character register bits */
-#define MAX3107_SPCHR_XON1_BIT		(1 << 0) /* XON1 character */
-#define MAX3107_SPCHR_XON2_BIT		(1 << 1) /* XON2 character */
-#define MAX3107_SPCHR_XOFF1_BIT		(1 << 2) /* XOFF1 character */
-#define MAX3107_SPCHR_XOFF2_BIT		(1 << 3) /* XOFF2 character */
-#define MAX3107_SPCHR_BREAK_BIT		(1 << 4) /* RX break */
-#define MAX3107_SPCHR_MULTIDROP_BIT	(1 << 5) /* 9-bit multidrop addr char */
-#define MAX3107_SPCHR_UNDEF6_BIT	(1 << 6) /* Undefined/not used */
-#define MAX3107_SPCHR_UNDEF7_BIT	(1 << 7) /* Undefined/not used */
-
-/* Status register bits */
-#define MAX3107_STS_GPIO0_BIT		(1 << 0) /* GPIO 0 interrupt */
-#define MAX3107_STS_GPIO1_BIT		(1 << 1) /* GPIO 1 interrupt */
-#define MAX3107_STS_GPIO2_BIT		(1 << 2) /* GPIO 2 interrupt */
-#define MAX3107_STS_GPIO3_BIT		(1 << 3) /* GPIO 3 interrupt */
-#define MAX3107_STS_UNDEF4_BIT		(1 << 4) /* Undefined/not used */
-#define MAX3107_STS_CLKREADY_BIT	(1 << 5) /* Clock ready */
-#define MAX3107_STS_SLEEP_BIT		(1 << 6) /* Sleep interrupt */
-#define MAX3107_STS_UNDEF7_BIT		(1 << 7) /* Undefined/not used */
-
-/* MODE1 register bits */
-#define MAX3107_MODE1_RXDIS_BIT		(1 << 0) /* RX disable */
-#define MAX3107_MODE1_TXDIS_BIT		(1 << 1) /* TX disable */
-#define MAX3107_MODE1_TXHIZ_BIT		(1 << 2) /* TX pin three-state */
-#define MAX3107_MODE1_RTSHIZ_BIT	(1 << 3) /* RTS pin three-state */
-#define MAX3107_MODE1_TRNSCVCTRL_BIT	(1 << 4) /* Transceiver ctrl enable */
-#define MAX3107_MODE1_FORCESLEEP_BIT	(1 << 5) /* Force sleep mode */
-#define MAX3107_MODE1_AUTOSLEEP_BIT	(1 << 6) /* Auto sleep enable */
-#define MAX3107_MODE1_IRQSEL_BIT	(1 << 7) /* IRQ pin enable */
-
-/* MODE2 register bits */
-#define MAX3107_MODE2_RST_BIT		(1 << 0) /* Chip reset */
-#define MAX3107_MODE2_FIFORST_BIT	(1 << 1) /* FIFO reset */
-#define MAX3107_MODE2_RXTRIGINV_BIT	(1 << 2) /* RX FIFO INT invert */
-#define MAX3107_MODE2_RXEMPTINV_BIT	(1 << 3) /* RX FIFO empty INT invert */
-#define MAX3107_MODE2_SPCHR_BIT		(1 << 4) /* Special chr detect enable */
-#define MAX3107_MODE2_LOOPBACK_BIT	(1 << 5) /* Internal loopback enable */
-#define MAX3107_MODE2_MULTIDROP_BIT	(1 << 6) /* 9-bit multidrop enable */
-#define MAX3107_MODE2_ECHOSUPR_BIT	(1 << 7) /* ECHO suppression enable */
-
-/* LCR register bits */
-#define MAX3107_LCR_LENGTH0_BIT		(1 << 0) /* Word length bit 0 */
-#define MAX3107_LCR_LENGTH1_BIT		(1 << 1) /* Word length bit 1
-						  *
-						  * Word length bits table:
-						  * 00 -> 5 bit words
-						  * 01 -> 6 bit words
-						  * 10 -> 7 bit words
-						  * 11 -> 8 bit words
-						  */
-#define MAX3107_LCR_STOPLEN_BIT		(1 << 2) /* STOP length bit
-						  *
-						  * STOP length bit table:
-						  * 0 -> 1 stop bit
-						  * 1 -> 1-1.5 stop bits if
-						  *      word length is 5,
-						  *      2 stop bits otherwise
-						  */
-#define MAX3107_LCR_PARITY_BIT		(1 << 3) /* Parity bit enable */
-#define MAX3107_LCR_EVENPARITY_BIT	(1 << 4) /* Even parity bit enable */
-#define MAX3107_LCR_FORCEPARITY_BIT	(1 << 5) /* 9-bit multidrop parity */
-#define MAX3107_LCR_TXBREAK_BIT		(1 << 6) /* TX break enable */
-#define MAX3107_LCR_RTS_BIT		(1 << 7) /* RTS pin control */
-#define MAX3107_LCR_WORD_LEN_5		(0x0000)
-#define MAX3107_LCR_WORD_LEN_6		(0x0001)
-#define MAX3107_LCR_WORD_LEN_7		(0x0002)
-#define MAX3107_LCR_WORD_LEN_8		(0x0003)
-
-
-/* IRDA register bits */
-#define MAX3107_IRDA_IRDAEN_BIT		(1 << 0) /* IRDA mode enable */
-#define MAX3107_IRDA_SIR_BIT		(1 << 1) /* SIR mode enable */
-#define MAX3107_IRDA_SHORTIR_BIT	(1 << 2) /* Short SIR mode enable */
-#define MAX3107_IRDA_MIR_BIT		(1 << 3) /* MIR mode enable */
-#define MAX3107_IRDA_RXINV_BIT		(1 << 4) /* RX logic inversion enable */
-#define MAX3107_IRDA_TXINV_BIT		(1 << 5) /* TX logic inversion enable */
-#define MAX3107_IRDA_UNDEF6_BIT		(1 << 6) /* Undefined/not used */
-#define MAX3107_IRDA_UNDEF7_BIT		(1 << 7) /* Undefined/not used */
-
-/* Flow control trigger level register masks */
-#define MAX3107_FLOWLVL_HALT_MASK	(0x000f) /* Flow control halt level */
-#define MAX3107_FLOWLVL_RES_MASK	(0x00f0) /* Flow control resume level */
-#define MAX3107_FLOWLVL_HALT(words)	((words/8) & 0x000f)
-#define MAX3107_FLOWLVL_RES(words)	(((words/8) & 0x000f) << 4)
-
-/* FIFO interrupt trigger level register masks */
-#define MAX3107_FIFOTRIGLVL_TX_MASK	(0x000f) /* TX FIFO trigger level */
-#define MAX3107_FIFOTRIGLVL_RX_MASK	(0x00f0) /* RX FIFO trigger level */
-#define MAX3107_FIFOTRIGLVL_TX(words)	((words/8) & 0x000f)
-#define MAX3107_FIFOTRIGLVL_RX(words)	(((words/8) & 0x000f) << 4)
-
-/* Flow control register bits */
-#define MAX3107_FLOWCTRL_AUTORTS_BIT	(1 << 0) /* Auto RTS flow ctrl enable */
-#define MAX3107_FLOWCTRL_AUTOCTS_BIT	(1 << 1) /* Auto CTS flow ctrl enable */
-#define MAX3107_FLOWCTRL_GPIADDR_BIT	(1 << 2) /* Enables that GPIO inputs
-						  * are used in conjunction with
-						  * XOFF2 for definition of
-						  * special character */
-#define MAX3107_FLOWCTRL_SWFLOWEN_BIT	(1 << 3) /* Auto SW flow ctrl enable */
-#define MAX3107_FLOWCTRL_SWFLOW0_BIT	(1 << 4) /* SWFLOW bit 0 */
-#define MAX3107_FLOWCTRL_SWFLOW1_BIT	(1 << 5) /* SWFLOW bit 1
-						  *
-						  * SWFLOW bits 1 & 0 table:
-						  * 00 -> no transmitter flow
-						  *       control
-						  * 01 -> receiver compares
-						  *       XON2 and XOFF2
-						  *       and controls
-						  *       transmitter
-						  * 10 -> receiver compares
-						  *       XON1 and XOFF1
-						  *       and controls
-						  *       transmitter
-						  * 11 -> receiver compares
-						  *       XON1, XON2, XOFF1 and
-						  *       XOFF2 and controls
-						  *       transmitter
-						  */
-#define MAX3107_FLOWCTRL_SWFLOW2_BIT	(1 << 6) /* SWFLOW bit 2 */
-#define MAX3107_FLOWCTRL_SWFLOW3_BIT	(1 << 7) /* SWFLOW bit 3
-						  *
-						  * SWFLOW bits 3 & 2 table:
-						  * 00 -> no received flow
-						  *       control
-						  * 01 -> transmitter generates
-						  *       XON2 and XOFF2
-						  * 10 -> transmitter generates
-						  *       XON1 and XOFF1
-						  * 11 -> transmitter generates
-						  *       XON1, XON2, XOFF1 and
-						  *       XOFF2
-						  */
-
-/* GPIO configuration register bits */
-#define MAX3107_GPIOCFG_GP0OUT_BIT	(1 << 0) /* GPIO 0 output enable */
-#define MAX3107_GPIOCFG_GP1OUT_BIT	(1 << 1) /* GPIO 1 output enable */
-#define MAX3107_GPIOCFG_GP2OUT_BIT	(1 << 2) /* GPIO 2 output enable */
-#define MAX3107_GPIOCFG_GP3OUT_BIT	(1 << 3) /* GPIO 3 output enable */
-#define MAX3107_GPIOCFG_GP0OD_BIT	(1 << 4) /* GPIO 0 open-drain enable */
-#define MAX3107_GPIOCFG_GP1OD_BIT	(1 << 5) /* GPIO 1 open-drain enable */
-#define MAX3107_GPIOCFG_GP2OD_BIT	(1 << 6) /* GPIO 2 open-drain enable */
-#define MAX3107_GPIOCFG_GP3OD_BIT	(1 << 7) /* GPIO 3 open-drain enable */
-
-/* GPIO DATA register bits */
-#define MAX3107_GPIODATA_GP0OUT_BIT	(1 << 0) /* GPIO 0 output value */
-#define MAX3107_GPIODATA_GP1OUT_BIT	(1 << 1) /* GPIO 1 output value */
-#define MAX3107_GPIODATA_GP2OUT_BIT	(1 << 2) /* GPIO 2 output value */
-#define MAX3107_GPIODATA_GP3OUT_BIT	(1 << 3) /* GPIO 3 output value */
-#define MAX3107_GPIODATA_GP0IN_BIT	(1 << 4) /* GPIO 0 input value */
-#define MAX3107_GPIODATA_GP1IN_BIT	(1 << 5) /* GPIO 1 input value */
-#define MAX3107_GPIODATA_GP2IN_BIT	(1 << 6) /* GPIO 2 input value */
-#define MAX3107_GPIODATA_GP3IN_BIT	(1 << 7) /* GPIO 3 input value */
-
-/* PLL configuration register masks */
-#define MAX3107_PLLCFG_PREDIV_MASK	(0x003f) /* PLL predivision value */
-#define MAX3107_PLLCFG_PLLFACTOR_MASK	(0x00c0) /* PLL multiplication factor */
-
-/* Baud rate generator configuration register masks and bits */
-#define MAX3107_BRGCFG_FRACT_MASK	(0x000f) /* Fractional portion of
-						  * Baud rate generator divisor
-						  */
-#define MAX3107_BRGCFG_2XMODE_BIT	(1 << 4) /* Double baud rate */
-#define MAX3107_BRGCFG_4XMODE_BIT	(1 << 5) /* Quadruple baud rate */
-#define MAX3107_BRGCFG_UNDEF6_BIT	(1 << 6) /* Undefined/not used */
-#define MAX3107_BRGCFG_UNDEF7_BIT	(1 << 7) /* Undefined/not used */
-
-/* Clock source register bits */
-#define MAX3107_CLKSRC_INTOSC_BIT	(1 << 0) /* Internal osc enable */
-#define MAX3107_CLKSRC_CRYST_BIT	(1 << 1) /* Crystal osc enable */
-#define MAX3107_CLKSRC_PLL_BIT		(1 << 2) /* PLL enable */
-#define MAX3107_CLKSRC_PLLBYP_BIT	(1 << 3) /* PLL bypass */
-#define MAX3107_CLKSRC_EXTCLK_BIT	(1 << 4) /* External clock enable */
-#define MAX3107_CLKSRC_UNDEF5_BIT	(1 << 5) /* Undefined/not used */
-#define MAX3107_CLKSRC_UNDEF6_BIT	(1 << 6) /* Undefined/not used */
-#define MAX3107_CLKSRC_CLK2RTS_BIT	(1 << 7) /* Baud clk to RTS pin */
-
-
-/* HW definitions */
-#define MAX3107_RX_FIFO_SIZE	128
-#define MAX3107_TX_FIFO_SIZE	128
-#define MAX3107_REVID1		0x00a0
-#define MAX3107_REVID2		0x00a1
-
-
-/* Baud rate generator configuration values for external clock 13MHz */
-#define MAX3107_BRG13_B300	(0x0A9400 | 0x05)
-#define MAX3107_BRG13_B600	(0x054A00 | 0x03)
-#define MAX3107_BRG13_B1200	(0x02A500 | 0x01)
-#define MAX3107_BRG13_B2400	(0x015200 | 0x09)
-#define MAX3107_BRG13_B4800	(0x00A900 | 0x04)
-#define MAX3107_BRG13_B9600	(0x005400 | 0x0A)
-#define MAX3107_BRG13_B19200	(0x002A00 | 0x05)
-#define MAX3107_BRG13_B38400	(0x001500 | 0x03)
-#define MAX3107_BRG13_B57600	(0x000E00 | 0x02)
-#define MAX3107_BRG13_B115200	(0x000700 | 0x01)
-#define MAX3107_BRG13_B230400	(0x000300 | 0x08)
-#define MAX3107_BRG13_B460800	(0x000100 | 0x0c)
-#define MAX3107_BRG13_B921600	(0x000100 | 0x1c)
-
-/* Baud rate generator configuration values for external clock 26MHz */
-#define MAX3107_BRG26_B300	(0x152800 | 0x0A)
-#define MAX3107_BRG26_B600	(0x0A9400 | 0x05)
-#define MAX3107_BRG26_B1200	(0x054A00 | 0x03)
-#define MAX3107_BRG26_B2400	(0x02A500 | 0x01)
-#define MAX3107_BRG26_B4800	(0x015200 | 0x09)
-#define MAX3107_BRG26_B9600	(0x00A900 | 0x04)
-#define MAX3107_BRG26_B19200	(0x005400 | 0x0A)
-#define MAX3107_BRG26_B38400	(0x002A00 | 0x05)
-#define MAX3107_BRG26_B57600	(0x001C00 | 0x03)
-#define MAX3107_BRG26_B115200	(0x000E00 | 0x02)
-#define MAX3107_BRG26_B230400	(0x000700 | 0x01)
-#define MAX3107_BRG26_B460800	(0x000300 | 0x08)
-#define MAX3107_BRG26_B921600	(0x000100 | 0x0C)
-
-/* Baud rate generator configuration values for internal clock */
-#define MAX3107_BRG13_IB300	(0x008000 | 0x00)
-#define MAX3107_BRG13_IB600	(0x004000 | 0x00)
-#define MAX3107_BRG13_IB1200	(0x002000 | 0x00)
-#define MAX3107_BRG13_IB2400	(0x001000 | 0x00)
-#define MAX3107_BRG13_IB4800	(0x000800 | 0x00)
-#define MAX3107_BRG13_IB9600	(0x000400 | 0x00)
-#define MAX3107_BRG13_IB19200	(0x000200 | 0x00)
-#define MAX3107_BRG13_IB38400	(0x000100 | 0x00)
-#define MAX3107_BRG13_IB57600	(0x000000 | 0x0B)
-#define MAX3107_BRG13_IB115200	(0x000000 | 0x05)
-#define MAX3107_BRG13_IB230400	(0x000000 | 0x03)
-#define MAX3107_BRG13_IB460800	(0x000000 | 0x00)
-#define MAX3107_BRG13_IB921600	(0x000000 | 0x00)
-
-
-struct baud_table {
-	int baud;
-	u32 new_brg;
-};
-
-struct max3107_port {
-	/* UART port structure */
-	struct uart_port port;
-
-	/* SPI device structure */
-	struct spi_device *spi;
-
-#if defined(CONFIG_GPIOLIB)
-	/* GPIO chip structure */
-	struct gpio_chip chip;
-#endif
-
-	/* Workqueue that does all the magic */
-	struct workqueue_struct *workqueue;
-	struct work_struct work;
-
-	/* Lock for shared data */
-	spinlock_t data_lock;
-
-	/* Device configuration */
-	int ext_clk;		/* 1 if external clock used */
-	int loopback;		/* Current loopback mode state */
-	int baud;			/* Current baud rate */
-
-	/* State flags */
-	int suspended;		/* Indicates suspend mode */
-	int tx_fifo_empty;	/* Flag for TX FIFO state */
-	int rx_enabled;		/* Flag for receiver state */
-	int tx_enabled;		/* Flag for transmitter state */
-
-	u16 irqen_reg;		/* Current IRQ enable register value */
-	/* Shared data */
-	u16 mode1_reg;		/* Current mode1 register value*/
-	int mode1_commit;	/* Flag for setting new mode1 register value */
-	u16 lcr_reg;		/* Current LCR register value */
-	int lcr_commit;		/* Flag for setting new LCR register value */
-	u32 brg_cfg;		/* Current Baud rate generator config  */
-	int brg_commit;		/* Flag for setting new baud rate generator
-				 * config
-				 */
-	struct baud_table *baud_tbl;
-	int handle_irq;		/* Indicates that IRQ should be handled */
-
-	/* Rx buffer and str*/
-	u16 *rxbuf;
-	u8  *rxstr;
-	/* Tx buffer*/
-	u16 *txbuf;
-
-	struct max3107_plat *pdata;	/* Platform data */
-};
-
-/* Platform data structure */
-struct max3107_plat {
-	/* Loopback mode enable */
-	int loopback;
-	/* External clock enable */
-	int ext_clk;
-	/* Called during the register initialisation */
-	void (*init)(struct max3107_port *s);
-	/* Called when the port is found and configured */
-	int (*configure)(struct max3107_port *s);
-	/* HW suspend function */
-	void (*hw_suspend) (struct max3107_port *s, int suspend);
-	/* Polling mode enable */
-	int polled_mode;
-	/* Polling period if polling mode enabled */
-	int poll_time;
-};
-
-extern int max3107_rw(struct max3107_port *s, u8 *tx, u8 *rx, int len);
-extern void max3107_hw_susp(struct max3107_port *s, int suspend);
-extern int max3107_probe(struct spi_device *spi, struct max3107_plat *pdata);
-extern int max3107_remove(struct spi_device *spi);
-extern int max3107_suspend(struct spi_device *spi, pm_message_t state);
-extern int max3107_resume(struct spi_device *spi);
-
-#endif /* _LINUX_SERIAL_MAX3107_H */
diff --git a/drivers/tty/serial/max310x.c b/drivers/tty/serial/max310x.c
new file mode 100644
index 000000000000..2bc28a59d385
--- /dev/null
+++ b/drivers/tty/serial/max310x.c
@@ -0,0 +1,1260 @@
+/*
+ *  Maxim (Dallas) MAX3107/8 serial driver
+ *
+ *  Copyright (C) 2012 Alexander Shiyan <shc_work@mail.ru>
+ *
+ *  Based on max3100.c, by Christian Pellegrin <chripell@evolware.org>
+ *  Based on max3110.c, by Feng Tang <feng.tang@intel.com>
+ *  Based on max3107.c, by Aavamobile
+ *
+ *  This program is free software; you can redistribute 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.
+ */
+
+/* TODO: MAX3109 support (Dual) */
+/* TODO: MAX14830 support (Quad) */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/regmap.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/platform_data/max310x.h>
+
+#define MAX310X_MAJOR			204
+#define MAX310X_MINOR			209
+
+/* MAX310X register definitions */
+#define MAX310X_RHR_REG			(0x00) /* RX FIFO */
+#define MAX310X_THR_REG			(0x00) /* TX FIFO */
+#define MAX310X_IRQEN_REG		(0x01) /* IRQ enable */
+#define MAX310X_IRQSTS_REG		(0x02) /* IRQ status */
+#define MAX310X_LSR_IRQEN_REG		(0x03) /* LSR IRQ enable */
+#define MAX310X_LSR_IRQSTS_REG		(0x04) /* LSR IRQ status */
+#define MAX310X_SPCHR_IRQEN_REG		(0x05) /* Special char IRQ enable */
+#define MAX310X_SPCHR_IRQSTS_REG	(0x06) /* Special char IRQ status */
+#define MAX310X_STS_IRQEN_REG		(0x07) /* Status IRQ enable */
+#define MAX310X_STS_IRQSTS_REG		(0x08) /* Status IRQ status */
+#define MAX310X_MODE1_REG		(0x09) /* MODE1 */
+#define MAX310X_MODE2_REG		(0x0a) /* MODE2 */
+#define MAX310X_LCR_REG			(0x0b) /* LCR */
+#define MAX310X_RXTO_REG		(0x0c) /* RX timeout */
+#define MAX310X_HDPIXDELAY_REG		(0x0d) /* Auto transceiver delays */
+#define MAX310X_IRDA_REG		(0x0e) /* IRDA settings */
+#define MAX310X_FLOWLVL_REG		(0x0f) /* Flow control levels */
+#define MAX310X_FIFOTRIGLVL_REG		(0x10) /* FIFO IRQ trigger levels */
+#define MAX310X_TXFIFOLVL_REG		(0x11) /* TX FIFO level */
+#define MAX310X_RXFIFOLVL_REG		(0x12) /* RX FIFO level */
+#define MAX310X_FLOWCTRL_REG		(0x13) /* Flow control */
+#define MAX310X_XON1_REG		(0x14) /* XON1 character */
+#define MAX310X_XON2_REG		(0x15) /* XON2 character */
+#define MAX310X_XOFF1_REG		(0x16) /* XOFF1 character */
+#define MAX310X_XOFF2_REG		(0x17) /* XOFF2 character */
+#define MAX310X_GPIOCFG_REG		(0x18) /* GPIO config */
+#define MAX310X_GPIODATA_REG		(0x19) /* GPIO data */
+#define MAX310X_PLLCFG_REG		(0x1a) /* PLL config */
+#define MAX310X_BRGCFG_REG		(0x1b) /* Baud rate generator conf */
+#define MAX310X_BRGDIVLSB_REG		(0x1c) /* Baud rate divisor LSB */
+#define MAX310X_BRGDIVMSB_REG		(0x1d) /* Baud rate divisor MSB */
+#define MAX310X_CLKSRC_REG		(0x1e) /* Clock source */
+/* Only present in MAX3107 */
+#define MAX3107_REVID_REG		(0x1f) /* Revision identification */
+
+/* IRQ register bits */
+#define MAX310X_IRQ_LSR_BIT		(1 << 0) /* LSR interrupt */
+#define MAX310X_IRQ_SPCHR_BIT		(1 << 1) /* Special char interrupt */
+#define MAX310X_IRQ_STS_BIT		(1 << 2) /* Status interrupt */
+#define MAX310X_IRQ_RXFIFO_BIT		(1 << 3) /* RX FIFO interrupt */
+#define MAX310X_IRQ_TXFIFO_BIT		(1 << 4) /* TX FIFO interrupt */
+#define MAX310X_IRQ_TXEMPTY_BIT		(1 << 5) /* TX FIFO empty interrupt */
+#define MAX310X_IRQ_RXEMPTY_BIT		(1 << 6) /* RX FIFO empty interrupt */
+#define MAX310X_IRQ_CTS_BIT		(1 << 7) /* CTS interrupt */
+
+/* LSR register bits */
+#define MAX310X_LSR_RXTO_BIT		(1 << 0) /* RX timeout */
+#define MAX310X_LSR_RXOVR_BIT		(1 << 1) /* RX overrun */
+#define MAX310X_LSR_RXPAR_BIT		(1 << 2) /* RX parity error */
+#define MAX310X_LSR_FRERR_BIT		(1 << 3) /* Frame error */
+#define MAX310X_LSR_RXBRK_BIT		(1 << 4) /* RX break */
+#define MAX310X_LSR_RXNOISE_BIT		(1 << 5) /* RX noise */
+#define MAX310X_LSR_CTS_BIT		(1 << 7) /* CTS pin state */
+
+/* Special character register bits */
+#define MAX310X_SPCHR_XON1_BIT		(1 << 0) /* XON1 character */
+#define MAX310X_SPCHR_XON2_BIT		(1 << 1) /* XON2 character */
+#define MAX310X_SPCHR_XOFF1_BIT		(1 << 2) /* XOFF1 character */
+#define MAX310X_SPCHR_XOFF2_BIT		(1 << 3) /* XOFF2 character */
+#define MAX310X_SPCHR_BREAK_BIT		(1 << 4) /* RX break */
+#define MAX310X_SPCHR_MULTIDROP_BIT	(1 << 5) /* 9-bit multidrop addr char */
+
+/* Status register bits */
+#define MAX310X_STS_GPIO0_BIT		(1 << 0) /* GPIO 0 interrupt */
+#define MAX310X_STS_GPIO1_BIT		(1 << 1) /* GPIO 1 interrupt */
+#define MAX310X_STS_GPIO2_BIT		(1 << 2) /* GPIO 2 interrupt */
+#define MAX310X_STS_GPIO3_BIT		(1 << 3) /* GPIO 3 interrupt */
+#define MAX310X_STS_CLKREADY_BIT	(1 << 5) /* Clock ready */
+#define MAX310X_STS_SLEEP_BIT		(1 << 6) /* Sleep interrupt */
+
+/* MODE1 register bits */
+#define MAX310X_MODE1_RXDIS_BIT		(1 << 0) /* RX disable */
+#define MAX310X_MODE1_TXDIS_BIT		(1 << 1) /* TX disable */
+#define MAX310X_MODE1_TXHIZ_BIT		(1 << 2) /* TX pin three-state */
+#define MAX310X_MODE1_RTSHIZ_BIT	(1 << 3) /* RTS pin three-state */
+#define MAX310X_MODE1_TRNSCVCTRL_BIT	(1 << 4) /* Transceiver ctrl enable */
+#define MAX310X_MODE1_FORCESLEEP_BIT	(1 << 5) /* Force sleep mode */
+#define MAX310X_MODE1_AUTOSLEEP_BIT	(1 << 6) /* Auto sleep enable */
+#define MAX310X_MODE1_IRQSEL_BIT	(1 << 7) /* IRQ pin enable */
+
+/* MODE2 register bits */
+#define MAX310X_MODE2_RST_BIT		(1 << 0) /* Chip reset */
+#define MAX310X_MODE2_FIFORST_BIT	(1 << 1) /* FIFO reset */
+#define MAX310X_MODE2_RXTRIGINV_BIT	(1 << 2) /* RX FIFO INT invert */
+#define MAX310X_MODE2_RXEMPTINV_BIT	(1 << 3) /* RX FIFO empty INT invert */
+#define MAX310X_MODE2_SPCHR_BIT		(1 << 4) /* Special chr detect enable */
+#define MAX310X_MODE2_LOOPBACK_BIT	(1 << 5) /* Internal loopback enable */
+#define MAX310X_MODE2_MULTIDROP_BIT	(1 << 6) /* 9-bit multidrop enable */
+#define MAX310X_MODE2_ECHOSUPR_BIT	(1 << 7) /* ECHO suppression enable */
+
+/* LCR register bits */
+#define MAX310X_LCR_LENGTH0_BIT		(1 << 0) /* Word length bit 0 */
+#define MAX310X_LCR_LENGTH1_BIT		(1 << 1) /* Word length bit 1
+						  *
+						  * Word length bits table:
+						  * 00 -> 5 bit words
+						  * 01 -> 6 bit words
+						  * 10 -> 7 bit words
+						  * 11 -> 8 bit words
+						  */
+#define MAX310X_LCR_STOPLEN_BIT		(1 << 2) /* STOP length bit
+						  *
+						  * STOP length bit table:
+						  * 0 -> 1 stop bit
+						  * 1 -> 1-1.5 stop bits if
+						  *      word length is 5,
+						  *      2 stop bits otherwise
+						  */
+#define MAX310X_LCR_PARITY_BIT		(1 << 3) /* Parity bit enable */
+#define MAX310X_LCR_EVENPARITY_BIT	(1 << 4) /* Even parity bit enable */
+#define MAX310X_LCR_FORCEPARITY_BIT	(1 << 5) /* 9-bit multidrop parity */
+#define MAX310X_LCR_TXBREAK_BIT		(1 << 6) /* TX break enable */
+#define MAX310X_LCR_RTS_BIT		(1 << 7) /* RTS pin control */
+#define MAX310X_LCR_WORD_LEN_5		(0x00)
+#define MAX310X_LCR_WORD_LEN_6		(0x01)
+#define MAX310X_LCR_WORD_LEN_7		(0x02)
+#define MAX310X_LCR_WORD_LEN_8		(0x03)
+
+/* IRDA register bits */
+#define MAX310X_IRDA_IRDAEN_BIT		(1 << 0) /* IRDA mode enable */
+#define MAX310X_IRDA_SIR_BIT		(1 << 1) /* SIR mode enable */
+#define MAX310X_IRDA_SHORTIR_BIT	(1 << 2) /* Short SIR mode enable */
+#define MAX310X_IRDA_MIR_BIT		(1 << 3) /* MIR mode enable */
+#define MAX310X_IRDA_RXINV_BIT		(1 << 4) /* RX logic inversion enable */
+#define MAX310X_IRDA_TXINV_BIT		(1 << 5) /* TX logic inversion enable */
+
+/* Flow control trigger level register masks */
+#define MAX310X_FLOWLVL_HALT_MASK	(0x000f) /* Flow control halt level */
+#define MAX310X_FLOWLVL_RES_MASK	(0x00f0) /* Flow control resume level */
+#define MAX310X_FLOWLVL_HALT(words)	((words / 8) & 0x0f)
+#define MAX310X_FLOWLVL_RES(words)	(((words / 8) & 0x0f) << 4)
+
+/* FIFO interrupt trigger level register masks */
+#define MAX310X_FIFOTRIGLVL_TX_MASK	(0x0f) /* TX FIFO trigger level */
+#define MAX310X_FIFOTRIGLVL_RX_MASK	(0xf0) /* RX FIFO trigger level */
+#define MAX310X_FIFOTRIGLVL_TX(words)	((words / 8) & 0x0f)
+#define MAX310X_FIFOTRIGLVL_RX(words)	(((words / 8) & 0x0f) << 4)
+
+/* Flow control register bits */
+#define MAX310X_FLOWCTRL_AUTORTS_BIT	(1 << 0) /* Auto RTS flow ctrl enable */
+#define MAX310X_FLOWCTRL_AUTOCTS_BIT	(1 << 1) /* Auto CTS flow ctrl enable */
+#define MAX310X_FLOWCTRL_GPIADDR_BIT	(1 << 2) /* Enables that GPIO inputs
+						  * are used in conjunction with
+						  * XOFF2 for definition of
+						  * special character */
+#define MAX310X_FLOWCTRL_SWFLOWEN_BIT	(1 << 3) /* Auto SW flow ctrl enable */
+#define MAX310X_FLOWCTRL_SWFLOW0_BIT	(1 << 4) /* SWFLOW bit 0 */
+#define MAX310X_FLOWCTRL_SWFLOW1_BIT	(1 << 5) /* SWFLOW bit 1
+						  *
+						  * SWFLOW bits 1 & 0 table:
+						  * 00 -> no transmitter flow
+						  *       control
+						  * 01 -> receiver compares
+						  *       XON2 and XOFF2
+						  *       and controls
+						  *       transmitter
+						  * 10 -> receiver compares
+						  *       XON1 and XOFF1
+						  *       and controls
+						  *       transmitter
+						  * 11 -> receiver compares
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2 and controls
+						  *       transmitter
+						  */
+#define MAX310X_FLOWCTRL_SWFLOW2_BIT	(1 << 6) /* SWFLOW bit 2 */
+#define MAX310X_FLOWCTRL_SWFLOW3_BIT	(1 << 7) /* SWFLOW bit 3
+						  *
+						  * SWFLOW bits 3 & 2 table:
+						  * 00 -> no received flow
+						  *       control
+						  * 01 -> transmitter generates
+						  *       XON2 and XOFF2
+						  * 10 -> transmitter generates
+						  *       XON1 and XOFF1
+						  * 11 -> transmitter generates
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2
+						  */
+
+/* GPIO configuration register bits */
+#define MAX310X_GPIOCFG_GP0OUT_BIT	(1 << 0) /* GPIO 0 output enable */
+#define MAX310X_GPIOCFG_GP1OUT_BIT	(1 << 1) /* GPIO 1 output enable */
+#define MAX310X_GPIOCFG_GP2OUT_BIT	(1 << 2) /* GPIO 2 output enable */
+#define MAX310X_GPIOCFG_GP3OUT_BIT	(1 << 3) /* GPIO 3 output enable */
+#define MAX310X_GPIOCFG_GP0OD_BIT	(1 << 4) /* GPIO 0 open-drain enable */
+#define MAX310X_GPIOCFG_GP1OD_BIT	(1 << 5) /* GPIO 1 open-drain enable */
+#define MAX310X_GPIOCFG_GP2OD_BIT	(1 << 6) /* GPIO 2 open-drain enable */
+#define MAX310X_GPIOCFG_GP3OD_BIT	(1 << 7) /* GPIO 3 open-drain enable */
+
+/* GPIO DATA register bits */
+#define MAX310X_GPIODATA_GP0OUT_BIT	(1 << 0) /* GPIO 0 output value */
+#define MAX310X_GPIODATA_GP1OUT_BIT	(1 << 1) /* GPIO 1 output value */
+#define MAX310X_GPIODATA_GP2OUT_BIT	(1 << 2) /* GPIO 2 output value */
+#define MAX310X_GPIODATA_GP3OUT_BIT	(1 << 3) /* GPIO 3 output value */
+#define MAX310X_GPIODATA_GP0IN_BIT	(1 << 4) /* GPIO 0 input value */
+#define MAX310X_GPIODATA_GP1IN_BIT	(1 << 5) /* GPIO 1 input value */
+#define MAX310X_GPIODATA_GP2IN_BIT	(1 << 6) /* GPIO 2 input value */
+#define MAX310X_GPIODATA_GP3IN_BIT	(1 << 7) /* GPIO 3 input value */
+
+/* PLL configuration register masks */
+#define MAX310X_PLLCFG_PREDIV_MASK	(0x3f) /* PLL predivision value */
+#define MAX310X_PLLCFG_PLLFACTOR_MASK	(0xc0) /* PLL multiplication factor */
+
+/* Baud rate generator configuration register bits */
+#define MAX310X_BRGCFG_2XMODE_BIT	(1 << 4) /* Double baud rate */
+#define MAX310X_BRGCFG_4XMODE_BIT	(1 << 5) /* Quadruple baud rate */
+
+/* Clock source register bits */
+#define MAX310X_CLKSRC_CRYST_BIT	(1 << 1) /* Crystal osc enable */
+#define MAX310X_CLKSRC_PLL_BIT		(1 << 2) /* PLL enable */
+#define MAX310X_CLKSRC_PLLBYP_BIT	(1 << 3) /* PLL bypass */
+#define MAX310X_CLKSRC_EXTCLK_BIT	(1 << 4) /* External clock enable */
+#define MAX310X_CLKSRC_CLK2RTS_BIT	(1 << 7) /* Baud clk to RTS pin */
+
+/* Misc definitions */
+#define MAX310X_FIFO_SIZE		(128)
+
+/* MAX3107 specific */
+#define MAX3107_REV_ID			(0xa0)
+#define MAX3107_REV_MASK		(0xfe)
+
+/* IRQ status bits definitions */
+#define MAX310X_IRQ_TX			(MAX310X_IRQ_TXFIFO_BIT | \
+					 MAX310X_IRQ_TXEMPTY_BIT)
+#define MAX310X_IRQ_RX			(MAX310X_IRQ_RXFIFO_BIT | \
+					 MAX310X_IRQ_RXEMPTY_BIT)
+
+/* Supported chip types */
+enum {
+	MAX310X_TYPE_MAX3107	= 3107,
+	MAX310X_TYPE_MAX3108	= 3108,
+};
+
+struct max310x_port {
+	struct uart_driver	uart;
+	struct uart_port	port;
+
+	const char		*name;
+	int			uartclk;
+
+	unsigned int		nr_gpio;
+#ifdef CONFIG_GPIOLIB
+	struct gpio_chip	gpio;
+#endif
+
+	struct regmap		*regmap;
+	struct regmap_config	regcfg;
+
+	struct workqueue_struct	*wq;
+	struct work_struct	tx_work;
+
+	struct mutex		max310x_mutex;
+
+	struct max310x_pdata	*pdata;
+};
+
+static bool max3107_8_reg_writeable(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX310X_IRQSTS_REG:
+	case MAX310X_LSR_IRQSTS_REG:
+	case MAX310X_SPCHR_IRQSTS_REG:
+	case MAX310X_STS_IRQSTS_REG:
+	case MAX310X_TXFIFOLVL_REG:
+	case MAX310X_RXFIFOLVL_REG:
+	case MAX3107_REVID_REG: /* Only available on MAX3107 */
+		return false;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool max310x_reg_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX310X_RHR_REG:
+	case MAX310X_IRQSTS_REG:
+	case MAX310X_LSR_IRQSTS_REG:
+	case MAX310X_SPCHR_IRQSTS_REG:
+	case MAX310X_STS_IRQSTS_REG:
+	case MAX310X_TXFIFOLVL_REG:
+	case MAX310X_RXFIFOLVL_REG:
+	case MAX310X_GPIODATA_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static bool max310x_reg_precious(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX310X_RHR_REG:
+	case MAX310X_IRQSTS_REG:
+	case MAX310X_SPCHR_IRQSTS_REG:
+	case MAX310X_STS_IRQSTS_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static void max310x_set_baud(struct max310x_port *s, int baud)
+{
+	unsigned int mode = 0, div = s->uartclk / baud;
+
+	if (!(div / 16)) {
+		/* Mode x2 */
+		mode = MAX310X_BRGCFG_2XMODE_BIT;
+		div = (s->uartclk * 2) / baud;
+	}
+
+	if (!(div / 16)) {
+		/* Mode x4 */
+		mode = MAX310X_BRGCFG_4XMODE_BIT;
+		div = (s->uartclk * 4) / baud;
+	}
+
+	regmap_write(s->regmap, MAX310X_BRGDIVMSB_REG,
+		     ((div / 16) >> 8) & 0xff);
+	regmap_write(s->regmap, MAX310X_BRGDIVLSB_REG, (div / 16) & 0xff);
+	regmap_write(s->regmap, MAX310X_BRGCFG_REG, (div % 16) | mode);
+}
+
+static void max310x_wait_pll(struct max310x_port *s)
+{
+	int tryes = 1000;
+
+	/* Wait for PLL only if crystal is used */
+	if (!(s->pdata->driver_flags & MAX310X_EXT_CLK)) {
+		unsigned int sts = 0;
+
+		while (tryes--) {
+			regmap_read(s->regmap, MAX310X_STS_IRQSTS_REG, &sts);
+			if (sts & MAX310X_STS_CLKREADY_BIT)
+				break;
+		}
+	}
+}
+
+static int __devinit max310x_update_best_err(unsigned long f, long *besterr)
+{
+	/* Use baudrate 115200 for calculate error */
+	long err = f % (115200 * 16);
+
+	if ((*besterr < 0) || (*besterr > err)) {
+		*besterr = err;
+		return 0;
+	}
+
+	return 1;
+}
+
+static int __devinit max310x_set_ref_clk(struct max310x_port *s)
+{
+	unsigned int div, clksrc, pllcfg = 0;
+	long besterr = -1;
+	unsigned long fdiv, fmul, bestfreq = s->pdata->frequency;
+
+	/* First, update error without PLL */
+	max310x_update_best_err(s->pdata->frequency, &besterr);
+
+	/* Try all possible PLL dividers */
+	for (div = 1; (div <= 63) && besterr; div++) {
+		fdiv = DIV_ROUND_CLOSEST(s->pdata->frequency, div);
+
+		/* Try multiplier 6 */
+		fmul = fdiv * 6;
+		if ((fdiv >= 500000) && (fdiv <= 800000))
+			if (!max310x_update_best_err(fmul, &besterr)) {
+				pllcfg = (0 << 6) | div;
+				bestfreq = fmul;
+			}
+		/* Try multiplier 48 */
+		fmul = fdiv * 48;
+		if ((fdiv >= 850000) && (fdiv <= 1200000))
+			if (!max310x_update_best_err(fmul, &besterr)) {
+				pllcfg = (1 << 6) | div;
+				bestfreq = fmul;
+			}
+		/* Try multiplier 96 */
+		fmul = fdiv * 96;
+		if ((fdiv >= 425000) && (fdiv <= 1000000))
+			if (!max310x_update_best_err(fmul, &besterr)) {
+				pllcfg = (2 << 6) | div;
+				bestfreq = fmul;
+			}
+		/* Try multiplier 144 */
+		fmul = fdiv * 144;
+		if ((fdiv >= 390000) && (fdiv <= 667000))
+			if (!max310x_update_best_err(fmul, &besterr)) {
+				pllcfg = (3 << 6) | div;
+				bestfreq = fmul;
+			}
+	}
+
+	/* Configure clock source */
+	if (s->pdata->driver_flags & MAX310X_EXT_CLK)
+		clksrc = MAX310X_CLKSRC_EXTCLK_BIT;
+	else
+		clksrc = MAX310X_CLKSRC_CRYST_BIT;
+
+	/* Configure PLL */
+	if (pllcfg) {
+		clksrc |= MAX310X_CLKSRC_PLL_BIT;
+		regmap_write(s->regmap, MAX310X_PLLCFG_REG, pllcfg);
+	} else
+		clksrc |= MAX310X_CLKSRC_PLLBYP_BIT;
+
+	regmap_write(s->regmap, MAX310X_CLKSRC_REG, clksrc);
+
+	if (pllcfg)
+		max310x_wait_pll(s);
+
+	dev_dbg(s->port.dev, "Reference clock set to %lu Hz\n", bestfreq);
+
+	return (int)bestfreq;
+}
+
+static void max310x_handle_rx(struct max310x_port *s, unsigned int rxlen)
+{
+	unsigned int sts = 0, ch = 0, flag;
+	struct tty_struct *tty = tty_port_tty_get(&s->port.state->port);
+
+	if (!tty)
+		return;
+
+	if (unlikely(rxlen >= MAX310X_FIFO_SIZE)) {
+		dev_warn(s->port.dev, "Possible RX FIFO overrun %d\n", rxlen);
+		/* Ensure sanity of RX level */
+		rxlen = MAX310X_FIFO_SIZE;
+	}
+
+	dev_dbg(s->port.dev, "RX Len = %u\n", rxlen);
+
+	while (rxlen--) {
+		regmap_read(s->regmap, MAX310X_RHR_REG, &ch);
+		regmap_read(s->regmap, MAX310X_LSR_IRQSTS_REG, &sts);
+
+		sts &= MAX310X_LSR_RXPAR_BIT | MAX310X_LSR_FRERR_BIT |
+		       MAX310X_LSR_RXOVR_BIT | MAX310X_LSR_RXBRK_BIT;
+
+		s->port.icount.rx++;
+		flag = TTY_NORMAL;
+
+		if (unlikely(sts)) {
+			if (sts & MAX310X_LSR_RXBRK_BIT) {
+				s->port.icount.brk++;
+				if (uart_handle_break(&s->port))
+					continue;
+			} else if (sts & MAX310X_LSR_RXPAR_BIT)
+				s->port.icount.parity++;
+			else if (sts & MAX310X_LSR_FRERR_BIT)
+				s->port.icount.frame++;
+			else if (sts & MAX310X_LSR_RXOVR_BIT)
+				s->port.icount.overrun++;
+
+			sts &= s->port.read_status_mask;
+			if (sts & MAX310X_LSR_RXBRK_BIT)
+				flag = TTY_BREAK;
+			else if (sts & MAX310X_LSR_RXPAR_BIT)
+				flag = TTY_PARITY;
+			else if (sts & MAX310X_LSR_FRERR_BIT)
+				flag = TTY_FRAME;
+			else if (sts & MAX310X_LSR_RXOVR_BIT)
+				flag = TTY_OVERRUN;
+		}
+
+		if (uart_handle_sysrq_char(s->port, ch))
+			continue;
+
+		if (sts & s->port.ignore_status_mask)
+			continue;
+
+		uart_insert_char(&s->port, sts, MAX310X_LSR_RXOVR_BIT,
+				 ch, flag);
+	}
+
+	tty_flip_buffer_push(tty);
+
+	tty_kref_put(tty);
+}
+
+static void max310x_handle_tx(struct max310x_port *s)
+{
+	struct circ_buf *xmit = &s->port.state->xmit;
+	unsigned int txlen = 0, to_send;
+
+	if (unlikely(s->port.x_char)) {
+		regmap_write(s->regmap, MAX310X_THR_REG, s->port.x_char);
+		s->port.icount.tx++;
+		s->port.x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&s->port))
+		return;
+
+	/* Get length of data pending in circular buffer */
+	to_send = uart_circ_chars_pending(xmit);
+	if (likely(to_send)) {
+		/* Limit to size of TX FIFO */
+		regmap_read(s->regmap, MAX310X_TXFIFOLVL_REG, &txlen);
+		txlen = MAX310X_FIFO_SIZE - txlen;
+		to_send = (to_send > txlen) ? txlen : to_send;
+
+		dev_dbg(s->port.dev, "TX Len = %u\n", to_send);
+
+		/* Add data to send */
+		s->port.icount.tx += to_send;
+		while (to_send--) {
+			regmap_write(s->regmap, MAX310X_THR_REG,
+				     xmit->buf[xmit->tail]);
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		};
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&s->port);
+}
+
+static irqreturn_t max310x_ist(int irq, void *dev_id)
+{
+	struct max310x_port *s = (struct max310x_port *)dev_id;
+	unsigned int ists = 0, lsr = 0, rxlen = 0;
+
+	mutex_lock(&s->max310x_mutex);
+
+	for (;;) {
+		/* Read IRQ status & RX FIFO level */
+		regmap_read(s->regmap, MAX310X_IRQSTS_REG, &ists);
+		regmap_read(s->regmap, MAX310X_LSR_IRQSTS_REG, &lsr);
+		regmap_read(s->regmap, MAX310X_RXFIFOLVL_REG, &rxlen);
+		if (!ists && !(lsr & MAX310X_LSR_RXTO_BIT) && !rxlen)
+			break;
+
+		dev_dbg(s->port.dev, "IRQ status: 0x%02x\n", ists);
+
+		if (rxlen)
+			max310x_handle_rx(s, rxlen);
+		if (ists & MAX310X_IRQ_TX)
+			max310x_handle_tx(s);
+		if (ists & MAX310X_IRQ_CTS_BIT)
+			uart_handle_cts_change(&s->port,
+					       !!(lsr & MAX310X_LSR_CTS_BIT));
+	}
+
+	mutex_unlock(&s->max310x_mutex);
+
+	return IRQ_HANDLED;
+}
+
+static void max310x_wq_proc(struct work_struct *ws)
+{
+	struct max310x_port *s = container_of(ws, struct max310x_port, tx_work);
+
+	mutex_lock(&s->max310x_mutex);
+	max310x_handle_tx(s);
+	mutex_unlock(&s->max310x_mutex);
+}
+
+static void max310x_start_tx(struct uart_port *port)
+{
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	queue_work(s->wq, &s->tx_work);
+}
+
+static void max310x_stop_tx(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static void max310x_stop_rx(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static unsigned int max310x_tx_empty(struct uart_port *port)
+{
+	unsigned int val = 0;
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	mutex_lock(&s->max310x_mutex);
+	regmap_read(s->regmap, MAX310X_TXFIFOLVL_REG, &val);
+	mutex_unlock(&s->max310x_mutex);
+
+	return val ? 0 : TIOCSER_TEMT;
+}
+
+static void max310x_enable_ms(struct uart_port *port)
+{
+	/* Modem status not supported */
+}
+
+static unsigned int max310x_get_mctrl(struct uart_port *port)
+{
+	/* DCD and DSR are not wired and CTS/RTS is handled automatically
+	 * so just indicate DSR and CAR asserted
+	 */
+	return TIOCM_DSR | TIOCM_CAR;
+}
+
+static void max310x_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	/* DCD and DSR are not wired and CTS/RTS is hadnled automatically
+	 * so do nothing
+	 */
+}
+
+static void max310x_break_ctl(struct uart_port *port, int break_state)
+{
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	mutex_lock(&s->max310x_mutex);
+	regmap_update_bits(s->regmap, MAX310X_LCR_REG,
+			   MAX310X_LCR_TXBREAK_BIT,
+			   break_state ? MAX310X_LCR_TXBREAK_BIT : 0);
+	mutex_unlock(&s->max310x_mutex);
+}
+
+static void max310x_set_termios(struct uart_port *port,
+				struct ktermios *termios,
+				struct ktermios *old)
+{
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+	unsigned int lcr, flow = 0;
+	int baud;
+
+	mutex_lock(&s->max310x_mutex);
+
+	/* Mask termios capabilities we don't support */
+	termios->c_cflag &= ~CMSPAR;
+	termios->c_iflag &= ~IXANY;
+
+	/* Word size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr = MAX310X_LCR_WORD_LEN_5;
+		break;
+	case CS6:
+		lcr = MAX310X_LCR_WORD_LEN_6;
+		break;
+	case CS7:
+		lcr = MAX310X_LCR_WORD_LEN_7;
+		break;
+	case CS8:
+	default:
+		lcr = MAX310X_LCR_WORD_LEN_8;
+		break;
+	}
+
+	/* Parity */
+	if (termios->c_cflag & PARENB) {
+		lcr |= MAX310X_LCR_PARITY_BIT;
+		if (!(termios->c_cflag & PARODD))
+			lcr |= MAX310X_LCR_EVENPARITY_BIT;
+	}
+
+	/* Stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= MAX310X_LCR_STOPLEN_BIT; /* 2 stops */
+
+	/* Update LCR register */
+	regmap_write(s->regmap, MAX310X_LCR_REG, lcr);
+
+	/* Set read status mask */
+	port->read_status_mask = MAX310X_LSR_RXOVR_BIT;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= MAX310X_LSR_RXPAR_BIT |
+					  MAX310X_LSR_FRERR_BIT;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= MAX310X_LSR_RXBRK_BIT;
+
+	/* Set status ignore mask */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNBRK)
+		port->ignore_status_mask |= MAX310X_LSR_RXBRK_BIT;
+	if (!(termios->c_cflag & CREAD))
+		port->ignore_status_mask |= MAX310X_LSR_RXPAR_BIT |
+					    MAX310X_LSR_RXOVR_BIT |
+					    MAX310X_LSR_FRERR_BIT |
+					    MAX310X_LSR_RXBRK_BIT;
+
+	/* Configure flow control */
+	regmap_write(s->regmap, MAX310X_XON1_REG, termios->c_cc[VSTART]);
+	regmap_write(s->regmap, MAX310X_XOFF1_REG, termios->c_cc[VSTOP]);
+	if (termios->c_cflag & CRTSCTS)
+		flow |= MAX310X_FLOWCTRL_AUTOCTS_BIT |
+			MAX310X_FLOWCTRL_AUTORTS_BIT;
+	if (termios->c_iflag & IXON)
+		flow |= MAX310X_FLOWCTRL_SWFLOW3_BIT |
+			MAX310X_FLOWCTRL_SWFLOWEN_BIT;
+	if (termios->c_iflag & IXOFF)
+		flow |= MAX310X_FLOWCTRL_SWFLOW1_BIT |
+			MAX310X_FLOWCTRL_SWFLOWEN_BIT;
+	regmap_write(s->regmap, MAX310X_FLOWCTRL_REG, flow);
+
+	/* Get baud rate generator configuration */
+	baud = uart_get_baud_rate(port, termios, old,
+				  port->uartclk / 16 / 0xffff,
+				  port->uartclk / 4);
+
+	/* Setup baudrate generator */
+	max310x_set_baud(s, baud);
+
+	/* Update timeout according to new baud rate */
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	mutex_unlock(&s->max310x_mutex);
+}
+
+static int max310x_startup(struct uart_port *port)
+{
+	unsigned int val, line = port->line;
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	if (s->pdata->suspend)
+		s->pdata->suspend(0);
+
+	mutex_lock(&s->max310x_mutex);
+
+	/* Configure baud rate, 9600 as default */
+	max310x_set_baud(s, 9600);
+
+	/* Configure LCR register, 8N1 mode by default */
+	val = MAX310X_LCR_WORD_LEN_8;
+	regmap_write(s->regmap, MAX310X_LCR_REG, val);
+
+	/* Configure MODE1 register */
+	regmap_update_bits(s->regmap, MAX310X_MODE1_REG,
+			   MAX310X_MODE1_TRNSCVCTRL_BIT,
+			   (s->pdata->uart_flags[line] & MAX310X_AUTO_DIR_CTRL)
+			   ? MAX310X_MODE1_TRNSCVCTRL_BIT : 0);
+
+	/* Configure MODE2 register */
+	val = MAX310X_MODE2_RXEMPTINV_BIT;
+	if (s->pdata->uart_flags[line] & MAX310X_LOOPBACK)
+		val |= MAX310X_MODE2_LOOPBACK_BIT;
+	if (s->pdata->uart_flags[line] & MAX310X_ECHO_SUPRESS)
+		val |= MAX310X_MODE2_ECHOSUPR_BIT;
+
+	/* Reset FIFOs */
+	val |= MAX310X_MODE2_FIFORST_BIT;
+	regmap_write(s->regmap, MAX310X_MODE2_REG, val);
+
+	/* Configure FIFO trigger level register */
+	/* RX FIFO trigger for 16 words, TX FIFO trigger for 64 words */
+	val = MAX310X_FIFOTRIGLVL_RX(16) | MAX310X_FIFOTRIGLVL_TX(64);
+	regmap_write(s->regmap, MAX310X_FIFOTRIGLVL_REG, val);
+
+	/* Configure flow control levels */
+	/* Flow control halt level 96, resume level 48 */
+	val = MAX310X_FLOWLVL_RES(48) | MAX310X_FLOWLVL_HALT(96);
+	regmap_write(s->regmap, MAX310X_FLOWLVL_REG, val);
+
+	/* Clear timeout register */
+	regmap_write(s->regmap, MAX310X_RXTO_REG, 0);
+
+	/* Configure LSR interrupt enable register */
+	/* Enable RX timeout interrupt */
+	val = MAX310X_LSR_RXTO_BIT;
+	regmap_write(s->regmap, MAX310X_LSR_IRQEN_REG, val);
+
+	/* Clear FIFO reset */
+	regmap_update_bits(s->regmap, MAX310X_MODE2_REG,
+			   MAX310X_MODE2_FIFORST_BIT, 0);
+
+	/* Clear IRQ status register by reading it */
+	regmap_read(s->regmap, MAX310X_IRQSTS_REG, &val);
+
+	/* Configure interrupt enable register */
+	/* Enable CTS change interrupt */
+	val = MAX310X_IRQ_CTS_BIT;
+	/* Enable RX, TX interrupts */
+	val |= MAX310X_IRQ_RX | MAX310X_IRQ_TX;
+	regmap_write(s->regmap, MAX310X_IRQEN_REG, val);
+
+	mutex_unlock(&s->max310x_mutex);
+
+	return 0;
+}
+
+static void max310x_shutdown(struct uart_port *port)
+{
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	/* Disable all interrupts */
+	mutex_lock(&s->max310x_mutex);
+	regmap_write(s->regmap, MAX310X_IRQEN_REG, 0);
+	mutex_unlock(&s->max310x_mutex);
+
+	if (s->pdata->suspend)
+		s->pdata->suspend(1);
+}
+
+static const char *max310x_type(struct uart_port *port)
+{
+	struct max310x_port *s = container_of(port, struct max310x_port, port);
+
+	return (port->type == PORT_MAX310X) ? s->name : NULL;
+}
+
+static int max310x_request_port(struct uart_port *port)
+{
+	/* Do nothing */
+	return 0;
+}
+
+static void max310x_release_port(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static void max310x_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		port->type = PORT_MAX310X;
+}
+
+static int max310x_verify_port(struct uart_port *port, struct serial_struct *ser)
+{
+	if ((ser->type == PORT_UNKNOWN) || (ser->type == PORT_MAX310X))
+		return 0;
+	if (ser->irq == port->irq)
+		return 0;
+
+	return -EINVAL;
+}
+
+static struct uart_ops max310x_ops = {
+	.tx_empty	= max310x_tx_empty,
+	.set_mctrl	= max310x_set_mctrl,
+	.get_mctrl	= max310x_get_mctrl,
+	.stop_tx	= max310x_stop_tx,
+	.start_tx	= max310x_start_tx,
+	.stop_rx	= max310x_stop_rx,
+	.enable_ms	= max310x_enable_ms,
+	.break_ctl	= max310x_break_ctl,
+	.startup	= max310x_startup,
+	.shutdown	= max310x_shutdown,
+	.set_termios	= max310x_set_termios,
+	.type		= max310x_type,
+	.request_port	= max310x_request_port,
+	.release_port	= max310x_release_port,
+	.config_port	= max310x_config_port,
+	.verify_port	= max310x_verify_port,
+};
+
+static int max310x_suspend(struct spi_device *spi, pm_message_t state)
+{
+	int ret;
+	struct max310x_port *s = dev_get_drvdata(&spi->dev);
+
+	dev_dbg(&spi->dev, "Suspend\n");
+
+	ret = uart_suspend_port(&s->uart, &s->port);
+
+	mutex_lock(&s->max310x_mutex);
+
+	/* Enable sleep mode */
+	regmap_update_bits(s->regmap, MAX310X_MODE1_REG,
+			   MAX310X_MODE1_FORCESLEEP_BIT,
+			   MAX310X_MODE1_FORCESLEEP_BIT);
+
+	mutex_unlock(&s->max310x_mutex);
+
+	if (s->pdata->suspend)
+		s->pdata->suspend(1);
+
+	return ret;
+}
+
+static int max310x_resume(struct spi_device *spi)
+{
+	struct max310x_port *s = dev_get_drvdata(&spi->dev);
+
+	dev_dbg(&spi->dev, "Resume\n");
+
+	if (s->pdata->suspend)
+		s->pdata->suspend(0);
+
+	mutex_lock(&s->max310x_mutex);
+
+	/* Disable sleep mode */
+	regmap_update_bits(s->regmap, MAX310X_MODE1_REG,
+			   MAX310X_MODE1_FORCESLEEP_BIT,
+			   0);
+
+	max310x_wait_pll(s);
+
+	mutex_unlock(&s->max310x_mutex);
+
+	return uart_resume_port(&s->uart, &s->port);
+}
+
+#ifdef CONFIG_GPIOLIB
+static int max310x_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	unsigned int val = 0;
+	struct max310x_port *s = container_of(chip, struct max310x_port, gpio);
+
+	mutex_lock(&s->max310x_mutex);
+	regmap_read(s->regmap, MAX310X_GPIODATA_REG, &val);
+	mutex_unlock(&s->max310x_mutex);
+
+	return !!((val >> 4) & (1 << offset));
+}
+
+static void max310x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct max310x_port *s = container_of(chip, struct max310x_port, gpio);
+
+	mutex_lock(&s->max310x_mutex);
+	regmap_update_bits(s->regmap, MAX310X_GPIODATA_REG, 1 << offset, value ?
+							    1 << offset : 0);
+	mutex_unlock(&s->max310x_mutex);
+}
+
+static int max310x_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	struct max310x_port *s = container_of(chip, struct max310x_port, gpio);
+
+	mutex_lock(&s->max310x_mutex);
+
+	regmap_update_bits(s->regmap, MAX310X_GPIOCFG_REG, 1 << offset, 0);
+
+	mutex_unlock(&s->max310x_mutex);
+
+	return 0;
+}
+
+static int max310x_gpio_direction_output(struct gpio_chip *chip,
+					 unsigned offset, int value)
+{
+	struct max310x_port *s = container_of(chip, struct max310x_port, gpio);
+
+	mutex_lock(&s->max310x_mutex);
+
+	regmap_update_bits(s->regmap, MAX310X_GPIOCFG_REG, 1 << offset,
+							   1 << offset);
+	regmap_update_bits(s->regmap, MAX310X_GPIODATA_REG, 1 << offset, value ?
+							    1 << offset : 0);
+
+	mutex_unlock(&s->max310x_mutex);
+
+	return 0;
+}
+#endif
+
+/* Generic platform data */
+static struct max310x_pdata generic_plat_data = {
+	.driver_flags	= MAX310X_EXT_CLK,
+	.uart_flags[0]	= MAX310X_ECHO_SUPRESS,
+	.frequency	= 26000000,
+};
+
+static int __devinit max310x_probe(struct spi_device *spi)
+{
+	struct max310x_port *s;
+	struct device *dev = &spi->dev;
+	int chiptype = spi_get_device_id(spi)->driver_data;
+	struct max310x_pdata *pdata = dev->platform_data;
+	unsigned int val = 0;
+	int ret;
+
+	/* Check for IRQ */
+	if (spi->irq <= 0) {
+		dev_err(dev, "No IRQ specified\n");
+		return -ENOTSUPP;
+	}
+
+	/* Alloc port structure */
+	s = devm_kzalloc(dev, sizeof(struct max310x_port), GFP_KERNEL);
+	if (!s) {
+		dev_err(dev, "Error allocating port structure\n");
+		return -ENOMEM;
+	}
+	dev_set_drvdata(dev, s);
+
+	if (!pdata) {
+		dev_warn(dev, "No platform data supplied, using defaults\n");
+		pdata = &generic_plat_data;
+	}
+	s->pdata = pdata;
+
+	/* Individual chip settings */
+	switch (chiptype) {
+	case MAX310X_TYPE_MAX3107:
+		s->name = "MAX3107";
+		s->nr_gpio = 4;
+		s->uart.nr = 1;
+		s->regcfg.max_register = 0x1f;
+		break;
+	case MAX310X_TYPE_MAX3108:
+		s->name = "MAX3108";
+		s->nr_gpio = 4;
+		s->uart.nr = 1;
+		s->regcfg.max_register = 0x1e;
+		break;
+	default:
+		dev_err(dev, "Unsupported chip type %i\n", chiptype);
+		return -ENOTSUPP;
+	}
+
+	/* Check input frequency */
+	if ((pdata->driver_flags & MAX310X_EXT_CLK) &&
+	   ((pdata->frequency < 500000) || (pdata->frequency > 35000000)))
+		goto err_freq;
+	/* Check frequency for quartz */
+	if (!(pdata->driver_flags & MAX310X_EXT_CLK) &&
+	   ((pdata->frequency < 1000000) || (pdata->frequency > 4000000)))
+		goto err_freq;
+
+	mutex_init(&s->max310x_mutex);
+
+	/* Setup SPI bus */
+	spi->mode		= SPI_MODE_0;
+	spi->bits_per_word	= 8;
+	spi->max_speed_hz	= 26000000;
+	spi_setup(spi);
+
+	/* Setup regmap */
+	s->regcfg.reg_bits		= 8;
+	s->regcfg.val_bits		= 8;
+	s->regcfg.read_flag_mask	= 0x00;
+	s->regcfg.write_flag_mask	= 0x80;
+	s->regcfg.cache_type		= REGCACHE_RBTREE;
+	s->regcfg.writeable_reg		= max3107_8_reg_writeable;
+	s->regcfg.volatile_reg		= max310x_reg_volatile;
+	s->regcfg.precious_reg		= max310x_reg_precious;
+	s->regmap = devm_regmap_init_spi(spi, &s->regcfg);
+	if (IS_ERR(s->regmap)) {
+		ret = PTR_ERR(s->regmap);
+		dev_err(dev, "Failed to initialize register map\n");
+		goto err_out;
+	}
+
+	/* Reset chip & check SPI function */
+	ret = regmap_write(s->regmap, MAX310X_MODE2_REG, MAX310X_MODE2_RST_BIT);
+	if (ret) {
+		dev_err(dev, "SPI transfer failed\n");
+		goto err_out;
+	}
+	/* Clear chip reset */
+	regmap_write(s->regmap, MAX310X_MODE2_REG, 0);
+
+	switch (chiptype) {
+	case MAX310X_TYPE_MAX3107:
+		/* Check REV ID to ensure we are talking to what we expect */
+		regmap_read(s->regmap, MAX3107_REVID_REG, &val);
+		if (((val & MAX3107_REV_MASK) != MAX3107_REV_ID)) {
+			dev_err(dev, "%s ID 0x%02x does not match\n",
+				s->name, val);
+			ret = -ENODEV;
+			goto err_out;
+		}
+		break;
+	case MAX310X_TYPE_MAX3108:
+		/* MAX3108 have not REV ID register, we just check default value
+		 * from clocksource register to make sure everything works.
+		 */
+		regmap_read(s->regmap, MAX310X_CLKSRC_REG, &val);
+		if (val != (MAX310X_CLKSRC_EXTCLK_BIT |
+			    MAX310X_CLKSRC_PLLBYP_BIT)) {
+			dev_err(dev, "%s not present\n", s->name);
+			ret = -ENODEV;
+			goto err_out;
+		}
+		break;
+	}
+
+	/* Board specific configure */
+	if (pdata->init)
+		pdata->init();
+	if (pdata->suspend)
+		pdata->suspend(0);
+
+	/* Calculate referecne clock */
+	s->uartclk = max310x_set_ref_clk(s);
+
+	/* Disable all interrupts */
+	regmap_write(s->regmap, MAX310X_IRQEN_REG, 0);
+
+	/* Setup MODE1 register */
+	val = MAX310X_MODE1_IRQSEL_BIT; /* Enable IRQ pin */
+	if (pdata->driver_flags & MAX310X_AUTOSLEEP)
+		val = MAX310X_MODE1_AUTOSLEEP_BIT;
+	regmap_write(s->regmap, MAX310X_MODE1_REG, val);
+
+	/* Setup interrupt */
+	ret = devm_request_threaded_irq(dev, spi->irq, NULL, max310x_ist,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					dev_name(dev), s);
+	if (ret) {
+		dev_err(dev, "Unable to reguest IRQ %i\n", spi->irq);
+		goto err_out;
+	}
+
+	/* Register UART driver */
+	s->uart.owner		= THIS_MODULE;
+	s->uart.driver_name	= dev_name(dev);
+	s->uart.dev_name	= "ttyMAX";
+	s->uart.major		= MAX310X_MAJOR;
+	s->uart.minor		= MAX310X_MINOR;
+	ret = uart_register_driver(&s->uart);
+	if (ret) {
+		dev_err(dev, "Registering UART driver failed\n");
+		goto err_out;
+	}
+
+	/* Initialize workqueue for start TX */
+	s->wq = create_freezable_workqueue(dev_name(dev));
+	INIT_WORK(&s->tx_work, max310x_wq_proc);
+
+	/* Initialize UART port data */
+	s->port.line		= 0;
+	s->port.dev		= dev;
+	s->port.irq		= spi->irq;
+	s->port.type		= PORT_MAX310X;
+	s->port.fifosize	= MAX310X_FIFO_SIZE;
+	s->port.flags		= UPF_SKIP_TEST | UPF_FIXED_TYPE;
+	s->port.iotype		= UPIO_PORT;
+	s->port.membase		= (void __iomem *)0xffffffff; /* Bogus value */
+	s->port.uartclk		= s->uartclk;
+	s->port.ops		= &max310x_ops;
+	uart_add_one_port(&s->uart, &s->port);
+
+#ifdef CONFIG_GPIOLIB
+	/* Setup GPIO cotroller */
+	if (pdata->gpio_base) {
+		s->gpio.owner		= THIS_MODULE;
+		s->gpio.dev		= dev;
+		s->gpio.label		= dev_name(dev);
+		s->gpio.direction_input	= max310x_gpio_direction_input;
+		s->gpio.get		= max310x_gpio_get;
+		s->gpio.direction_output= max310x_gpio_direction_output;
+		s->gpio.set		= max310x_gpio_set;
+		s->gpio.base		= pdata->gpio_base;
+		s->gpio.ngpio		= s->nr_gpio;
+		if (gpiochip_add(&s->gpio)) {
+			/* Indicate that we should not call gpiochip_remove */
+			s->gpio.base = 0;
+		}
+	} else
+		dev_info(dev, "GPIO support not enabled\n");
+#endif
+
+	/* Go to suspend mode */
+	if (pdata->suspend)
+		pdata->suspend(1);
+
+	return 0;
+
+err_freq:
+	dev_err(dev, "Frequency parameter incorrect\n");
+	ret = -EINVAL;
+
+err_out:
+	dev_set_drvdata(dev, NULL);
+
+	return ret;
+}
+
+static int __devexit max310x_remove(struct spi_device *spi)
+{
+	struct device *dev = &spi->dev;
+	struct max310x_port *s = dev_get_drvdata(dev);
+	int ret = 0;
+
+	dev_dbg(dev, "Removing port\n");
+
+	devm_free_irq(dev, s->port.irq, s);
+
+	destroy_workqueue(s->wq);
+
+	uart_remove_one_port(&s->uart, &s->port);
+
+	uart_unregister_driver(&s->uart);
+
+#ifdef CONFIG_GPIOLIB
+	if (s->pdata->gpio_base) {
+		ret = gpiochip_remove(&s->gpio);
+		if (ret)
+			dev_err(dev, "Failed to remove gpio chip: %d\n", ret);
+	}
+#endif
+
+	dev_set_drvdata(dev, NULL);
+
+	if (s->pdata->suspend)
+		s->pdata->suspend(1);
+	if (s->pdata->exit)
+		s->pdata->exit();
+
+	return ret;
+}
+
+static const struct spi_device_id max310x_id_table[] = {
+	{ "max3107",	MAX310X_TYPE_MAX3107 },
+	{ "max3108",	MAX310X_TYPE_MAX3108 },
+};
+MODULE_DEVICE_TABLE(spi, max310x_id_table);
+
+static struct spi_driver max310x_driver = {
+	.driver = {
+		.name	= "max310x",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max310x_probe,
+	.remove		= __devexit_p(max310x_remove),
+	.suspend	= max310x_suspend,
+	.resume		= max310x_resume,
+	.id_table	= max310x_id_table,
+};
+module_spi_driver(max310x_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
+MODULE_DESCRIPTION("MAX310X serial driver");
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index bedac0d4c9ce..f19d04ed8586 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -775,11 +775,15 @@ mpc52xx_uart_set_termios(struct uart_port *port, struct ktermios *new,
 	}
 
 	if (new->c_cflag & PARENB) {
+		if (new->c_cflag & CMSPAR)
+			mr1 |= MPC52xx_PSC_MODE_PARFORCE;
+
+		/* With CMSPAR, PARODD also means high parity (same as termios) */
 		mr1 |= (new->c_cflag & PARODD) ?
 			MPC52xx_PSC_MODE_PARODD : MPC52xx_PSC_MODE_PAREVEN;
-	} else
+	} else {
 		mr1 |= MPC52xx_PSC_MODE_PARNONE;
-
+	}
 
 	mr2 = 0;
 
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 8131e2c28015..033e0bc9ebab 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -896,7 +896,7 @@ static int __init msm_serial_probe(struct platform_device *pdev)
 			return PTR_ERR(msm_port->clk);
 
 	if (msm_port->is_uartdm)
-		clk_set_rate(msm_port->clk, 7372800);
+		clk_set_rate(msm_port->clk, 1843200);
 
 	port->uartclk = clk_get_rate(msm_port->clk);
 	printk(KERN_INFO "uartclk = %d\n", port->uartclk);
diff --git a/drivers/tty/serial/msm_smd_tty.c b/drivers/tty/serial/msm_smd_tty.c
index b25e6ee71443..925d1fa153db 100644
--- a/drivers/tty/serial/msm_smd_tty.c
+++ b/drivers/tty/serial/msm_smd_tty.c
@@ -223,9 +223,11 @@ static int __init smd_tty_init(void)
 		return ret;
 
 	for (i = 0; i < smd_tty_channels_len; i++) {
-		tty_port_init(&smd_tty[smd_tty_channels[i].id].port);
-		smd_tty[smd_tty_channels[i].id].port.ops = &smd_tty_port_ops;
-		tty_register_device(smd_tty_driver, smd_tty_channels[i].id, 0);
+		struct tty_port *port = &smd_tty[smd_tty_channels[i].id].port;
+		tty_port_init(port);
+		port->ops = &smd_tty_port_ops;
+		tty_port_register_device(port, smd_tty_driver,
+				smd_tty_channels[i].id, NULL);
 	}
 
 	return 0;
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index 3a667eed63d6..6db3baa39a97 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -262,7 +262,7 @@ static void mxs_auart_set_mctrl(struct uart_port *u, unsigned mctrl)
 
 	ctrl &= ~AUART_CTRL2_RTSEN;
 	if (mctrl & TIOCM_RTS) {
-		if (u->state->port.flags & ASYNC_CTS_FLOW)
+		if (tty_port_cts_enabled(&u->state->port))
 			ctrl |= AUART_CTRL2_RTSEN;
 	}
 
@@ -457,11 +457,11 @@ static void mxs_auart_shutdown(struct uart_port *u)
 
 	writel(AUART_CTRL2_UARTEN, u->membase + AUART_CTRL2_CLR);
 
-	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_SET);
-
 	writel(AUART_INTR_RXIEN | AUART_INTR_RTIEN | AUART_INTR_CTSMIEN,
 			u->membase + AUART_INTR_CLR);
 
+	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_SET);
+
 	clk_disable_unprepare(s->clk);
 }
 
@@ -781,6 +781,7 @@ out_free_irq:
 	auart_port[pdev->id] = NULL;
 	free_irq(s->irq, s);
 out_free_clk:
+	put_device(s->dev);
 	clk_put(s->clk);
 out_free:
 	kfree(s);
@@ -796,6 +797,7 @@ static int __devexit mxs_auart_remove(struct platform_device *pdev)
 
 	auart_port[pdev->id] = NULL;
 
+	put_device(s->dev);
 	clk_put(s->clk);
 	free_irq(s->irq, s);
 	kfree(s);
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 34e71874a892..df443b908ca3 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -105,6 +105,10 @@ static int __devinit of_platform_serial_setup(struct platform_device *ofdev,
 	port->uartclk = clk;
 	port->flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF | UPF_IOREMAP
 		| UPF_FIXED_PORT | UPF_FIXED_TYPE;
+
+	if (of_find_property(np, "no-loopback-test", NULL))
+		port->flags |= UPF_SKIP_TEST;
+
 	port->dev = &ofdev->dev;
 
 	if (type == PORT_TEGRA)
@@ -144,8 +148,15 @@ static int __devinit of_platform_serial_probe(struct platform_device *ofdev)
 	switch (port_type) {
 #ifdef CONFIG_SERIAL_8250
 	case PORT_8250 ... PORT_MAX_8250:
-		ret = serial8250_register_port(&port);
+	{
+		/* For now the of bindings don't support the extra
+		   8250 specific bits */
+		struct uart_8250_port port8250;
+		memset(&port8250, 0, sizeof(port8250));
+		port8250.port = port;
+		ret = serial8250_register_8250_port(&port8250);
 		break;
+	}
 #endif
 #ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL
 	case PORT_NWPSERIAL:
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index d3cda0cb2df0..ccc2f35adff1 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -32,16 +32,16 @@
 #include <linux/slab.h>
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
+#include <linux/platform_device.h>
 #include <linux/io.h>
-#include <linux/dma-mapping.h>
 #include <linux/clk.h>
 #include <linux/serial_core.h>
 #include <linux/irq.h>
 #include <linux/pm_runtime.h>
 #include <linux/of.h>
+#include <linux/gpio.h>
+#include <linux/pinctrl/consumer.h>
 
-#include <plat/dma.h>
-#include <plat/dmtimer.h>
 #include <plat/omap-serial.h>
 
 #define UART_BUILD_REVISION(x, y)	(((x) << 8) | (y))
@@ -57,8 +57,8 @@
 #define OMAP_UART_SCR_RX_TRIG_GRANU1_MASK		(1 << 7)
 
 /* FCR register bitmasks */
-#define OMAP_UART_FCR_RX_FIFO_TRIG_SHIFT		6
 #define OMAP_UART_FCR_RX_FIFO_TRIG_MASK			(0x3 << 6)
+#define OMAP_UART_FCR_TX_FIFO_TRIG_MASK			(0x3 << 4)
 
 /* MVR register bitmasks */
 #define OMAP_UART_MVR_SCHEME_SHIFT	30
@@ -71,12 +71,52 @@
 #define OMAP_UART_MVR_MAJ_SHIFT		8
 #define OMAP_UART_MVR_MIN_MASK		0x3f
 
+struct uart_omap_port {
+	struct uart_port	port;
+	struct uart_omap_dma	uart_dma;
+	struct device		*dev;
+
+	unsigned char		ier;
+	unsigned char		lcr;
+	unsigned char		mcr;
+	unsigned char		fcr;
+	unsigned char		efr;
+	unsigned char		dll;
+	unsigned char		dlh;
+	unsigned char		mdr1;
+	unsigned char		scr;
+
+	int			use_dma;
+	/*
+	 * Some bits in registers are cleared on a read, so they must
+	 * be saved whenever the register is read but the bits will not
+	 * be immediately processed.
+	 */
+	unsigned int		lsr_break_flag;
+	unsigned char		msr_saved_flags;
+	char			name[20];
+	unsigned long		port_activity;
+	u32			context_loss_cnt;
+	u32			errata;
+	u8			wakeups_enabled;
+	unsigned int		irq_pending:1;
+
+	int			DTR_gpio;
+	int			DTR_inverted;
+	int			DTR_active;
+
+	struct pm_qos_request	pm_qos_request;
+	u32			latency;
+	u32			calc_latency;
+	struct work_struct	qos_work;
+	struct pinctrl		*pins;
+};
+
+#define to_uart_omap_port(p)	((container_of((p), struct uart_omap_port, port)))
+
 static struct uart_omap_port *ui[OMAP_MAX_HSUART_PORTS];
 
 /* Forward declaration of functions */
-static void uart_tx_dma_callback(int lch, u16 ch_status, void *data);
-static void serial_omap_rxdma_poll(unsigned long uart_no);
-static int serial_omap_start_rxdma(struct uart_omap_port *up);
 static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1);
 
 static struct workqueue_struct *serial_omap_uart_wq;
@@ -101,6 +141,46 @@ static inline void serial_omap_clear_fifos(struct uart_omap_port *up)
 	serial_out(up, UART_FCR, 0);
 }
 
+static int serial_omap_get_context_loss_count(struct uart_omap_port *up)
+{
+	struct omap_uart_port_info *pdata = up->dev->platform_data;
+
+	if (!pdata || !pdata->get_context_loss_count)
+		return 0;
+
+	return pdata->get_context_loss_count(up->dev);
+}
+
+static void serial_omap_set_forceidle(struct uart_omap_port *up)
+{
+	struct omap_uart_port_info *pdata = up->dev->platform_data;
+
+	if (!pdata || !pdata->set_forceidle)
+		return;
+
+	pdata->set_forceidle(up->dev);
+}
+
+static void serial_omap_set_noidle(struct uart_omap_port *up)
+{
+	struct omap_uart_port_info *pdata = up->dev->platform_data;
+
+	if (!pdata || !pdata->set_noidle)
+		return;
+
+	pdata->set_noidle(up->dev);
+}
+
+static void serial_omap_enable_wakeup(struct uart_omap_port *up, bool enable)
+{
+	struct omap_uart_port_info *pdata = up->dev->platform_data;
+
+	if (!pdata || !pdata->enable_wakeup)
+		return;
+
+	pdata->enable_wakeup(up->dev, enable);
+}
+
 /*
  * serial_omap_get_divisor - calculate divisor value
  * @port: uart port info
@@ -126,151 +206,55 @@ serial_omap_get_divisor(struct uart_port *port, unsigned int baud)
 	return port->uartclk/(baud * divisor);
 }
 
-static void serial_omap_stop_rxdma(struct uart_omap_port *up)
-{
-	if (up->uart_dma.rx_dma_used) {
-		del_timer(&up->uart_dma.rx_timer);
-		omap_stop_dma(up->uart_dma.rx_dma_channel);
-		omap_free_dma(up->uart_dma.rx_dma_channel);
-		up->uart_dma.rx_dma_channel = OMAP_UART_DMA_CH_FREE;
-		up->uart_dma.rx_dma_used = false;
-		pm_runtime_mark_last_busy(&up->pdev->dev);
-		pm_runtime_put_autosuspend(&up->pdev->dev);
-	}
-}
-
 static void serial_omap_enable_ms(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
 	dev_dbg(up->port.dev, "serial_omap_enable_ms+%d\n", up->port.line);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	up->ier |= UART_IER_MSI;
 	serial_out(up, UART_IER, up->ier);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static void serial_omap_stop_tx(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
-	struct omap_uart_port_info *pdata = up->pdev->dev.platform_data;
-
-	if (up->use_dma &&
-		up->uart_dma.tx_dma_channel != OMAP_UART_DMA_CH_FREE) {
-		/*
-		 * Check if dma is still active. If yes do nothing,
-		 * return. Else stop dma
-		 */
-		if (omap_get_dma_active_status(up->uart_dma.tx_dma_channel))
-			return;
-		omap_stop_dma(up->uart_dma.tx_dma_channel);
-		omap_free_dma(up->uart_dma.tx_dma_channel);
-		up->uart_dma.tx_dma_channel = OMAP_UART_DMA_CH_FREE;
-		pm_runtime_mark_last_busy(&up->pdev->dev);
-		pm_runtime_put_autosuspend(&up->pdev->dev);
-	}
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	if (up->ier & UART_IER_THRI) {
 		up->ier &= ~UART_IER_THRI;
 		serial_out(up, UART_IER, up->ier);
 	}
 
-	if (!up->use_dma && pdata && pdata->set_forceidle)
-		pdata->set_forceidle(up->pdev);
+	serial_omap_set_forceidle(up);
 
-	pm_runtime_mark_last_busy(&up->pdev->dev);
-	pm_runtime_put_autosuspend(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static void serial_omap_stop_rx(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
-	pm_runtime_get_sync(&up->pdev->dev);
-	if (up->use_dma)
-		serial_omap_stop_rxdma(up);
+	pm_runtime_get_sync(up->dev);
 	up->ier &= ~UART_IER_RLSI;
 	up->port.read_status_mask &= ~UART_LSR_DR;
 	serial_out(up, UART_IER, up->ier);
-	pm_runtime_mark_last_busy(&up->pdev->dev);
-	pm_runtime_put_autosuspend(&up->pdev->dev);
-}
-
-static inline void receive_chars(struct uart_omap_port *up,
-		unsigned int *status)
-{
-	struct tty_struct *tty = up->port.state->port.tty;
-	unsigned int flag, lsr = *status;
-	unsigned char ch = 0;
-	int max_count = 256;
-
-	do {
-		if (likely(lsr & UART_LSR_DR))
-			ch = serial_in(up, UART_RX);
-		flag = TTY_NORMAL;
-		up->port.icount.rx++;
-
-		if (unlikely(lsr & UART_LSR_BRK_ERROR_BITS)) {
-			/*
-			 * For statistics only
-			 */
-			if (lsr & UART_LSR_BI) {
-				lsr &= ~(UART_LSR_FE | UART_LSR_PE);
-				up->port.icount.brk++;
-				/*
-				 * We do the SysRQ and SAK checking
-				 * here because otherwise the break
-				 * may get masked by ignore_status_mask
-				 * or read_status_mask.
-				 */
-				if (uart_handle_break(&up->port))
-					goto ignore_char;
-			} else if (lsr & UART_LSR_PE) {
-				up->port.icount.parity++;
-			} else if (lsr & UART_LSR_FE) {
-				up->port.icount.frame++;
-			}
-
-			if (lsr & UART_LSR_OE)
-				up->port.icount.overrun++;
-
-			/*
-			 * Mask off conditions which should be ignored.
-			 */
-			lsr &= up->port.read_status_mask;
-
-#ifdef CONFIG_SERIAL_OMAP_CONSOLE
-			if (up->port.line == up->port.cons->index) {
-				/* Recover the break flag from console xmit */
-				lsr |= up->lsr_break_flag;
-			}
-#endif
-			if (lsr & UART_LSR_BI)
-				flag = TTY_BREAK;
-			else if (lsr & UART_LSR_PE)
-				flag = TTY_PARITY;
-			else if (lsr & UART_LSR_FE)
-				flag = TTY_FRAME;
-		}
-
-		if (uart_handle_sysrq_char(&up->port, ch))
-			goto ignore_char;
-		uart_insert_char(&up->port, lsr, UART_LSR_OE, ch, flag);
-ignore_char:
-		lsr = serial_in(up, UART_LSR);
-	} while ((lsr & (UART_LSR_DR | UART_LSR_BI)) && (max_count-- > 0));
-	spin_unlock(&up->port.lock);
-	tty_flip_buffer_push(tty);
-	spin_lock(&up->port.lock);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
-static void transmit_chars(struct uart_omap_port *up)
+static void transmit_chars(struct uart_omap_port *up, unsigned int lsr)
 {
 	struct circ_buf *xmit = &up->port.state->xmit;
 	int count;
 
+	if (!(lsr & UART_LSR_THRE))
+		return;
+
 	if (up->port.x_char) {
 		serial_out(up, UART_TX, up->port.x_char);
 		up->port.icount.tx++;
@@ -290,8 +274,11 @@ static void transmit_chars(struct uart_omap_port *up)
 			break;
 	} while (--count > 0);
 
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) {
+		spin_unlock(&up->port.lock);
 		uart_write_wakeup(&up->port);
+		spin_lock(&up->port.lock);
+	}
 
 	if (uart_circ_empty(xmit))
 		serial_omap_stop_tx(&up->port);
@@ -307,70 +294,13 @@ static inline void serial_omap_enable_ier_thri(struct uart_omap_port *up)
 
 static void serial_omap_start_tx(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
-	struct omap_uart_port_info *pdata = up->pdev->dev.platform_data;
-	struct circ_buf *xmit;
-	unsigned int start;
-	int ret = 0;
-
-	if (!up->use_dma) {
-		pm_runtime_get_sync(&up->pdev->dev);
-		serial_omap_enable_ier_thri(up);
-		if (pdata && pdata->set_noidle)
-			pdata->set_noidle(up->pdev);
-		pm_runtime_mark_last_busy(&up->pdev->dev);
-		pm_runtime_put_autosuspend(&up->pdev->dev);
-		return;
-	}
-
-	if (up->uart_dma.tx_dma_used)
-		return;
-
-	xmit = &up->port.state->xmit;
-
-	if (up->uart_dma.tx_dma_channel == OMAP_UART_DMA_CH_FREE) {
-		pm_runtime_get_sync(&up->pdev->dev);
-		ret = omap_request_dma(up->uart_dma.uart_dma_tx,
-				"UART Tx DMA",
-				(void *)uart_tx_dma_callback, up,
-				&(up->uart_dma.tx_dma_channel));
-
-		if (ret < 0) {
-			serial_omap_enable_ier_thri(up);
-			return;
-		}
-	}
-	spin_lock(&(up->uart_dma.tx_lock));
-	up->uart_dma.tx_dma_used = true;
-	spin_unlock(&(up->uart_dma.tx_lock));
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
-	start = up->uart_dma.tx_buf_dma_phys +
-				(xmit->tail & (UART_XMIT_SIZE - 1));
-
-	up->uart_dma.tx_buf_size = uart_circ_chars_pending(xmit);
-	/*
-	 * It is a circular buffer. See if the buffer has wounded back.
-	 * If yes it will have to be transferred in two separate dma
-	 * transfers
-	 */
-	if (start + up->uart_dma.tx_buf_size >=
-			up->uart_dma.tx_buf_dma_phys + UART_XMIT_SIZE)
-		up->uart_dma.tx_buf_size =
-			(up->uart_dma.tx_buf_dma_phys +
-			UART_XMIT_SIZE) - start;
-
-	omap_set_dma_dest_params(up->uart_dma.tx_dma_channel, 0,
-				OMAP_DMA_AMODE_CONSTANT,
-				up->uart_dma.uart_base, 0, 0);
-	omap_set_dma_src_params(up->uart_dma.tx_dma_channel, 0,
-				OMAP_DMA_AMODE_POST_INC, start, 0, 0);
-	omap_set_dma_transfer_params(up->uart_dma.tx_dma_channel,
-				OMAP_DMA_DATA_TYPE_S8,
-				up->uart_dma.tx_buf_size, 1,
-				OMAP_DMA_SYNC_ELEMENT,
-				up->uart_dma.uart_dma_tx, 0);
-	/* FIXME: Cache maintenance needed here? */
-	omap_start_dma(up->uart_dma.tx_dma_channel);
+	pm_runtime_get_sync(up->dev);
+	serial_omap_enable_ier_thri(up);
+	serial_omap_set_noidle(up);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static unsigned int check_modem_status(struct uart_omap_port *up)
@@ -401,76 +331,162 @@ static unsigned int check_modem_status(struct uart_omap_port *up)
 	return status;
 }
 
+static void serial_omap_rlsi(struct uart_omap_port *up, unsigned int lsr)
+{
+	unsigned int flag;
+	unsigned char ch = 0;
+
+	if (likely(lsr & UART_LSR_DR))
+		ch = serial_in(up, UART_RX);
+
+	up->port.icount.rx++;
+	flag = TTY_NORMAL;
+
+	if (lsr & UART_LSR_BI) {
+		flag = TTY_BREAK;
+		lsr &= ~(UART_LSR_FE | UART_LSR_PE);
+		up->port.icount.brk++;
+		/*
+		 * We do the SysRQ and SAK checking
+		 * here because otherwise the break
+		 * may get masked by ignore_status_mask
+		 * or read_status_mask.
+		 */
+		if (uart_handle_break(&up->port))
+			return;
+
+	}
+
+	if (lsr & UART_LSR_PE) {
+		flag = TTY_PARITY;
+		up->port.icount.parity++;
+	}
+
+	if (lsr & UART_LSR_FE) {
+		flag = TTY_FRAME;
+		up->port.icount.frame++;
+	}
+
+	if (lsr & UART_LSR_OE)
+		up->port.icount.overrun++;
+
+#ifdef CONFIG_SERIAL_OMAP_CONSOLE
+	if (up->port.line == up->port.cons->index) {
+		/* Recover the break flag from console xmit */
+		lsr |= up->lsr_break_flag;
+	}
+#endif
+	uart_insert_char(&up->port, lsr, UART_LSR_OE, 0, flag);
+}
+
+static void serial_omap_rdi(struct uart_omap_port *up, unsigned int lsr)
+{
+	unsigned char ch = 0;
+	unsigned int flag;
+
+	if (!(lsr & UART_LSR_DR))
+		return;
+
+	ch = serial_in(up, UART_RX);
+	flag = TTY_NORMAL;
+	up->port.icount.rx++;
+
+	if (uart_handle_sysrq_char(&up->port, ch))
+		return;
+
+	uart_insert_char(&up->port, lsr, UART_LSR_OE, ch, flag);
+}
+
 /**
  * serial_omap_irq() - This handles the interrupt from one port
  * @irq: uart port irq number
  * @dev_id: uart port info
  */
-static inline irqreturn_t serial_omap_irq(int irq, void *dev_id)
+static irqreturn_t serial_omap_irq(int irq, void *dev_id)
 {
 	struct uart_omap_port *up = dev_id;
+	struct tty_struct *tty = up->port.state->port.tty;
 	unsigned int iir, lsr;
-	unsigned long flags;
+	unsigned int type;
+	irqreturn_t ret = IRQ_NONE;
+	int max_count = 256;
 
-	pm_runtime_get_sync(&up->pdev->dev);
-	iir = serial_in(up, UART_IIR);
-	if (iir & UART_IIR_NO_INT) {
-		pm_runtime_mark_last_busy(&up->pdev->dev);
-		pm_runtime_put_autosuspend(&up->pdev->dev);
-		return IRQ_NONE;
-	}
+	spin_lock(&up->port.lock);
+	pm_runtime_get_sync(up->dev);
 
-	spin_lock_irqsave(&up->port.lock, flags);
-	lsr = serial_in(up, UART_LSR);
-	if (iir & UART_IIR_RLSI) {
-		if (!up->use_dma) {
-			if (lsr & UART_LSR_DR)
-				receive_chars(up, &lsr);
-		} else {
-			up->ier &= ~(UART_IER_RDI | UART_IER_RLSI);
-			serial_out(up, UART_IER, up->ier);
-			if ((serial_omap_start_rxdma(up) != 0) &&
-					(lsr & UART_LSR_DR))
-				receive_chars(up, &lsr);
+	do {
+		iir = serial_in(up, UART_IIR);
+		if (iir & UART_IIR_NO_INT)
+			break;
+
+		ret = IRQ_HANDLED;
+		lsr = serial_in(up, UART_LSR);
+
+		/* extract IRQ type from IIR register */
+		type = iir & 0x3e;
+
+		switch (type) {
+		case UART_IIR_MSI:
+			check_modem_status(up);
+			break;
+		case UART_IIR_THRI:
+			transmit_chars(up, lsr);
+			break;
+		case UART_IIR_RX_TIMEOUT:
+			/* FALLTHROUGH */
+		case UART_IIR_RDI:
+			serial_omap_rdi(up, lsr);
+			break;
+		case UART_IIR_RLSI:
+			serial_omap_rlsi(up, lsr);
+			break;
+		case UART_IIR_CTS_RTS_DSR:
+			/* simply try again */
+			break;
+		case UART_IIR_XOFF:
+			/* FALLTHROUGH */
+		default:
+			break;
 		}
-	}
+	} while (!(iir & UART_IIR_NO_INT) && max_count--);
 
-	check_modem_status(up);
-	if ((lsr & UART_LSR_THRE) && (iir & UART_IIR_THRI))
-		transmit_chars(up);
+	spin_unlock(&up->port.lock);
 
-	spin_unlock_irqrestore(&up->port.lock, flags);
-	pm_runtime_mark_last_busy(&up->pdev->dev);
-	pm_runtime_put_autosuspend(&up->pdev->dev);
+	tty_flip_buffer_push(tty);
 
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	up->port_activity = jiffies;
-	return IRQ_HANDLED;
+
+	return ret;
 }
 
 static unsigned int serial_omap_tx_empty(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned long flags = 0;
 	unsigned int ret = 0;
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	dev_dbg(up->port.dev, "serial_omap_tx_empty+%d\n", up->port.line);
 	spin_lock_irqsave(&up->port.lock, flags);
 	ret = serial_in(up, UART_LSR) & UART_LSR_TEMT ? TIOCSER_TEMT : 0;
 	spin_unlock_irqrestore(&up->port.lock, flags);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	return ret;
 }
 
 static unsigned int serial_omap_get_mctrl(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned int status;
 	unsigned int ret = 0;
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	status = check_modem_status(up);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 
 	dev_dbg(up->port.dev, "serial_omap_get_mctrl+%d\n", up->port.line);
 
@@ -487,7 +503,7 @@ static unsigned int serial_omap_get_mctrl(struct uart_port *port)
 
 static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned char mcr = 0;
 
 	dev_dbg(up->port.dev, "serial_omap_set_mctrl+%d\n", up->port.line);
@@ -502,20 +518,31 @@ static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	if (mctrl & TIOCM_LOOP)
 		mcr |= UART_MCR_LOOP;
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	up->mcr = serial_in(up, UART_MCR);
 	up->mcr |= mcr;
 	serial_out(up, UART_MCR, up->mcr);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
+
+	if (gpio_is_valid(up->DTR_gpio) &&
+	    !!(mctrl & TIOCM_DTR) != up->DTR_active) {
+		up->DTR_active = !up->DTR_active;
+		if (gpio_cansleep(up->DTR_gpio))
+			schedule_work(&up->qos_work);
+		else
+			gpio_set_value(up->DTR_gpio,
+				       up->DTR_active != up->DTR_inverted);
+	}
 }
 
 static void serial_omap_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned long flags = 0;
 
 	dev_dbg(up->port.dev, "serial_omap_break_ctl+%d\n", up->port.line);
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	spin_lock_irqsave(&up->port.lock, flags);
 	if (break_state == -1)
 		up->lcr |= UART_LCR_SBC;
@@ -523,12 +550,13 @@ static void serial_omap_break_ctl(struct uart_port *port, int break_state)
 		up->lcr &= ~UART_LCR_SBC;
 	serial_out(up, UART_LCR, up->lcr);
 	spin_unlock_irqrestore(&up->port.lock, flags);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static int serial_omap_startup(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned long flags = 0;
 	int retval;
 
@@ -542,7 +570,7 @@ static int serial_omap_startup(struct uart_port *port)
 
 	dev_dbg(up->port.dev, "serial_omap_startup+%d\n", up->port.line);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	/*
 	 * Clear the FIFO buffers and disable them.
 	 * (they will be reenabled in set_termios())
@@ -573,20 +601,6 @@ static int serial_omap_startup(struct uart_port *port)
 	spin_unlock_irqrestore(&up->port.lock, flags);
 
 	up->msr_saved_flags = 0;
-	if (up->use_dma) {
-		free_page((unsigned long)up->port.state->xmit.buf);
-		up->port.state->xmit.buf = dma_alloc_coherent(NULL,
-			UART_XMIT_SIZE,
-			(dma_addr_t *)&(up->uart_dma.tx_buf_dma_phys),
-			0);
-		init_timer(&(up->uart_dma.rx_timer));
-		up->uart_dma.rx_timer.function = serial_omap_rxdma_poll;
-		up->uart_dma.rx_timer.data = up->port.line;
-		/* Currently the buffer size is 4KB. Can increase it */
-		up->uart_dma.rx_buf = dma_alloc_coherent(NULL,
-			up->uart_dma.rx_buf_size,
-			(dma_addr_t *)&(up->uart_dma.rx_buf_dma_phys), 0);
-	}
 	/*
 	 * Finally, enable interrupts. Note: Modem status interrupts
 	 * are set via set_termios(), which will be occurring imminently
@@ -598,20 +612,20 @@ static int serial_omap_startup(struct uart_port *port)
 	/* Enable module level wake up */
 	serial_out(up, UART_OMAP_WER, OMAP_UART_WER_MOD_WKUP);
 
-	pm_runtime_mark_last_busy(&up->pdev->dev);
-	pm_runtime_put_autosuspend(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	up->port_activity = jiffies;
 	return 0;
 }
 
 static void serial_omap_shutdown(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned long flags = 0;
 
 	dev_dbg(up->port.dev, "serial_omap_shutdown+%d\n", up->port.line);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	/*
 	 * Disable interrupts from this port
 	 */
@@ -634,19 +648,9 @@ static void serial_omap_shutdown(struct uart_port *port)
 	 */
 	if (serial_in(up, UART_LSR) & UART_LSR_DR)
 		(void) serial_in(up, UART_RX);
-	if (up->use_dma) {
-		dma_free_coherent(up->port.dev,
-			UART_XMIT_SIZE,	up->port.state->xmit.buf,
-			up->uart_dma.tx_buf_dma_phys);
-		up->port.state->xmit.buf = NULL;
-		serial_omap_stop_rx(port);
-		dma_free_coherent(up->port.dev,
-			up->uart_dma.rx_buf_size, up->uart_dma.rx_buf,
-			up->uart_dma.rx_buf_dma_phys);
-		up->uart_dma.rx_buf = NULL;
-	}
 
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	free_irq(up->port.irq, up);
 }
 
@@ -667,19 +671,19 @@ serial_omap_configure_xonxoff
 
 	/*
 	 * IXON Flag:
-	 * Enable XON/XOFF flow control on output.
-	 * Transmit XON1, XOFF1
+	 * Flow control for OMAP.TX
+	 * OMAP.RX should listen for XON/XOFF
 	 */
 	if (termios->c_iflag & IXON)
-		up->efr |= OMAP_UART_SW_TX;
+		up->efr |= OMAP_UART_SW_RX;
 
 	/*
 	 * IXOFF Flag:
-	 * Enable XON/XOFF flow control on input.
-	 * Receiver compares XON1, XOFF1.
+	 * Flow control for OMAP.RX
+	 * OMAP.TX should send XON/XOFF
 	 */
 	if (termios->c_iflag & IXOFF)
-		up->efr |= OMAP_UART_SW_RX;
+		up->efr |= OMAP_UART_SW_TX;
 
 	serial_out(up, UART_EFR, up->efr | UART_EFR_ECB);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
@@ -715,13 +719,16 @@ static void serial_omap_uart_qos_work(struct work_struct *work)
 						qos_work);
 
 	pm_qos_update_request(&up->pm_qos_request, up->latency);
+	if (gpio_is_valid(up->DTR_gpio))
+		gpio_set_value_cansleep(up->DTR_gpio,
+					up->DTR_active != up->DTR_inverted);
 }
 
 static void
 serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 			struct ktermios *old)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned char cval = 0;
 	unsigned char efr = 0;
 	unsigned long flags = 0;
@@ -768,14 +775,12 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	up->fcr = UART_FCR_R_TRIG_01 | UART_FCR_T_TRIG_01 |
 			UART_FCR_ENABLE_FIFO;
-	if (up->use_dma)
-		up->fcr |= UART_FCR_DMA_SELECT;
 
 	/*
 	 * Ok, we're now changing the port state. Do it with
 	 * interrupts disabled.
 	 */
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	spin_lock_irqsave(&up->port.lock, flags);
 
 	/*
@@ -845,14 +850,13 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	up->scr |= OMAP_UART_SCR_RX_TRIG_GRANU1_MASK;
 
-	if (up->use_dma) {
-		serial_out(up, UART_TI752_TLR, 0);
-		up->scr |= UART_FCR_TRIGGER_4;
-	} else {
-		/* Set receive FIFO threshold to 1 byte */
-		up->fcr &= ~OMAP_UART_FCR_RX_FIFO_TRIG_MASK;
-		up->fcr |= (0x1 << OMAP_UART_FCR_RX_FIFO_TRIG_SHIFT);
-	}
+	/* Set receive FIFO threshold to 16 characters and
+	 * transmit FIFO threshold to 16 spaces
+	 */
+	up->fcr &= ~OMAP_UART_FCR_RX_FIFO_TRIG_MASK;
+	up->fcr &= ~OMAP_UART_FCR_TX_FIFO_TRIG_MASK;
+	up->fcr |= UART_FCR6_R_TRIGGER_16 | UART_FCR6_T_TRIGGER_24 |
+		UART_FCR_ENABLE_FIFO;
 
 	serial_out(up, UART_FCR, up->fcr);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
@@ -924,20 +928,30 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	serial_omap_configure_xonxoff(up, termios);
 
 	spin_unlock_irqrestore(&up->port.lock, flags);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	dev_dbg(up->port.dev, "serial_omap_set_termios+%d\n", up->port.line);
 }
 
+static int serial_omap_set_wake(struct uart_port *port, unsigned int state)
+{
+	struct uart_omap_port *up = to_uart_omap_port(port);
+
+	serial_omap_enable_wakeup(up, state);
+
+	return 0;
+}
+
 static void
 serial_omap_pm(struct uart_port *port, unsigned int state,
 	       unsigned int oldstate)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned char efr;
 
 	dev_dbg(up->port.dev, "serial_omap_pm+%d\n", up->port.line);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 	efr = serial_in(up, UART_EFR);
 	serial_out(up, UART_EFR, efr | UART_EFR_ECB);
@@ -948,14 +962,15 @@ serial_omap_pm(struct uart_port *port, unsigned int state,
 	serial_out(up, UART_EFR, efr);
 	serial_out(up, UART_LCR, 0);
 
-	if (!device_may_wakeup(&up->pdev->dev)) {
+	if (!device_may_wakeup(up->dev)) {
 		if (!state)
-			pm_runtime_forbid(&up->pdev->dev);
+			pm_runtime_forbid(up->dev);
 		else
-			pm_runtime_allow(&up->pdev->dev);
+			pm_runtime_allow(up->dev);
 	}
 
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static void serial_omap_release_port(struct uart_port *port)
@@ -971,7 +986,7 @@ static int serial_omap_request_port(struct uart_port *port)
 
 static void serial_omap_config_port(struct uart_port *port, int flags)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
 	dev_dbg(up->port.dev, "serial_omap_config_port+%d\n",
 							up->port.line);
@@ -989,7 +1004,7 @@ serial_omap_verify_port(struct uart_port *port, struct serial_struct *ser)
 static const char *
 serial_omap_type(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
 	dev_dbg(up->port.dev, "serial_omap_type+%d\n", up->port.line);
 	return up->name;
@@ -1032,26 +1047,33 @@ static inline void wait_for_xmitr(struct uart_omap_port *up)
 
 static void serial_omap_poll_put_char(struct uart_port *port, unsigned char ch)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	wait_for_xmitr(up);
 	serial_out(up, UART_TX, ch);
-	pm_runtime_put(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 }
 
 static int serial_omap_poll_get_char(struct uart_port *port)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned int status;
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 	status = serial_in(up, UART_LSR);
-	if (!(status & UART_LSR_DR))
-		return NO_POLL_CHAR;
+	if (!(status & UART_LSR_DR)) {
+		status = NO_POLL_CHAR;
+		goto out;
+	}
 
 	status = serial_in(up, UART_RX);
-	pm_runtime_put(&up->pdev->dev);
+
+out:
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
+
 	return status;
 }
 
@@ -1065,7 +1087,7 @@ static struct uart_driver serial_omap_reg;
 
 static void serial_omap_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_omap_port *up = (struct uart_omap_port *)port;
+	struct uart_omap_port *up = to_uart_omap_port(port);
 
 	wait_for_xmitr(up);
 	serial_out(up, UART_TX, ch);
@@ -1080,7 +1102,7 @@ serial_omap_console_write(struct console *co, const char *s,
 	unsigned int ier;
 	int locked = 1;
 
-	pm_runtime_get_sync(&up->pdev->dev);
+	pm_runtime_get_sync(up->dev);
 
 	local_irq_save(flags);
 	if (up->port.sysrq)
@@ -1114,8 +1136,8 @@ serial_omap_console_write(struct console *co, const char *s,
 	if (up->msr_saved_flags)
 		check_modem_status(up);
 
-	pm_runtime_mark_last_busy(&up->pdev->dev);
-	pm_runtime_put_autosuspend(&up->pdev->dev);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	if (locked)
 		spin_unlock(&up->port.lock);
 	local_irq_restore(flags);
@@ -1179,6 +1201,7 @@ static struct uart_ops serial_omap_pops = {
 	.shutdown	= serial_omap_shutdown,
 	.set_termios	= serial_omap_set_termios,
 	.pm		= serial_omap_pm,
+	.set_wake	= serial_omap_set_wake,
 	.type		= serial_omap_type,
 	.release_port	= serial_omap_release_port,
 	.request_port	= serial_omap_request_port,
@@ -1203,10 +1226,8 @@ static int serial_omap_suspend(struct device *dev)
 {
 	struct uart_omap_port *up = dev_get_drvdata(dev);
 
-	if (up) {
-		uart_suspend_port(&serial_omap_reg, &up->port);
-		flush_work_sync(&up->qos_work);
-	}
+	uart_suspend_port(&serial_omap_reg, &up->port);
+	flush_work_sync(&up->qos_work);
 
 	return 0;
 }
@@ -1215,156 +1236,13 @@ static int serial_omap_resume(struct device *dev)
 {
 	struct uart_omap_port *up = dev_get_drvdata(dev);
 
-	if (up)
-		uart_resume_port(&serial_omap_reg, &up->port);
+	uart_resume_port(&serial_omap_reg, &up->port);
+
 	return 0;
 }
 #endif
 
-static void serial_omap_rxdma_poll(unsigned long uart_no)
-{
-	struct uart_omap_port *up = ui[uart_no];
-	unsigned int curr_dma_pos, curr_transmitted_size;
-	int ret = 0;
-
-	curr_dma_pos = omap_get_dma_dst_pos(up->uart_dma.rx_dma_channel);
-	if ((curr_dma_pos == up->uart_dma.prev_rx_dma_pos) ||
-			     (curr_dma_pos == 0)) {
-		if (jiffies_to_msecs(jiffies - up->port_activity) <
-						up->uart_dma.rx_timeout) {
-			mod_timer(&up->uart_dma.rx_timer, jiffies +
-				usecs_to_jiffies(up->uart_dma.rx_poll_rate));
-		} else {
-			serial_omap_stop_rxdma(up);
-			up->ier |= (UART_IER_RDI | UART_IER_RLSI);
-			serial_out(up, UART_IER, up->ier);
-		}
-		return;
-	}
-
-	curr_transmitted_size = curr_dma_pos -
-					up->uart_dma.prev_rx_dma_pos;
-	up->port.icount.rx += curr_transmitted_size;
-	tty_insert_flip_string(up->port.state->port.tty,
-			up->uart_dma.rx_buf +
-			(up->uart_dma.prev_rx_dma_pos -
-			up->uart_dma.rx_buf_dma_phys),
-			curr_transmitted_size);
-	tty_flip_buffer_push(up->port.state->port.tty);
-	up->uart_dma.prev_rx_dma_pos = curr_dma_pos;
-	if (up->uart_dma.rx_buf_size +
-			up->uart_dma.rx_buf_dma_phys == curr_dma_pos) {
-		ret = serial_omap_start_rxdma(up);
-		if (ret < 0) {
-			serial_omap_stop_rxdma(up);
-			up->ier |= (UART_IER_RDI | UART_IER_RLSI);
-			serial_out(up, UART_IER, up->ier);
-		}
-	} else  {
-		mod_timer(&up->uart_dma.rx_timer, jiffies +
-			usecs_to_jiffies(up->uart_dma.rx_poll_rate));
-	}
-	up->port_activity = jiffies;
-}
-
-static void uart_rx_dma_callback(int lch, u16 ch_status, void *data)
-{
-	return;
-}
-
-static int serial_omap_start_rxdma(struct uart_omap_port *up)
-{
-	int ret = 0;
-
-	if (up->uart_dma.rx_dma_channel == -1) {
-		pm_runtime_get_sync(&up->pdev->dev);
-		ret = omap_request_dma(up->uart_dma.uart_dma_rx,
-				"UART Rx DMA",
-				(void *)uart_rx_dma_callback, up,
-				&(up->uart_dma.rx_dma_channel));
-		if (ret < 0)
-			return ret;
-
-		omap_set_dma_src_params(up->uart_dma.rx_dma_channel, 0,
-				OMAP_DMA_AMODE_CONSTANT,
-				up->uart_dma.uart_base, 0, 0);
-		omap_set_dma_dest_params(up->uart_dma.rx_dma_channel, 0,
-				OMAP_DMA_AMODE_POST_INC,
-				up->uart_dma.rx_buf_dma_phys, 0, 0);
-		omap_set_dma_transfer_params(up->uart_dma.rx_dma_channel,
-				OMAP_DMA_DATA_TYPE_S8,
-				up->uart_dma.rx_buf_size, 1,
-				OMAP_DMA_SYNC_ELEMENT,
-				up->uart_dma.uart_dma_rx, 0);
-	}
-	up->uart_dma.prev_rx_dma_pos = up->uart_dma.rx_buf_dma_phys;
-	/* FIXME: Cache maintenance needed here? */
-	omap_start_dma(up->uart_dma.rx_dma_channel);
-	mod_timer(&up->uart_dma.rx_timer, jiffies +
-				usecs_to_jiffies(up->uart_dma.rx_poll_rate));
-	up->uart_dma.rx_dma_used = true;
-	return ret;
-}
-
-static void serial_omap_continue_tx(struct uart_omap_port *up)
-{
-	struct circ_buf *xmit = &up->port.state->xmit;
-	unsigned int start = up->uart_dma.tx_buf_dma_phys
-			+ (xmit->tail & (UART_XMIT_SIZE - 1));
-
-	if (uart_circ_empty(xmit))
-		return;
-
-	up->uart_dma.tx_buf_size = uart_circ_chars_pending(xmit);
-	/*
-	 * It is a circular buffer. See if the buffer has wounded back.
-	 * If yes it will have to be transferred in two separate dma
-	 * transfers
-	 */
-	if (start + up->uart_dma.tx_buf_size >=
-			up->uart_dma.tx_buf_dma_phys + UART_XMIT_SIZE)
-		up->uart_dma.tx_buf_size =
-			(up->uart_dma.tx_buf_dma_phys + UART_XMIT_SIZE) - start;
-	omap_set_dma_dest_params(up->uart_dma.tx_dma_channel, 0,
-				OMAP_DMA_AMODE_CONSTANT,
-				up->uart_dma.uart_base, 0, 0);
-	omap_set_dma_src_params(up->uart_dma.tx_dma_channel, 0,
-				OMAP_DMA_AMODE_POST_INC, start, 0, 0);
-	omap_set_dma_transfer_params(up->uart_dma.tx_dma_channel,
-				OMAP_DMA_DATA_TYPE_S8,
-				up->uart_dma.tx_buf_size, 1,
-				OMAP_DMA_SYNC_ELEMENT,
-				up->uart_dma.uart_dma_tx, 0);
-	/* FIXME: Cache maintenance needed here? */
-	omap_start_dma(up->uart_dma.tx_dma_channel);
-}
-
-static void uart_tx_dma_callback(int lch, u16 ch_status, void *data)
-{
-	struct uart_omap_port *up = (struct uart_omap_port *)data;
-	struct circ_buf *xmit = &up->port.state->xmit;
-
-	xmit->tail = (xmit->tail + up->uart_dma.tx_buf_size) & \
-			(UART_XMIT_SIZE - 1);
-	up->port.icount.tx += up->uart_dma.tx_buf_size;
-
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
-		uart_write_wakeup(&up->port);
-
-	if (uart_circ_empty(xmit)) {
-		spin_lock(&(up->uart_dma.tx_lock));
-		serial_omap_stop_tx(&up->port);
-		up->uart_dma.tx_dma_used = false;
-		spin_unlock(&(up->uart_dma.tx_lock));
-	} else {
-		omap_stop_dma(up->uart_dma.tx_dma_channel);
-		serial_omap_continue_tx(up);
-	}
-	up->port_activity = jiffies;
-	return;
-}
-
-static void omap_serial_fill_features_erratas(struct uart_omap_port *up)
+static void __devinit omap_serial_fill_features_erratas(struct uart_omap_port *up)
 {
 	u32 mvr, scheme;
 	u16 revision, major, minor;
@@ -1389,7 +1267,7 @@ static void omap_serial_fill_features_erratas(struct uart_omap_port *up)
 		minor = (mvr & OMAP_UART_MVR_MIN_MASK);
 		break;
 	default:
-		dev_warn(&up->pdev->dev,
+		dev_warn(up->dev,
 			"Unknown %s revision, defaulting to highest\n",
 			up->name);
 		/* highest possible revision */
@@ -1417,7 +1295,7 @@ static void omap_serial_fill_features_erratas(struct uart_omap_port *up)
 	}
 }
 
-static struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
+static __devinit struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
 {
 	struct omap_uart_port_info *omap_up_info;
 
@@ -1430,12 +1308,12 @@ static struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
 	return omap_up_info;
 }
 
-static int serial_omap_probe(struct platform_device *pdev)
+static int __devinit serial_omap_probe(struct platform_device *pdev)
 {
 	struct uart_omap_port	*up;
-	struct resource		*mem, *irq, *dma_tx, *dma_rx;
+	struct resource		*mem, *irq;
 	struct omap_uart_port_info *omap_up_info = pdev->dev.platform_data;
-	int ret = -ENOSPC;
+	int ret;
 
 	if (pdev->dev.of_node)
 		omap_up_info = of_get_uart_port_info(&pdev->dev);
@@ -1458,19 +1336,30 @@ static int serial_omap_probe(struct platform_device *pdev)
 		return -EBUSY;
 	}
 
-	dma_rx = platform_get_resource_byname(pdev, IORESOURCE_DMA, "rx");
-	if (!dma_rx)
-		return -ENXIO;
-
-	dma_tx = platform_get_resource_byname(pdev, IORESOURCE_DMA, "tx");
-	if (!dma_tx)
-		return -ENXIO;
+	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
+	    omap_up_info->DTR_present) {
+		ret = gpio_request(omap_up_info->DTR_gpio, "omap-serial");
+		if (ret < 0)
+			return ret;
+		ret = gpio_direction_output(omap_up_info->DTR_gpio,
+					    omap_up_info->DTR_inverted);
+		if (ret < 0)
+			return ret;
+	}
 
 	up = devm_kzalloc(&pdev->dev, sizeof(*up), GFP_KERNEL);
 	if (!up)
 		return -ENOMEM;
 
-	up->pdev = pdev;
+	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
+	    omap_up_info->DTR_present) {
+		up->DTR_gpio = omap_up_info->DTR_gpio;
+		up->DTR_inverted = omap_up_info->DTR_inverted;
+	} else
+		up->DTR_gpio = -EINVAL;
+	up->DTR_active = 0;
+
+	up->dev = &pdev->dev;
 	up->port.dev = &pdev->dev;
 	up->port.type = PORT_OMAP;
 	up->port.iotype = UPIO_MEM;
@@ -1492,6 +1381,13 @@ static int serial_omap_probe(struct platform_device *pdev)
 		goto err_port_line;
 	}
 
+	up->pins = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(up->pins)) {
+		dev_warn(&pdev->dev, "did not get pins for uart%i error: %li\n",
+			 up->port.line, PTR_ERR(up->pins));
+		up->pins = NULL;
+	}
+
 	sprintf(up->name, "OMAP UART%d", up->port.line);
 	up->port.mapbase = mem->start;
 	up->port.membase = devm_ioremap(&pdev->dev, mem->start,
@@ -1509,20 +1405,6 @@ static int serial_omap_probe(struct platform_device *pdev)
 		dev_warn(&pdev->dev, "No clock speed specified: using default:"
 						"%d\n", DEFAULT_CLK_SPEED);
 	}
-	up->uart_dma.uart_base = mem->start;
-
-	if (omap_up_info->dma_enabled) {
-		up->uart_dma.uart_dma_tx = dma_tx->start;
-		up->uart_dma.uart_dma_rx = dma_rx->start;
-		up->use_dma = 1;
-		up->uart_dma.rx_buf_size = omap_up_info->dma_rx_buf_size;
-		up->uart_dma.rx_timeout = omap_up_info->dma_rx_timeout;
-		up->uart_dma.rx_poll_rate = omap_up_info->dma_rx_poll_rate;
-		spin_lock_init(&(up->uart_dma.tx_lock));
-		spin_lock_init(&(up->uart_dma.rx_lock));
-		up->uart_dma.tx_dma_channel = OMAP_UART_DMA_CH_FREE;
-		up->uart_dma.rx_dma_channel = OMAP_UART_DMA_CH_FREE;
-	}
 
 	up->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
 	up->calc_latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
@@ -1531,12 +1413,13 @@ static int serial_omap_probe(struct platform_device *pdev)
 	serial_omap_uart_wq = create_singlethread_workqueue(up->name);
 	INIT_WORK(&up->qos_work, serial_omap_uart_qos_work);
 
+	platform_set_drvdata(pdev, up);
+	pm_runtime_enable(&pdev->dev);
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev,
 			omap_up_info->autosuspend_timeout);
 
 	pm_runtime_irq_safe(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
 
 	omap_serial_fill_features_erratas(up);
@@ -1548,8 +1431,8 @@ static int serial_omap_probe(struct platform_device *pdev)
 	if (ret != 0)
 		goto err_add_port;
 
-	pm_runtime_put(&pdev->dev);
-	platform_set_drvdata(pdev, up);
+	pm_runtime_mark_last_busy(up->dev);
+	pm_runtime_put_autosuspend(up->dev);
 	return 0;
 
 err_add_port:
@@ -1562,17 +1445,15 @@ err_port_line:
 	return ret;
 }
 
-static int serial_omap_remove(struct platform_device *dev)
+static int __devexit serial_omap_remove(struct platform_device *dev)
 {
 	struct uart_omap_port *up = platform_get_drvdata(dev);
 
-	if (up) {
-		pm_runtime_disable(&up->pdev->dev);
-		uart_remove_one_port(&serial_omap_reg, &up->port);
-		pm_qos_remove_request(&up->pm_qos_request);
-	}
+	pm_runtime_put_sync(up->dev);
+	pm_runtime_disable(up->dev);
+	uart_remove_one_port(&serial_omap_reg, &up->port);
+	pm_qos_remove_request(&up->pm_qos_request);
 
-	platform_set_drvdata(dev, NULL);
 	return 0;
 }
 
@@ -1602,7 +1483,7 @@ static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1)
 		timeout--;
 		if (!timeout) {
 			/* Should *never* happen. we warn and carry on */
-			dev_crit(&up->pdev->dev, "Errata i202: timedout %x\n",
+			dev_crit(up->dev, "Errata i202: timedout %x\n",
 						serial_in(up, UART_LSR));
 			break;
 		}
@@ -1648,29 +1529,23 @@ static int serial_omap_runtime_suspend(struct device *dev)
 	if (!up)
 		return -EINVAL;
 
-	if (!pdata || !pdata->enable_wakeup)
+	if (!pdata)
 		return 0;
 
-	if (pdata->get_context_loss_count)
-		up->context_loss_cnt = pdata->get_context_loss_count(dev);
+	up->context_loss_cnt = serial_omap_get_context_loss_count(up);
 
 	if (device_may_wakeup(dev)) {
 		if (!up->wakeups_enabled) {
-			pdata->enable_wakeup(up->pdev, true);
+			serial_omap_enable_wakeup(up, true);
 			up->wakeups_enabled = true;
 		}
 	} else {
 		if (up->wakeups_enabled) {
-			pdata->enable_wakeup(up->pdev, false);
+			serial_omap_enable_wakeup(up, false);
 			up->wakeups_enabled = false;
 		}
 	}
 
-	/* Errata i291 */
-	if (up->use_dma && pdata->set_forceidle &&
-			(up->errata & UART_ERRATA_i291_DMA_FORCEIDLE))
-		pdata->set_forceidle(up->pdev);
-
 	up->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
 	schedule_work(&up->qos_work);
 
@@ -1680,24 +1555,14 @@ static int serial_omap_runtime_suspend(struct device *dev)
 static int serial_omap_runtime_resume(struct device *dev)
 {
 	struct uart_omap_port *up = dev_get_drvdata(dev);
-	struct omap_uart_port_info *pdata = dev->platform_data;
-
-	if (up && pdata) {
-		if (pdata->get_context_loss_count) {
-			u32 loss_cnt = pdata->get_context_loss_count(dev);
 
-			if (up->context_loss_cnt != loss_cnt)
-				serial_omap_restore_context(up);
-		}
+	u32 loss_cnt = serial_omap_get_context_loss_count(up);
 
-		/* Errata i291 */
-		if (up->use_dma && pdata->set_noidle &&
-				(up->errata & UART_ERRATA_i291_DMA_FORCEIDLE))
-			pdata->set_noidle(up->pdev);
+	if (up->context_loss_cnt != loss_cnt)
+		serial_omap_restore_context(up);
 
-		up->latency = up->calc_latency;
-		schedule_work(&up->qos_work);
-	}
+	up->latency = up->calc_latency;
+	schedule_work(&up->qos_work);
 
 	return 0;
 }
@@ -1721,7 +1586,7 @@ MODULE_DEVICE_TABLE(of, omap_serial_of_match);
 
 static struct platform_driver serial_omap_driver = {
 	.probe          = serial_omap_probe,
-	.remove         = serial_omap_remove,
+	.remove         = __devexit_p(serial_omap_remove),
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.pm	= &serial_omap_dev_pm_ops,
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index 558ce8509a9a..4cd6c2381528 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -979,6 +979,10 @@ static unsigned int dma_handle_tx(struct eg20t_port *priv)
 	priv->tx_dma_use = 1;
 
 	priv->sg_tx_p = kzalloc(sizeof(struct scatterlist)*num, GFP_ATOMIC);
+	if (!priv->sg_tx_p) {
+		dev_err(priv->port.dev, "%s:kzalloc Failed\n", __func__);
+		return 0;
+	}
 
 	sg_init_table(priv->sg_tx_p, num); /* Initialize SG table */
 	sg = priv->sg_tx_p;
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index 5847a4b855f7..9033fc6e0e4e 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -670,9 +670,19 @@ serial_pxa_console_write(struct console *co, const char *s, unsigned int count)
 {
 	struct uart_pxa_port *up = serial_pxa_ports[co->index];
 	unsigned int ier;
+	unsigned long flags;
+	int locked = 1;
 
 	clk_prepare_enable(up->clk);
 
+	local_irq_save(flags);
+	if (up->port.sysrq)
+		locked = 0;
+	else if (oops_in_progress)
+		locked = spin_trylock(&up->port.lock);
+	else
+		spin_lock(&up->port.lock);
+
 	/*
 	 *	First save the IER then disable the interrupts
 	 */
@@ -688,6 +698,10 @@ serial_pxa_console_write(struct console *co, const char *s, unsigned int count)
 	wait_for_xmitr(up);
 	serial_out(up, UART_IER, ier);
 
+	if (locked)
+		spin_unlock(&up->port.lock);
+	local_irq_restore(flags);
+
 	clk_disable_unprepare(up->clk);
 }
 
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index 02d07bfcfa8a..7f04717176aa 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -82,7 +82,7 @@ static inline const char *s3c24xx_serial_portname(struct uart_port *port)
 
 static int s3c24xx_serial_txempty_nofifo(struct uart_port *port)
 {
-	return (rd_regl(port, S3C2410_UTRSTAT) & S3C2410_UTRSTAT_TXE);
+	return rd_regl(port, S3C2410_UTRSTAT) & S3C2410_UTRSTAT_TXE;
 }
 
 /*
@@ -268,7 +268,7 @@ s3c24xx_serial_rx_chars(int irq, void *dev_id)
 				dbg("break!\n");
 				port->icount.brk++;
 				if (uart_handle_break(port))
-				    goto ignore_char;
+					goto ignore_char;
 			}
 
 			if (uerstat & S3C2410_UERSTAT_FRAME)
@@ -459,7 +459,7 @@ static int s3c24xx_serial_startup(struct uart_port *port)
 			  s3c24xx_serial_portname(port), ourport);
 
 	if (ret != 0) {
-		printk(KERN_ERR "cannot get irq %d\n", ourport->rx_irq);
+		dev_err(port->dev, "cannot get irq %d\n", ourport->rx_irq);
 		return ret;
 	}
 
@@ -473,7 +473,7 @@ static int s3c24xx_serial_startup(struct uart_port *port)
 			  s3c24xx_serial_portname(port), ourport);
 
 	if (ret) {
-		printk(KERN_ERR "cannot get irq %d\n", ourport->tx_irq);
+		dev_err(port->dev, "cannot get irq %d\n", ourport->tx_irq);
 		goto err;
 	}
 
@@ -502,7 +502,7 @@ static int s3c64xx_serial_startup(struct uart_port *port)
 	ret = request_irq(port->irq, s3c64xx_serial_handle_irq, IRQF_SHARED,
 			  s3c24xx_serial_portname(port), ourport);
 	if (ret) {
-		printk(KERN_ERR "cannot get irq %d\n", port->irq);
+		dev_err(port->dev, "cannot get irq %d\n", port->irq);
 		return ret;
 	}
 
@@ -529,7 +529,7 @@ static void s3c24xx_serial_pm(struct uart_port *port, unsigned int level,
 
 	switch (level) {
 	case 3:
-		if (!IS_ERR(ourport->baudclk) && ourport->baudclk != NULL)
+		if (!IS_ERR(ourport->baudclk))
 			clk_disable(ourport->baudclk);
 
 		clk_disable(ourport->clk);
@@ -538,12 +538,12 @@ static void s3c24xx_serial_pm(struct uart_port *port, unsigned int level,
 	case 0:
 		clk_enable(ourport->clk);
 
-		if (!IS_ERR(ourport->baudclk) && ourport->baudclk != NULL)
+		if (!IS_ERR(ourport->baudclk))
 			clk_enable(ourport->baudclk);
 
 		break;
 	default:
-		printk(KERN_ERR "s3c24xx_serial: unknown pm %d\n", level);
+		dev_err(port->dev, "s3c24xx_serial: unknown pm %d\n", level);
 	}
 }
 
@@ -604,7 +604,6 @@ static unsigned int s3c24xx_serial_getclk(struct s3c24xx_uart_port *ourport,
 	char clkname[MAX_CLK_NAME_LENGTH];
 	int calc_deviation, deviation = (1 << 30) - 1;
 
-	*best_clk = NULL;
 	clk_sel = (ourport->cfg->clk_sel) ? ourport->cfg->clk_sel :
 			ourport->info->def_clk_sel;
 	for (cnt = 0; cnt < info->num_clks; cnt++) {
@@ -613,7 +612,7 @@ static unsigned int s3c24xx_serial_getclk(struct s3c24xx_uart_port *ourport,
 
 		sprintf(clkname, "clk_uart_baud%d", cnt);
 		clk = clk_get(ourport->port.dev, clkname);
-		if (IS_ERR_OR_NULL(clk))
+		if (IS_ERR(clk))
 			continue;
 
 		rate = clk_get_rate(clk);
@@ -684,7 +683,7 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 {
 	struct s3c2410_uartcfg *cfg = s3c24xx_port_to_cfg(port);
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
-	struct clk *clk = NULL;
+	struct clk *clk = ERR_PTR(-EINVAL);
 	unsigned long flags;
 	unsigned int baud, quot, clk_sel = 0;
 	unsigned int ulcon;
@@ -705,7 +704,7 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 	quot = s3c24xx_serial_getclk(ourport, baud, &clk, &clk_sel);
 	if (baud == 38400 && (port->flags & UPF_SPD_MASK) == UPF_SPD_CUST)
 		quot = port->custom_divisor;
-	if (!clk)
+	if (IS_ERR(clk))
 		return;
 
 	/* check to see if we need  to change clock source */
@@ -713,9 +712,9 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 	if (ourport->baudclk != clk) {
 		s3c24xx_serial_setsource(port, clk_sel);
 
-		if (ourport->baudclk != NULL && !IS_ERR(ourport->baudclk)) {
+		if (!IS_ERR(ourport->baudclk)) {
 			clk_disable(ourport->baudclk);
-			ourport->baudclk  = NULL;
+			ourport->baudclk = ERR_PTR(-EINVAL);
 		}
 
 		clk_enable(clk);
@@ -877,11 +876,24 @@ s3c24xx_serial_verify_port(struct uart_port *port, struct serial_struct *ser)
 
 static struct console s3c24xx_serial_console;
 
+static int __init s3c24xx_serial_console_init(void)
+{
+	register_console(&s3c24xx_serial_console);
+	return 0;
+}
+console_initcall(s3c24xx_serial_console_init);
+
 #define S3C24XX_SERIAL_CONSOLE &s3c24xx_serial_console
 #else
 #define S3C24XX_SERIAL_CONSOLE NULL
 #endif
 
+#ifdef CONFIG_CONSOLE_POLL
+static int s3c24xx_serial_get_poll_char(struct uart_port *port);
+static void s3c24xx_serial_put_poll_char(struct uart_port *port,
+			 unsigned char c);
+#endif
+
 static struct uart_ops s3c24xx_serial_ops = {
 	.pm		= s3c24xx_serial_pm,
 	.tx_empty	= s3c24xx_serial_tx_empty,
@@ -900,6 +912,10 @@ static struct uart_ops s3c24xx_serial_ops = {
 	.request_port	= s3c24xx_serial_request_port,
 	.config_port	= s3c24xx_serial_config_port,
 	.verify_port	= s3c24xx_serial_verify_port,
+#ifdef CONFIG_CONSOLE_POLL
+	.poll_get_char = s3c24xx_serial_get_poll_char,
+	.poll_put_char = s3c24xx_serial_put_poll_char,
+#endif
 };
 
 static struct uart_driver s3c24xx_uart_drv = {
@@ -1036,10 +1052,10 @@ static int s3c24xx_serial_cpufreq_transition(struct notifier_block *nb,
 		if (tty == NULL)
 			goto exit;
 
-		termios = tty->termios;
+		termios = &tty->termios;
 
 		if (termios == NULL) {
-			printk(KERN_WARNING "%s: no termios?\n", __func__);
+			dev_warn(uport->dev, "%s: no termios?\n", __func__);
 			goto exit;
 		}
 
@@ -1114,7 +1130,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 
 	res = platform_get_resource(platdev, IORESOURCE_MEM, 0);
 	if (res == NULL) {
-		printk(KERN_ERR "failed to find memory resource for uart\n");
+		dev_err(port->dev, "failed to find memory resource for uart\n");
 		return -EINVAL;
 	}
 
@@ -1130,7 +1146,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 		ourport->rx_irq = ret;
 		ourport->tx_irq = ret + 1;
 	}
-	
+
 	ret = platform_get_irq(platdev, 1);
 	if (ret > 0)
 		ourport->tx_irq = ret;
@@ -1160,7 +1176,11 @@ static ssize_t s3c24xx_serial_show_clksrc(struct device *dev,
 	struct uart_port *port = s3c24xx_dev_to_port(dev);
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 
-	return snprintf(buf, PAGE_SIZE, "* %s\n", ourport->baudclk->name);
+	if (IS_ERR(ourport->baudclk))
+		return -EINVAL;
+
+	return snprintf(buf, PAGE_SIZE, "* %s\n",
+			ourport->baudclk->name ?: "(null)");
 }
 
 static DEVICE_ATTR(clock_source, S_IRUGO, s3c24xx_serial_show_clksrc, NULL);
@@ -1200,6 +1220,7 @@ static int s3c24xx_serial_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	ourport->baudclk = ERR_PTR(-EINVAL);
 	ourport->info = ourport->drv_data->info;
 	ourport->cfg = (pdev->dev.platform_data) ?
 			(struct s3c2410_uartcfg *)pdev->dev.platform_data :
@@ -1312,6 +1333,36 @@ s3c24xx_serial_console_txrdy(struct uart_port *port, unsigned int ufcon)
 	return (utrstat & S3C2410_UTRSTAT_TXE) ? 1 : 0;
 }
 
+#ifdef CONFIG_CONSOLE_POLL
+/*
+ * Console polling routines for writing and reading from the uart while
+ * in an interrupt or debug context.
+ */
+
+static int s3c24xx_serial_get_poll_char(struct uart_port *port)
+{
+	struct s3c24xx_uart_port *ourport = to_ourport(port);
+	unsigned int ufstat;
+
+	ufstat = rd_regl(port, S3C2410_UFSTAT);
+	if (s3c24xx_serial_rx_fifocnt(ourport, ufstat) == 0)
+		return NO_POLL_CHAR;
+
+	return rd_regb(port, S3C2410_URXH);
+}
+
+static void s3c24xx_serial_put_poll_char(struct uart_port *port,
+		unsigned char c)
+{
+	unsigned int ufcon = rd_regl(cons_uart, S3C2410_UFCON);
+
+	while (!s3c24xx_serial_console_txrdy(port, ufcon))
+		cpu_relax();
+	wr_regb(cons_uart, S3C2410_UTXH, c);
+}
+
+#endif /* CONFIG_CONSOLE_POLL */
+
 static void
 s3c24xx_serial_console_putchar(struct uart_port *port, int ch)
 {
@@ -1387,7 +1438,7 @@ s3c24xx_serial_get_options(struct uart_port *port, int *baud,
 		sprintf(clk_name, "clk_uart_baud%d", clk_sel);
 
 		clk = clk_get(port->dev, clk_name);
-		if (!IS_ERR(clk) && clk != NULL)
+		if (!IS_ERR(clk))
 			rate = clk_get_rate(clk);
 		else
 			rate = 1;
@@ -1679,8 +1730,8 @@ static int __init s3c24xx_serial_modinit(void)
 
 	ret = uart_register_driver(&s3c24xx_uart_drv);
 	if (ret < 0) {
-		printk(KERN_ERR "failed to register UART driver\n");
-		return -1;
+		pr_err("Failed to register Samsung UART driver\n");
+		return ret;
 	}
 
 	return platform_driver_register(&samsung_serial_driver);
diff --git a/drivers/tty/serial/sc26xx.c b/drivers/tty/serial/sc26xx.c
index e0b4b0a30a5a..9d664242b312 100644
--- a/drivers/tty/serial/sc26xx.c
+++ b/drivers/tty/serial/sc26xx.c
@@ -20,6 +20,10 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/irq.h>
+#include <linux/io.h>
+
+#warning "Please try migrate to use new driver SCCNXP and report the status" \
+	 "in the linux-serial mailing list."
 
 #if defined(CONFIG_MAGIC_SYSRQ)
 #define SUPPORT_SYSRQ
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
new file mode 100644
index 000000000000..b7086d004f5f
--- /dev/null
+++ b/drivers/tty/serial/sccnxp.c
@@ -0,0 +1,990 @@
+/*
+ *  NXP (Philips) SCC+++(SCN+++) serial driver
+ *
+ *  Copyright (C) 2012 Alexander Shiyan <shc_work@mail.ru>
+ *
+ *  Based on sc26xx.c, by Thomas Bogendörfer (tsbogend@alpha.franken.de)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#if defined(CONFIG_SERIAL_SCCNXP_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/console.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/io.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/sccnxp.h>
+
+#define SCCNXP_NAME			"uart-sccnxp"
+#define SCCNXP_MAJOR			204
+#define SCCNXP_MINOR			205
+
+#define SCCNXP_MR_REG			(0x00)
+#	define MR0_BAUD_NORMAL		(0 << 0)
+#	define MR0_BAUD_EXT1		(1 << 0)
+#	define MR0_BAUD_EXT2		(5 << 0)
+#	define MR0_FIFO			(1 << 3)
+#	define MR0_TXLVL		(1 << 4)
+#	define MR1_BITS_5		(0 << 0)
+#	define MR1_BITS_6		(1 << 0)
+#	define MR1_BITS_7		(2 << 0)
+#	define MR1_BITS_8		(3 << 0)
+#	define MR1_PAR_EVN		(0 << 2)
+#	define MR1_PAR_ODD		(1 << 2)
+#	define MR1_PAR_NO		(4 << 2)
+#	define MR2_STOP1		(7 << 0)
+#	define MR2_STOP2		(0xf << 0)
+#define SCCNXP_SR_REG			(0x01)
+#define SCCNXP_CSR_REG			SCCNXP_SR_REG
+#	define SR_RXRDY			(1 << 0)
+#	define SR_FULL			(1 << 1)
+#	define SR_TXRDY			(1 << 2)
+#	define SR_TXEMT			(1 << 3)
+#	define SR_OVR			(1 << 4)
+#	define SR_PE			(1 << 5)
+#	define SR_FE			(1 << 6)
+#	define SR_BRK			(1 << 7)
+#define SCCNXP_CR_REG			(0x02)
+#	define CR_RX_ENABLE		(1 << 0)
+#	define CR_RX_DISABLE		(1 << 1)
+#	define CR_TX_ENABLE		(1 << 2)
+#	define CR_TX_DISABLE		(1 << 3)
+#	define CR_CMD_MRPTR1		(0x01 << 4)
+#	define CR_CMD_RX_RESET		(0x02 << 4)
+#	define CR_CMD_TX_RESET		(0x03 << 4)
+#	define CR_CMD_STATUS_RESET	(0x04 << 4)
+#	define CR_CMD_BREAK_RESET	(0x05 << 4)
+#	define CR_CMD_START_BREAK	(0x06 << 4)
+#	define CR_CMD_STOP_BREAK	(0x07 << 4)
+#	define CR_CMD_MRPTR0		(0x0b << 4)
+#define SCCNXP_RHR_REG			(0x03)
+#define SCCNXP_THR_REG			SCCNXP_RHR_REG
+#define SCCNXP_IPCR_REG			(0x04)
+#define SCCNXP_ACR_REG			SCCNXP_IPCR_REG
+#	define ACR_BAUD0		(0 << 7)
+#	define ACR_BAUD1		(1 << 7)
+#	define ACR_TIMER_MODE		(6 << 4)
+#define SCCNXP_ISR_REG			(0x05)
+#define SCCNXP_IMR_REG			SCCNXP_ISR_REG
+#	define IMR_TXRDY		(1 << 0)
+#	define IMR_RXRDY		(1 << 1)
+#	define ISR_TXRDY(x)		(1 << ((x * 4) + 0))
+#	define ISR_RXRDY(x)		(1 << ((x * 4) + 1))
+#define SCCNXP_IPR_REG			(0x0d)
+#define SCCNXP_OPCR_REG			SCCNXP_IPR_REG
+#define SCCNXP_SOP_REG			(0x0e)
+#define SCCNXP_ROP_REG			(0x0f)
+
+/* Route helpers */
+#define MCTRL_MASK(sig)			(0xf << (sig))
+#define MCTRL_IBIT(cfg, sig)		((((cfg) >> (sig)) & 0xf) - LINE_IP0)
+#define MCTRL_OBIT(cfg, sig)		((((cfg) >> (sig)) & 0xf) - LINE_OP0)
+
+/* Supported chip types */
+enum {
+	SCCNXP_TYPE_SC2681	= 2681,
+	SCCNXP_TYPE_SC2691	= 2691,
+	SCCNXP_TYPE_SC2692	= 2692,
+	SCCNXP_TYPE_SC2891	= 2891,
+	SCCNXP_TYPE_SC2892	= 2892,
+	SCCNXP_TYPE_SC28202	= 28202,
+	SCCNXP_TYPE_SC68681	= 68681,
+	SCCNXP_TYPE_SC68692	= 68692,
+};
+
+struct sccnxp_port {
+	struct uart_driver	uart;
+	struct uart_port	port[SCCNXP_MAX_UARTS];
+
+	const char		*name;
+	int			irq;
+
+	u8			imr;
+	u8			addr_mask;
+	int			freq_std;
+
+	int			flags;
+#define SCCNXP_HAVE_IO		0x00000001
+#define SCCNXP_HAVE_MR0		0x00000002
+
+#ifdef CONFIG_SERIAL_SCCNXP_CONSOLE
+	struct console		console;
+#endif
+
+	struct mutex		sccnxp_mutex;
+
+	struct sccnxp_pdata	pdata;
+};
+
+static inline u8 sccnxp_raw_read(void __iomem *base, u8 reg, u8 shift)
+{
+	return readb(base + (reg << shift));
+}
+
+static inline void sccnxp_raw_write(void __iomem *base, u8 reg, u8 shift, u8 v)
+{
+	writeb(v, base + (reg << shift));
+}
+
+static inline u8 sccnxp_read(struct uart_port *port, u8 reg)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	return sccnxp_raw_read(port->membase, reg & s->addr_mask,
+			       port->regshift);
+}
+
+static inline void sccnxp_write(struct uart_port *port, u8 reg, u8 v)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	sccnxp_raw_write(port->membase, reg & s->addr_mask, port->regshift, v);
+}
+
+static inline u8 sccnxp_port_read(struct uart_port *port, u8 reg)
+{
+	return sccnxp_read(port, (port->line << 3) + reg);
+}
+
+static inline void sccnxp_port_write(struct uart_port *port, u8 reg, u8 v)
+{
+	sccnxp_write(port, (port->line << 3) + reg, v);
+}
+
+static int sccnxp_update_best_err(int a, int b, int *besterr)
+{
+	int err = abs(a - b);
+
+	if ((*besterr < 0) || (*besterr > err)) {
+		*besterr = err;
+		return 0;
+	}
+
+	return 1;
+}
+
+struct baud_table {
+	u8	csr;
+	u8	acr;
+	u8	mr0;
+	int	baud;
+};
+
+const struct baud_table baud_std[] = {
+	{ 0,	ACR_BAUD0,	MR0_BAUD_NORMAL,	50, },
+	{ 0,	ACR_BAUD1,	MR0_BAUD_NORMAL,	75, },
+	{ 1,	ACR_BAUD0,	MR0_BAUD_NORMAL,	110, },
+	{ 2,	ACR_BAUD0,	MR0_BAUD_NORMAL,	134, },
+	{ 3,	ACR_BAUD1,	MR0_BAUD_NORMAL,	150, },
+	{ 3,	ACR_BAUD0,	MR0_BAUD_NORMAL,	200, },
+	{ 4,	ACR_BAUD0,	MR0_BAUD_NORMAL,	300, },
+	{ 0,	ACR_BAUD1,	MR0_BAUD_EXT1,		450, },
+	{ 1,	ACR_BAUD0,	MR0_BAUD_EXT2,		880, },
+	{ 3,	ACR_BAUD1,	MR0_BAUD_EXT1,		900, },
+	{ 5,	ACR_BAUD0,	MR0_BAUD_NORMAL,	600, },
+	{ 7,	ACR_BAUD0,	MR0_BAUD_NORMAL,	1050, },
+	{ 2,	ACR_BAUD0,	MR0_BAUD_EXT2,		1076, },
+	{ 6,	ACR_BAUD0,	MR0_BAUD_NORMAL,	1200, },
+	{ 10,	ACR_BAUD1,	MR0_BAUD_NORMAL,	1800, },
+	{ 7,	ACR_BAUD1,	MR0_BAUD_NORMAL,	2000, },
+	{ 8,	ACR_BAUD0,	MR0_BAUD_NORMAL,	2400, },
+	{ 5,	ACR_BAUD1,	MR0_BAUD_EXT1,		3600, },
+	{ 9,	ACR_BAUD0,	MR0_BAUD_NORMAL,	4800, },
+	{ 10,	ACR_BAUD0,	MR0_BAUD_NORMAL,	7200, },
+	{ 11,	ACR_BAUD0,	MR0_BAUD_NORMAL,	9600, },
+	{ 8,	ACR_BAUD0,	MR0_BAUD_EXT1,		14400, },
+	{ 12,	ACR_BAUD1,	MR0_BAUD_NORMAL,	19200, },
+	{ 9,	ACR_BAUD0,	MR0_BAUD_EXT1,		28800, },
+	{ 12,	ACR_BAUD0,	MR0_BAUD_NORMAL,	38400, },
+	{ 11,	ACR_BAUD0,	MR0_BAUD_EXT1,		57600, },
+	{ 12,	ACR_BAUD1,	MR0_BAUD_EXT1,		115200, },
+	{ 12,	ACR_BAUD0,	MR0_BAUD_EXT1,		230400, },
+	{ 0, 0, 0, 0 }
+};
+
+static int sccnxp_set_baud(struct uart_port *port, int baud)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+	int div_std, tmp_baud, bestbaud = baud, besterr = -1;
+	u8 i, acr = 0, csr = 0, mr0 = 0;
+
+	/* Find best baud from table */
+	for (i = 0; baud_std[i].baud && besterr; i++) {
+		if (baud_std[i].mr0 && !(s->flags & SCCNXP_HAVE_MR0))
+			continue;
+		div_std = DIV_ROUND_CLOSEST(s->freq_std, baud_std[i].baud);
+		tmp_baud = DIV_ROUND_CLOSEST(port->uartclk, div_std);
+		if (!sccnxp_update_best_err(baud, tmp_baud, &besterr)) {
+			acr = baud_std[i].acr;
+			csr = baud_std[i].csr;
+			mr0 = baud_std[i].mr0;
+			bestbaud = tmp_baud;
+		}
+	}
+
+	if (s->flags & SCCNXP_HAVE_MR0) {
+		/* Enable FIFO, set half level for TX */
+		mr0 |= MR0_FIFO | MR0_TXLVL;
+		/* Update MR0 */
+		sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_MRPTR0);
+		sccnxp_port_write(port, SCCNXP_MR_REG, mr0);
+	}
+
+	sccnxp_port_write(port, SCCNXP_ACR_REG, acr | ACR_TIMER_MODE);
+	sccnxp_port_write(port, SCCNXP_CSR_REG, (csr << 4) | csr);
+
+	if (baud != bestbaud)
+		dev_dbg(port->dev, "Baudrate desired: %i, calculated: %i\n",
+			baud, bestbaud);
+
+	return bestbaud;
+}
+
+static void sccnxp_enable_irq(struct uart_port *port, int mask)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	s->imr |= mask << (port->line * 4);
+	sccnxp_write(port, SCCNXP_IMR_REG, s->imr);
+}
+
+static void sccnxp_disable_irq(struct uart_port *port, int mask)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	s->imr &= ~(mask << (port->line * 4));
+	sccnxp_write(port, SCCNXP_IMR_REG, s->imr);
+}
+
+static void sccnxp_set_bit(struct uart_port *port, int sig, int state)
+{
+	u8 bitmask;
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	if (s->pdata.mctrl_cfg[port->line] & MCTRL_MASK(sig)) {
+		bitmask = 1 << MCTRL_OBIT(s->pdata.mctrl_cfg[port->line], sig);
+		if (state)
+			sccnxp_write(port, SCCNXP_SOP_REG, bitmask);
+		else
+			sccnxp_write(port, SCCNXP_ROP_REG, bitmask);
+	}
+}
+
+static void sccnxp_handle_rx(struct uart_port *port)
+{
+	u8 sr;
+	unsigned int ch, flag;
+	struct tty_struct *tty = tty_port_tty_get(&port->state->port);
+
+	if (!tty)
+		return;
+
+	for (;;) {
+		sr = sccnxp_port_read(port, SCCNXP_SR_REG);
+		if (!(sr & SR_RXRDY))
+			break;
+		sr &= SR_PE | SR_FE | SR_OVR | SR_BRK;
+
+		ch = sccnxp_port_read(port, SCCNXP_RHR_REG);
+
+		port->icount.rx++;
+		flag = TTY_NORMAL;
+
+		if (unlikely(sr)) {
+			if (sr & SR_BRK) {
+				port->icount.brk++;
+				if (uart_handle_break(port))
+					continue;
+			} else if (sr & SR_PE)
+				port->icount.parity++;
+			else if (sr & SR_FE)
+				port->icount.frame++;
+			else if (sr & SR_OVR)
+				port->icount.overrun++;
+
+			sr &= port->read_status_mask;
+			if (sr & SR_BRK)
+				flag = TTY_BREAK;
+			else if (sr & SR_PE)
+				flag = TTY_PARITY;
+			else if (sr & SR_FE)
+				flag = TTY_FRAME;
+			else if (sr & SR_OVR)
+				flag = TTY_OVERRUN;
+		}
+
+		if (uart_handle_sysrq_char(port, ch))
+			continue;
+
+		if (sr & port->ignore_status_mask)
+			continue;
+
+		uart_insert_char(port, sr, SR_OVR, ch, flag);
+	}
+
+	tty_flip_buffer_push(tty);
+
+	tty_kref_put(tty);
+}
+
+static void sccnxp_handle_tx(struct uart_port *port)
+{
+	u8 sr;
+	struct circ_buf *xmit = &port->state->xmit;
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	if (unlikely(port->x_char)) {
+		sccnxp_port_write(port, SCCNXP_THR_REG, port->x_char);
+		port->icount.tx++;
+		port->x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port)) {
+		/* Disable TX if FIFO is empty */
+		if (sccnxp_port_read(port, SCCNXP_SR_REG) & SR_TXEMT) {
+			sccnxp_disable_irq(port, IMR_TXRDY);
+
+			/* Set direction to input */
+			if (s->flags & SCCNXP_HAVE_IO)
+				sccnxp_set_bit(port, DIR_OP, 0);
+		}
+		return;
+	}
+
+	while (!uart_circ_empty(xmit)) {
+		sr = sccnxp_port_read(port, SCCNXP_SR_REG);
+		if (!(sr & SR_TXRDY))
+			break;
+
+		sccnxp_port_write(port, SCCNXP_THR_REG, xmit->buf[xmit->tail]);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		port->icount.tx++;
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+}
+
+static irqreturn_t sccnxp_ist(int irq, void *dev_id)
+{
+	int i;
+	u8 isr;
+	struct sccnxp_port *s = (struct sccnxp_port *)dev_id;
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	for (;;) {
+		isr = sccnxp_read(&s->port[0], SCCNXP_ISR_REG);
+		isr &= s->imr;
+		if (!isr)
+			break;
+
+		dev_dbg(s->port[0].dev, "IRQ status: 0x%02x\n", isr);
+
+		for (i = 0; i < s->uart.nr; i++) {
+			if (isr & ISR_RXRDY(i))
+				sccnxp_handle_rx(&s->port[i]);
+			if (isr & ISR_TXRDY(i))
+				sccnxp_handle_tx(&s->port[i]);
+		}
+	}
+
+	mutex_unlock(&s->sccnxp_mutex);
+
+	return IRQ_HANDLED;
+}
+
+static void sccnxp_start_tx(struct uart_port *port)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	/* Set direction to output */
+	if (s->flags & SCCNXP_HAVE_IO)
+		sccnxp_set_bit(port, DIR_OP, 1);
+
+	sccnxp_enable_irq(port, IMR_TXRDY);
+
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static void sccnxp_stop_tx(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static void sccnxp_stop_rx(struct uart_port *port)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_RX_DISABLE);
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static unsigned int sccnxp_tx_empty(struct uart_port *port)
+{
+	u8 val;
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+	val = sccnxp_port_read(port, SCCNXP_SR_REG);
+	mutex_unlock(&s->sccnxp_mutex);
+
+	return (val & SR_TXEMT) ? TIOCSER_TEMT : 0;
+}
+
+static void sccnxp_enable_ms(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static void sccnxp_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	if (!(s->flags & SCCNXP_HAVE_IO))
+		return;
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	sccnxp_set_bit(port, DTR_OP, mctrl & TIOCM_DTR);
+	sccnxp_set_bit(port, RTS_OP, mctrl & TIOCM_RTS);
+
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static unsigned int sccnxp_get_mctrl(struct uart_port *port)
+{
+	u8 bitmask, ipr;
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+	unsigned int mctrl = TIOCM_DSR | TIOCM_CTS | TIOCM_CAR;
+
+	if (!(s->flags & SCCNXP_HAVE_IO))
+		return mctrl;
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	ipr = ~sccnxp_read(port, SCCNXP_IPCR_REG);
+
+	if (s->pdata.mctrl_cfg[port->line] & MCTRL_MASK(DSR_IP)) {
+		bitmask = 1 << MCTRL_IBIT(s->pdata.mctrl_cfg[port->line],
+					  DSR_IP);
+		mctrl &= ~TIOCM_DSR;
+		mctrl |= (ipr & bitmask) ? TIOCM_DSR : 0;
+	}
+	if (s->pdata.mctrl_cfg[port->line] & MCTRL_MASK(CTS_IP)) {
+		bitmask = 1 << MCTRL_IBIT(s->pdata.mctrl_cfg[port->line],
+					  CTS_IP);
+		mctrl &= ~TIOCM_CTS;
+		mctrl |= (ipr & bitmask) ? TIOCM_CTS : 0;
+	}
+	if (s->pdata.mctrl_cfg[port->line] & MCTRL_MASK(DCD_IP)) {
+		bitmask = 1 << MCTRL_IBIT(s->pdata.mctrl_cfg[port->line],
+					  DCD_IP);
+		mctrl &= ~TIOCM_CAR;
+		mctrl |= (ipr & bitmask) ? TIOCM_CAR : 0;
+	}
+	if (s->pdata.mctrl_cfg[port->line] & MCTRL_MASK(RNG_IP)) {
+		bitmask = 1 << MCTRL_IBIT(s->pdata.mctrl_cfg[port->line],
+					  RNG_IP);
+		mctrl &= ~TIOCM_RNG;
+		mctrl |= (ipr & bitmask) ? TIOCM_RNG : 0;
+	}
+
+	mutex_unlock(&s->sccnxp_mutex);
+
+	return mctrl;
+}
+
+static void sccnxp_break_ctl(struct uart_port *port, int break_state)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+	sccnxp_port_write(port, SCCNXP_CR_REG, break_state ?
+			  CR_CMD_START_BREAK : CR_CMD_STOP_BREAK);
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static void sccnxp_set_termios(struct uart_port *port,
+			       struct ktermios *termios, struct ktermios *old)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+	u8 mr1, mr2;
+	int baud;
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	/* Mask termios capabilities we don't support */
+	termios->c_cflag &= ~CMSPAR;
+
+	/* Disable RX & TX, reset break condition, status and FIFOs */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_RX_RESET |
+					       CR_RX_DISABLE | CR_TX_DISABLE);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_TX_RESET);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_STATUS_RESET);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_BREAK_RESET);
+
+	/* Word size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		mr1 = MR1_BITS_5;
+		break;
+	case CS6:
+		mr1 = MR1_BITS_6;
+		break;
+	case CS7:
+		mr1 = MR1_BITS_7;
+		break;
+	case CS8:
+	default:
+		mr1 = MR1_BITS_8;
+		break;
+	}
+
+	/* Parity */
+	if (termios->c_cflag & PARENB) {
+		if (termios->c_cflag & PARODD)
+			mr1 |= MR1_PAR_ODD;
+	} else
+		mr1 |= MR1_PAR_NO;
+
+	/* Stop bits */
+	mr2 = (termios->c_cflag & CSTOPB) ? MR2_STOP2 : MR2_STOP1;
+
+	/* Update desired format */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_MRPTR1);
+	sccnxp_port_write(port, SCCNXP_MR_REG, mr1);
+	sccnxp_port_write(port, SCCNXP_MR_REG, mr2);
+
+	/* Set read status mask */
+	port->read_status_mask = SR_OVR;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= SR_PE | SR_FE;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= SR_BRK;
+
+	/* Set status ignore mask */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNBRK)
+		port->ignore_status_mask |= SR_BRK;
+	if (!(termios->c_cflag & CREAD))
+		port->ignore_status_mask |= SR_PE | SR_OVR | SR_FE | SR_BRK;
+
+	/* Setup baudrate */
+	baud = uart_get_baud_rate(port, termios, old, 50,
+				  (s->flags & SCCNXP_HAVE_MR0) ?
+				  230400 : 38400);
+	baud = sccnxp_set_baud(port, baud);
+
+	/* Update timeout according to new baud rate */
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+
+	/* Enable RX & TX */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_RX_ENABLE | CR_TX_ENABLE);
+
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static int sccnxp_startup(struct uart_port *port)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	if (s->flags & SCCNXP_HAVE_IO) {
+		/* Outputs are controlled manually */
+		sccnxp_write(port, SCCNXP_OPCR_REG, 0);
+	}
+
+	/* Reset break condition, status and FIFOs */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_RX_RESET);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_TX_RESET);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_STATUS_RESET);
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_CMD_BREAK_RESET);
+
+	/* Enable RX & TX */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_RX_ENABLE | CR_TX_ENABLE);
+
+	/* Enable RX interrupt */
+	sccnxp_enable_irq(port, IMR_RXRDY);
+
+	mutex_unlock(&s->sccnxp_mutex);
+
+	return 0;
+}
+
+static void sccnxp_shutdown(struct uart_port *port)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	mutex_lock(&s->sccnxp_mutex);
+
+	/* Disable interrupts */
+	sccnxp_disable_irq(port, IMR_TXRDY | IMR_RXRDY);
+
+	/* Disable TX & RX */
+	sccnxp_port_write(port, SCCNXP_CR_REG, CR_RX_DISABLE | CR_TX_DISABLE);
+
+	/* Leave direction to input */
+	if (s->flags & SCCNXP_HAVE_IO)
+		sccnxp_set_bit(port, DIR_OP, 0);
+
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static const char *sccnxp_type(struct uart_port *port)
+{
+	struct sccnxp_port *s = dev_get_drvdata(port->dev);
+
+	return (port->type == PORT_SC26XX) ? s->name : NULL;
+}
+
+static void sccnxp_release_port(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static int sccnxp_request_port(struct uart_port *port)
+{
+	/* Do nothing */
+	return 0;
+}
+
+static void sccnxp_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		port->type = PORT_SC26XX;
+}
+
+static int sccnxp_verify_port(struct uart_port *port, struct serial_struct *s)
+{
+	if ((s->type == PORT_UNKNOWN) || (s->type == PORT_SC26XX))
+		return 0;
+	if (s->irq == port->irq)
+		return 0;
+
+	return -EINVAL;
+}
+
+static const struct uart_ops sccnxp_ops = {
+	.tx_empty	= sccnxp_tx_empty,
+	.set_mctrl	= sccnxp_set_mctrl,
+	.get_mctrl	= sccnxp_get_mctrl,
+	.stop_tx	= sccnxp_stop_tx,
+	.start_tx	= sccnxp_start_tx,
+	.stop_rx	= sccnxp_stop_rx,
+	.enable_ms	= sccnxp_enable_ms,
+	.break_ctl	= sccnxp_break_ctl,
+	.startup	= sccnxp_startup,
+	.shutdown	= sccnxp_shutdown,
+	.set_termios	= sccnxp_set_termios,
+	.type		= sccnxp_type,
+	.release_port	= sccnxp_release_port,
+	.request_port	= sccnxp_request_port,
+	.config_port	= sccnxp_config_port,
+	.verify_port	= sccnxp_verify_port,
+};
+
+#ifdef CONFIG_SERIAL_SCCNXP_CONSOLE
+static void sccnxp_console_putchar(struct uart_port *port, int c)
+{
+	int tryes = 100000;
+
+	while (tryes--) {
+		if (sccnxp_port_read(port, SCCNXP_SR_REG) & SR_TXRDY) {
+			sccnxp_port_write(port, SCCNXP_THR_REG, c);
+			break;
+		}
+		barrier();
+	}
+}
+
+static void sccnxp_console_write(struct console *co, const char *c, unsigned n)
+{
+	struct sccnxp_port *s = (struct sccnxp_port *)co->data;
+	struct uart_port *port = &s->port[co->index];
+
+	mutex_lock(&s->sccnxp_mutex);
+	uart_console_write(port, c, n, sccnxp_console_putchar);
+	mutex_unlock(&s->sccnxp_mutex);
+}
+
+static int sccnxp_console_setup(struct console *co, char *options)
+{
+	struct sccnxp_port *s = (struct sccnxp_port *)co->data;
+	struct uart_port *port = &s->port[(co->index > 0) ? co->index : 0];
+	int baud = 9600, bits = 8, parity = 'n', flow = 'n';
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+#endif
+
+static int __devinit sccnxp_probe(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int chiptype = pdev->id_entry->driver_data;
+	struct sccnxp_pdata *pdata = dev_get_platdata(&pdev->dev);
+	int i, ret, fifosize, freq_min, freq_max;
+	struct sccnxp_port *s;
+	void __iomem *membase;
+
+	if (!res) {
+		dev_err(&pdev->dev, "Missing memory resource data\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	dev_set_name(&pdev->dev, SCCNXP_NAME);
+
+	s = devm_kzalloc(&pdev->dev, sizeof(struct sccnxp_port), GFP_KERNEL);
+	if (!s) {
+		dev_err(&pdev->dev, "Error allocating port structure\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, s);
+
+	mutex_init(&s->sccnxp_mutex);
+
+	/* Individual chip settings */
+	switch (chiptype) {
+	case SCCNXP_TYPE_SC2681:
+		s->name		= "SC2681";
+		s->uart.nr	= 2;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO;
+		fifosize	= 3;
+		freq_min	= 1000000;
+		freq_max	= 4000000;
+		break;
+	case SCCNXP_TYPE_SC2691:
+		s->name		= "SC2691";
+		s->uart.nr	= 1;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x07;
+		s->flags	= 0;
+		fifosize	= 3;
+		freq_min	= 1000000;
+		freq_max	= 4000000;
+		break;
+	case SCCNXP_TYPE_SC2692:
+		s->name		= "SC2692";
+		s->uart.nr	= 2;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO;
+		fifosize	= 3;
+		freq_min	= 1000000;
+		freq_max	= 4000000;
+		break;
+	case SCCNXP_TYPE_SC2891:
+		s->name		= "SC2891";
+		s->uart.nr	= 1;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO | SCCNXP_HAVE_MR0;
+		fifosize	= 16;
+		freq_min	= 100000;
+		freq_max	= 8000000;
+		break;
+	case SCCNXP_TYPE_SC2892:
+		s->name		= "SC2892";
+		s->uart.nr	= 2;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO | SCCNXP_HAVE_MR0;
+		fifosize	= 16;
+		freq_min	= 100000;
+		freq_max	= 8000000;
+		break;
+	case SCCNXP_TYPE_SC28202:
+		s->name		= "SC28202";
+		s->uart.nr	= 2;
+		s->freq_std	= 14745600;
+		s->addr_mask	= 0x7f;
+		s->flags	= SCCNXP_HAVE_IO | SCCNXP_HAVE_MR0;
+		fifosize	= 256;
+		freq_min	= 1000000;
+		freq_max	= 50000000;
+		break;
+	case SCCNXP_TYPE_SC68681:
+		s->name		= "SC68681";
+		s->uart.nr	= 2;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO;
+		fifosize	= 3;
+		freq_min	= 1000000;
+		freq_max	= 4000000;
+		break;
+	case SCCNXP_TYPE_SC68692:
+		s->name		= "SC68692";
+		s->uart.nr	= 2;
+		s->freq_std	= 3686400;
+		s->addr_mask	= 0x0f;
+		s->flags	= SCCNXP_HAVE_IO;
+		fifosize	= 3;
+		freq_min	= 1000000;
+		freq_max	= 4000000;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported chip type %i\n", chiptype);
+		ret = -ENOTSUPP;
+		goto err_out;
+	}
+
+	if (!pdata) {
+		dev_warn(&pdev->dev,
+			 "No platform data supplied, using defaults\n");
+		s->pdata.frequency = s->freq_std;
+	} else
+		memcpy(&s->pdata, pdata, sizeof(struct sccnxp_pdata));
+
+	s->irq = platform_get_irq(pdev, 0);
+	if (s->irq <= 0) {
+		dev_err(&pdev->dev, "Missing irq resource data\n");
+		ret = -ENXIO;
+		goto err_out;
+	}
+
+	/* Check input frequency */
+	if ((s->pdata.frequency < freq_min) ||
+	    (s->pdata.frequency > freq_max)) {
+		dev_err(&pdev->dev, "Frequency out of bounds\n");
+		ret = -EINVAL;
+		goto err_out;
+	}
+
+	membase = devm_request_and_ioremap(&pdev->dev, res);
+	if (!membase) {
+		dev_err(&pdev->dev, "Failed to ioremap\n");
+		ret = -EIO;
+		goto err_out;
+	}
+
+	s->uart.owner		= THIS_MODULE;
+	s->uart.dev_name	= "ttySC";
+	s->uart.major		= SCCNXP_MAJOR;
+	s->uart.minor		= SCCNXP_MINOR;
+#ifdef CONFIG_SERIAL_SCCNXP_CONSOLE
+	s->uart.cons		= &s->console;
+	s->uart.cons->device	= uart_console_device;
+	s->uart.cons->write	= sccnxp_console_write;
+	s->uart.cons->setup	= sccnxp_console_setup;
+	s->uart.cons->flags	= CON_PRINTBUFFER;
+	s->uart.cons->index	= -1;
+	s->uart.cons->data	= s;
+	strcpy(s->uart.cons->name, "ttySC");
+#endif
+	ret = uart_register_driver(&s->uart);
+	if (ret) {
+		dev_err(&pdev->dev, "Registering UART driver failed\n");
+		goto err_out;
+	}
+
+	for (i = 0; i < s->uart.nr; i++) {
+		s->port[i].line		= i;
+		s->port[i].dev		= &pdev->dev;
+		s->port[i].irq		= s->irq;
+		s->port[i].type		= PORT_SC26XX;
+		s->port[i].fifosize	= fifosize;
+		s->port[i].flags	= UPF_SKIP_TEST | UPF_FIXED_TYPE;
+		s->port[i].iotype	= UPIO_MEM;
+		s->port[i].mapbase	= res->start;
+		s->port[i].membase	= membase;
+		s->port[i].regshift	= s->pdata.reg_shift;
+		s->port[i].uartclk	= s->pdata.frequency;
+		s->port[i].ops		= &sccnxp_ops;
+		uart_add_one_port(&s->uart, &s->port[i]);
+		/* Set direction to input */
+		if (s->flags & SCCNXP_HAVE_IO)
+			sccnxp_set_bit(&s->port[i], DIR_OP, 0);
+	}
+
+	/* Disable interrupts */
+	s->imr = 0;
+	sccnxp_write(&s->port[0], SCCNXP_IMR_REG, 0);
+
+	/* Board specific configure */
+	if (s->pdata.init)
+		s->pdata.init();
+
+	ret = devm_request_threaded_irq(&pdev->dev, s->irq, NULL, sccnxp_ist,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					dev_name(&pdev->dev), s);
+	if (!ret)
+		return 0;
+
+	dev_err(&pdev->dev, "Unable to reguest IRQ %i\n", s->irq);
+
+err_out:
+	platform_set_drvdata(pdev, NULL);
+
+	return ret;
+}
+
+static int __devexit sccnxp_remove(struct platform_device *pdev)
+{
+	int i;
+	struct sccnxp_port *s = platform_get_drvdata(pdev);
+
+	devm_free_irq(&pdev->dev, s->irq, s);
+
+	for (i = 0; i < s->uart.nr; i++)
+		uart_remove_one_port(&s->uart, &s->port[i]);
+
+	uart_unregister_driver(&s->uart);
+	platform_set_drvdata(pdev, NULL);
+
+	if (s->pdata.exit)
+		s->pdata.exit();
+
+	return 0;
+}
+
+static const struct platform_device_id sccnxp_id_table[] = {
+	{ "sc2681",	SCCNXP_TYPE_SC2681 },
+	{ "sc2691",	SCCNXP_TYPE_SC2691 },
+	{ "sc2692",	SCCNXP_TYPE_SC2692 },
+	{ "sc2891",	SCCNXP_TYPE_SC2891 },
+	{ "sc2892",	SCCNXP_TYPE_SC2892 },
+	{ "sc28202",	SCCNXP_TYPE_SC28202 },
+	{ "sc68681",	SCCNXP_TYPE_SC68681 },
+	{ "sc68692",	SCCNXP_TYPE_SC68692 },
+};
+MODULE_DEVICE_TABLE(platform, sccnxp_id_table);
+
+static struct platform_driver sccnxp_uart_driver = {
+	.driver = {
+		.name	= SCCNXP_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= sccnxp_probe,
+	.remove		= __devexit_p(sccnxp_remove),
+	.id_table	= sccnxp_id_table,
+};
+module_platform_driver(sccnxp_uart_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
+MODULE_DESCRIPTION("SCCNXP serial driver");
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index a21dc8e3b7c0..0fcfd98a9566 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -159,7 +159,7 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 	retval = uport->ops->startup(uport);
 	if (retval == 0) {
 		if (uart_console(uport) && uport->cons->cflag) {
-			tty->termios->c_cflag = uport->cons->cflag;
+			tty->termios.c_cflag = uport->cons->cflag;
 			uport->cons->cflag = 0;
 		}
 		/*
@@ -172,11 +172,11 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 			 * Setup the RTS and DTR signals once the
 			 * port is open and ready to respond.
 			 */
-			if (tty->termios->c_cflag & CBAUD)
+			if (tty->termios.c_cflag & CBAUD)
 				uart_set_mctrl(uport, TIOCM_RTS | TIOCM_DTR);
 		}
 
-		if (port->flags & ASYNC_CTS_FLOW) {
+		if (tty_port_cts_enabled(port)) {
 			spin_lock_irq(&uport->lock);
 			if (!(uport->ops->get_mctrl(uport) & TIOCM_CTS))
 				tty->hw_stopped = 1;
@@ -240,7 +240,7 @@ static void uart_shutdown(struct tty_struct *tty, struct uart_state *state)
 		/*
 		 * Turn off DTR and RTS early.
 		 */
-		if (!tty || (tty->termios->c_cflag & HUPCL))
+		if (!tty || (tty->termios.c_cflag & HUPCL))
 			uart_clear_mctrl(uport, TIOCM_DTR | TIOCM_RTS);
 
 		uart_port_shutdown(port);
@@ -440,10 +440,10 @@ static void uart_change_speed(struct tty_struct *tty, struct uart_state *state,
 	 * If we have no tty, termios, or the port does not exist,
 	 * then we can't set the parameters for this port.
 	 */
-	if (!tty || !tty->termios || uport->type == PORT_UNKNOWN)
+	if (!tty || uport->type == PORT_UNKNOWN)
 		return;
 
-	termios = tty->termios;
+	termios = &tty->termios;
 
 	/*
 	 * Set flags based on termios cflag
@@ -614,7 +614,7 @@ static void uart_throttle(struct tty_struct *tty)
 	if (I_IXOFF(tty))
 		uart_send_xchar(tty, STOP_CHAR(tty));
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		uart_clear_mctrl(state->uart_port, TIOCM_RTS);
 }
 
@@ -630,42 +630,48 @@ static void uart_unthrottle(struct tty_struct *tty)
 			uart_send_xchar(tty, START_CHAR(tty));
 	}
 
-	if (tty->termios->c_cflag & CRTSCTS)
+	if (tty->termios.c_cflag & CRTSCTS)
 		uart_set_mctrl(port, TIOCM_RTS);
 }
 
-static int uart_get_info(struct uart_state *state,
-			 struct serial_struct __user *retinfo)
+static void uart_get_info(struct tty_port *port,
+                        struct uart_state *state,
+			struct serial_struct *retinfo)
 {
 	struct uart_port *uport = state->uart_port;
-	struct tty_port *port = &state->port;
-	struct serial_struct tmp;
 
-	memset(&tmp, 0, sizeof(tmp));
+	memset(retinfo, 0, sizeof(*retinfo));
 
-	/* Ensure the state we copy is consistent and no hardware changes
-	   occur as we go */
-	mutex_lock(&port->mutex);
-
-	tmp.type	    = uport->type;
-	tmp.line	    = uport->line;
-	tmp.port	    = uport->iobase;
+	retinfo->type	    = uport->type;
+	retinfo->line	    = uport->line;
+	retinfo->port	    = uport->iobase;
 	if (HIGH_BITS_OFFSET)
-		tmp.port_high = (long) uport->iobase >> HIGH_BITS_OFFSET;
-	tmp.irq		    = uport->irq;
-	tmp.flags	    = uport->flags;
-	tmp.xmit_fifo_size  = uport->fifosize;
-	tmp.baud_base	    = uport->uartclk / 16;
-	tmp.close_delay	    = jiffies_to_msecs(port->close_delay) / 10;
-	tmp.closing_wait    = port->closing_wait == ASYNC_CLOSING_WAIT_NONE ?
+		retinfo->port_high = (long) uport->iobase >> HIGH_BITS_OFFSET;
+	retinfo->irq		    = uport->irq;
+	retinfo->flags	    = uport->flags;
+	retinfo->xmit_fifo_size  = uport->fifosize;
+	retinfo->baud_base	    = uport->uartclk / 16;
+	retinfo->close_delay	    = jiffies_to_msecs(port->close_delay) / 10;
+	retinfo->closing_wait    = port->closing_wait == ASYNC_CLOSING_WAIT_NONE ?
 				ASYNC_CLOSING_WAIT_NONE :
 				jiffies_to_msecs(port->closing_wait) / 10;
-	tmp.custom_divisor  = uport->custom_divisor;
-	tmp.hub6	    = uport->hub6;
-	tmp.io_type         = uport->iotype;
-	tmp.iomem_reg_shift = uport->regshift;
-	tmp.iomem_base      = (void *)(unsigned long)uport->mapbase;
+	retinfo->custom_divisor  = uport->custom_divisor;
+	retinfo->hub6	    = uport->hub6;
+	retinfo->io_type         = uport->iotype;
+	retinfo->iomem_reg_shift = uport->regshift;
+	retinfo->iomem_base      = (void *)(unsigned long)uport->mapbase;
+}
+
+static int uart_get_info_user(struct uart_state *state,
+			 struct serial_struct __user *retinfo)
+{
+	struct tty_port *port = &state->port;
+	struct serial_struct tmp;
 
+	/* Ensure the state we copy is consistent and no hardware changes
+	   occur as we go */
+	mutex_lock(&port->mutex);
+	uart_get_info(port, state, &tmp);
 	mutex_unlock(&port->mutex);
 
 	if (copy_to_user(retinfo, &tmp, sizeof(*retinfo)))
@@ -673,42 +679,30 @@ static int uart_get_info(struct uart_state *state,
 	return 0;
 }
 
-static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
-			 struct serial_struct __user *newinfo)
+static int uart_set_info(struct tty_struct *tty, struct tty_port *port,
+			 struct uart_state *state,
+			 struct serial_struct *new_info)
 {
-	struct serial_struct new_serial;
 	struct uart_port *uport = state->uart_port;
-	struct tty_port *port = &state->port;
 	unsigned long new_port;
 	unsigned int change_irq, change_port, closing_wait;
 	unsigned int old_custom_divisor, close_delay;
 	upf_t old_flags, new_flags;
 	int retval = 0;
 
-	if (copy_from_user(&new_serial, newinfo, sizeof(new_serial)))
-		return -EFAULT;
-
-	new_port = new_serial.port;
+	new_port = new_info->port;
 	if (HIGH_BITS_OFFSET)
-		new_port += (unsigned long) new_serial.port_high << HIGH_BITS_OFFSET;
+		new_port += (unsigned long) new_info->port_high << HIGH_BITS_OFFSET;
 
-	new_serial.irq = irq_canonicalize(new_serial.irq);
-	close_delay = msecs_to_jiffies(new_serial.close_delay * 10);
-	closing_wait = new_serial.closing_wait == ASYNC_CLOSING_WAIT_NONE ?
+	new_info->irq = irq_canonicalize(new_info->irq);
+	close_delay = msecs_to_jiffies(new_info->close_delay * 10);
+	closing_wait = new_info->closing_wait == ASYNC_CLOSING_WAIT_NONE ?
 			ASYNC_CLOSING_WAIT_NONE :
-			msecs_to_jiffies(new_serial.closing_wait * 10);
+			msecs_to_jiffies(new_info->closing_wait * 10);
 
-	/*
-	 * This semaphore protects port->count.  It is also
-	 * very useful to prevent opens.  Also, take the
-	 * port configuration semaphore to make sure that a
-	 * module insertion/removal doesn't change anything
-	 * under us.
-	 */
-	mutex_lock(&port->mutex);
 
 	change_irq  = !(uport->flags & UPF_FIXED_PORT)
-		&& new_serial.irq != uport->irq;
+		&& new_info->irq != uport->irq;
 
 	/*
 	 * Since changing the 'type' of the port changes its resource
@@ -717,29 +711,29 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 	 */
 	change_port = !(uport->flags & UPF_FIXED_PORT)
 		&& (new_port != uport->iobase ||
-		    (unsigned long)new_serial.iomem_base != uport->mapbase ||
-		    new_serial.hub6 != uport->hub6 ||
-		    new_serial.io_type != uport->iotype ||
-		    new_serial.iomem_reg_shift != uport->regshift ||
-		    new_serial.type != uport->type);
+		    (unsigned long)new_info->iomem_base != uport->mapbase ||
+		    new_info->hub6 != uport->hub6 ||
+		    new_info->io_type != uport->iotype ||
+		    new_info->iomem_reg_shift != uport->regshift ||
+		    new_info->type != uport->type);
 
 	old_flags = uport->flags;
-	new_flags = new_serial.flags;
+	new_flags = new_info->flags;
 	old_custom_divisor = uport->custom_divisor;
 
 	if (!capable(CAP_SYS_ADMIN)) {
 		retval = -EPERM;
 		if (change_irq || change_port ||
-		    (new_serial.baud_base != uport->uartclk / 16) ||
+		    (new_info->baud_base != uport->uartclk / 16) ||
 		    (close_delay != port->close_delay) ||
 		    (closing_wait != port->closing_wait) ||
-		    (new_serial.xmit_fifo_size &&
-		     new_serial.xmit_fifo_size != uport->fifosize) ||
+		    (new_info->xmit_fifo_size &&
+		     new_info->xmit_fifo_size != uport->fifosize) ||
 		    (((new_flags ^ old_flags) & ~UPF_USR_MASK) != 0))
 			goto exit;
 		uport->flags = ((uport->flags & ~UPF_USR_MASK) |
 			       (new_flags & UPF_USR_MASK));
-		uport->custom_divisor = new_serial.custom_divisor;
+		uport->custom_divisor = new_info->custom_divisor;
 		goto check_and_exit;
 	}
 
@@ -747,10 +741,10 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 	 * Ask the low level driver to verify the settings.
 	 */
 	if (uport->ops->verify_port)
-		retval = uport->ops->verify_port(uport, &new_serial);
+		retval = uport->ops->verify_port(uport, new_info);
 
-	if ((new_serial.irq >= nr_irqs) || (new_serial.irq < 0) ||
-	    (new_serial.baud_base < 9600))
+	if ((new_info->irq >= nr_irqs) || (new_info->irq < 0) ||
+	    (new_info->baud_base < 9600))
 		retval = -EINVAL;
 
 	if (retval)
@@ -790,11 +784,11 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 			uport->ops->release_port(uport);
 
 		uport->iobase = new_port;
-		uport->type = new_serial.type;
-		uport->hub6 = new_serial.hub6;
-		uport->iotype = new_serial.io_type;
-		uport->regshift = new_serial.iomem_reg_shift;
-		uport->mapbase = (unsigned long)new_serial.iomem_base;
+		uport->type = new_info->type;
+		uport->hub6 = new_info->hub6;
+		uport->iotype = new_info->io_type;
+		uport->regshift = new_info->iomem_reg_shift;
+		uport->mapbase = (unsigned long)new_info->iomem_base;
 
 		/*
 		 * Claim and map the new regions
@@ -835,16 +829,16 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 	}
 
 	if (change_irq)
-		uport->irq      = new_serial.irq;
+		uport->irq      = new_info->irq;
 	if (!(uport->flags & UPF_FIXED_PORT))
-		uport->uartclk  = new_serial.baud_base * 16;
+		uport->uartclk  = new_info->baud_base * 16;
 	uport->flags            = (uport->flags & ~UPF_CHANGE_MASK) |
 				 (new_flags & UPF_CHANGE_MASK);
-	uport->custom_divisor   = new_serial.custom_divisor;
+	uport->custom_divisor   = new_info->custom_divisor;
 	port->close_delay     = close_delay;
 	port->closing_wait    = closing_wait;
-	if (new_serial.xmit_fifo_size)
-		uport->fifosize = new_serial.xmit_fifo_size;
+	if (new_info->xmit_fifo_size)
+		uport->fifosize = new_info->xmit_fifo_size;
 	if (port->tty)
 		port->tty->low_latency =
 			(uport->flags & UPF_LOW_LATENCY) ? 1 : 0;
@@ -873,6 +867,28 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 	} else
 		retval = uart_startup(tty, state, 1);
  exit:
+	return retval;
+}
+
+static int uart_set_info_user(struct tty_struct *tty, struct uart_state *state,
+			 struct serial_struct __user *newinfo)
+{
+	struct serial_struct new_serial;
+	struct tty_port *port = &state->port;
+	int retval;
+
+	if (copy_from_user(&new_serial, newinfo, sizeof(new_serial)))
+		return -EFAULT;
+
+	/*
+	 * This semaphore protects port->count.  It is also
+	 * very useful to prevent opens.  Also, take the
+	 * port configuration semaphore to make sure that a
+	 * module insertion/removal doesn't change anything
+	 * under us.
+	 */
+	mutex_lock(&port->mutex);
+	retval = uart_set_info(tty, port, state, &new_serial);
 	mutex_unlock(&port->mutex);
 	return retval;
 }
@@ -1115,11 +1131,11 @@ uart_ioctl(struct tty_struct *tty, unsigned int cmd,
 	 */
 	switch (cmd) {
 	case TIOCGSERIAL:
-		ret = uart_get_info(state, uarg);
+		ret = uart_get_info_user(state, uarg);
 		break;
 
 	case TIOCSSERIAL:
-		ret = uart_set_info(tty, state, uarg);
+		ret = uart_set_info_user(tty, state, uarg);
 		break;
 
 	case TIOCSERCONFIG:
@@ -1187,7 +1203,7 @@ static void uart_set_ldisc(struct tty_struct *tty)
 	struct uart_port *uport = state->uart_port;
 
 	if (uport->ops->set_ldisc)
-		uport->ops->set_ldisc(uport, tty->termios->c_line);
+		uport->ops->set_ldisc(uport, tty->termios.c_line);
 }
 
 static void uart_set_termios(struct tty_struct *tty,
@@ -1195,7 +1211,7 @@ static void uart_set_termios(struct tty_struct *tty,
 {
 	struct uart_state *state = tty->driver_data;
 	unsigned long flags;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 
 
 	/*
@@ -1206,9 +1222,9 @@ static void uart_set_termios(struct tty_struct *tty,
 	 */
 #define RELEVANT_IFLAG(iflag)	((iflag) & (IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK))
 	if ((cflag ^ old_termios->c_cflag) == 0 &&
-	    tty->termios->c_ospeed == old_termios->c_ospeed &&
-	    tty->termios->c_ispeed == old_termios->c_ispeed &&
-	    RELEVANT_IFLAG(tty->termios->c_iflag ^ old_termios->c_iflag) == 0) {
+	    tty->termios.c_ospeed == old_termios->c_ospeed &&
+	    tty->termios.c_ispeed == old_termios->c_ispeed &&
+	    RELEVANT_IFLAG(tty->termios.c_iflag ^ old_termios->c_iflag) == 0) {
 		return;
 	}
 
@@ -1960,8 +1976,8 @@ int uart_resume_port(struct uart_driver *drv, struct uart_port *uport)
 		/*
 		 * If that's unset, use the tty termios setting.
 		 */
-		if (port->tty && port->tty->termios && termios.c_cflag == 0)
-			termios = *(port->tty->termios);
+		if (port->tty && termios.c_cflag == 0)
+			termios = port->tty->termios;
 
 		if (console_suspend_enabled)
 			uart_change_pm(state, 0);
@@ -2113,6 +2129,7 @@ static int uart_poll_init(struct tty_driver *driver, int line, char *options)
 	int bits = 8;
 	int parity = 'n';
 	int flow = 'n';
+	int ret;
 
 	if (!state || !state->uart_port)
 		return -1;
@@ -2121,6 +2138,22 @@ static int uart_poll_init(struct tty_driver *driver, int line, char *options)
 	if (!(port->ops->poll_get_char && port->ops->poll_put_char))
 		return -1;
 
+	if (port->ops->poll_init) {
+		struct tty_port *tport = &state->port;
+
+		ret = 0;
+		mutex_lock(&tport->mutex);
+		/*
+		 * We don't set ASYNCB_INITIALIZED as we only initialized the
+		 * hw, e.g. state->xmit is still uninitialized.
+		 */
+		if (!test_bit(ASYNCB_INITIALIZED, &tport->flags))
+			ret = port->ops->poll_init(port);
+		mutex_unlock(&tport->mutex);
+		if (ret)
+			return ret;
+	}
+
 	if (options) {
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
 		return uart_set_options(port, NULL, baud, parity, bits, flow);
@@ -2293,6 +2326,36 @@ struct tty_driver *uart_console_device(struct console *co, int *index)
 	return p->tty_driver;
 }
 
+static ssize_t uart_get_attr_uartclk(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	int ret;
+	struct tty_port *port = dev_get_drvdata(dev);
+	struct uart_state *state = container_of(port, struct uart_state, port);
+
+	mutex_lock(&state->port.mutex);
+	ret = snprintf(buf, PAGE_SIZE, "%d\n", state->uart_port->uartclk);
+	mutex_unlock(&state->port.mutex);
+
+	return ret;
+}
+
+static DEVICE_ATTR(uartclk, S_IRUSR | S_IRGRP, uart_get_attr_uartclk, NULL);
+
+static struct attribute *tty_dev_attrs[] = {
+	&dev_attr_uartclk.attr,
+	NULL,
+	};
+
+static const struct attribute_group tty_dev_attr_group = {
+	.attrs = tty_dev_attrs,
+	};
+
+static const struct attribute_group *tty_dev_attr_groups[] = {
+	&tty_dev_attr_group,
+	NULL
+	};
+
 /**
  *	uart_add_one_port - attach a driver-defined port structure
  *	@drv: pointer to the uart low level driver structure for this port
@@ -2346,7 +2409,8 @@ int uart_add_one_port(struct uart_driver *drv, struct uart_port *uport)
 	 * Register the port whether it's detected or not.  This allows
 	 * setserial to be used to alter this ports parameters.
 	 */
-	tty_dev = tty_register_device(drv->tty_driver, uport->line, uport->dev);
+	tty_dev = tty_port_register_device_attr(port, drv->tty_driver,
+			uport->line, uport->dev, port, tty_dev_attr_groups);
 	if (likely(!IS_ERR(tty_dev))) {
 		device_set_wakeup_capable(tty_dev, 1);
 	} else {
@@ -2454,9 +2518,12 @@ void uart_handle_dcd_change(struct uart_port *uport, unsigned int status)
 {
 	struct uart_state *state = uport->state;
 	struct tty_port *port = &state->port;
-	struct tty_ldisc *ld = tty_ldisc_ref(port->tty);
+	struct tty_ldisc *ld = NULL;
 	struct pps_event_time ts;
+	struct tty_struct *tty = port->tty;
 
+	if (tty)
+	        ld = tty_ldisc_ref(tty);
 	if (ld && ld->ops->dcd_change)
 		pps_get_ts(&ts);
 
@@ -2469,12 +2536,12 @@ void uart_handle_dcd_change(struct uart_port *uport, unsigned int status)
 	if (port->flags & ASYNC_CHECK_CD) {
 		if (status)
 			wake_up_interruptible(&port->open_wait);
-		else if (port->tty)
-			tty_hangup(port->tty);
+		else if (tty)
+			tty_hangup(tty);
 	}
 
 	if (ld && ld->ops->dcd_change)
-		ld->ops->dcd_change(port->tty, status, &ts);
+		ld->ops->dcd_change(tty, status, &ts);
 	if (ld)
 		tty_ldisc_deref(ld);
 }
@@ -2492,7 +2559,7 @@ void uart_handle_cts_change(struct uart_port *uport, unsigned int status)
 
 	uport->icount.cts++;
 
-	if (port->flags & ASYNC_CTS_FLOW) {
+	if (tty_port_cts_enabled(port)) {
 		if (tty->hw_stopped) {
 			if (status) {
 				tty->hw_stopped = 0;
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 5b3eda2024fe..a9e2bd1ab534 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -668,7 +668,7 @@ int sirfsoc_uart_probe(struct platform_device *pdev)
 	if (res == NULL) {
 		dev_err(&pdev->dev, "Insufficient resources.\n");
 		ret = -EFAULT;
-		goto irq_err;
+		goto err;
 	}
 	port->irq = res->start;
 
@@ -676,7 +676,7 @@ int sirfsoc_uart_probe(struct platform_device *pdev)
 		sirfport->p = pinctrl_get_select_default(&pdev->dev);
 		ret = IS_ERR(sirfport->p);
 		if (ret)
-			goto pin_err;
+			goto err;
 	}
 
 	port->ops = &sirfsoc_uart_ops;
@@ -695,9 +695,6 @@ port_err:
 	platform_set_drvdata(pdev, NULL);
 	if (sirfport->hw_flow_ctrl)
 		pinctrl_put(sirfport->p);
-pin_err:
-irq_err:
-	devm_iounmap(&pdev->dev, port->membase);
 err:
 	return ret;
 }
@@ -709,7 +706,6 @@ static int sirfsoc_uart_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 	if (sirfport->hw_flow_ctrl)
 		pinctrl_put(sirfport->p);
-	devm_iounmap(&pdev->dev, port->membase);
 	uart_remove_one_port(&sirfsoc_uart_drv, port);
 	return 0;
 }
diff --git a/drivers/tty/serial/sunsu.c b/drivers/tty/serial/sunsu.c
index 675303b8ed84..b97913dcdbff 100644
--- a/drivers/tty/serial/sunsu.c
+++ b/drivers/tty/serial/sunsu.c
@@ -58,10 +58,16 @@
 enum su_type { SU_PORT_NONE, SU_PORT_MS, SU_PORT_KBD, SU_PORT_PORT };
 static char *su_typev[] = { "su(???)", "su(mouse)", "su(kbd)", "su(serial)" };
 
+struct serial_uart_config {
+	char	*name;
+	int	dfl_xmit_fifo_size;
+	int	flags;
+};
+
 /*
  * Here we define the default xmit fifo size used for each type of UART.
  */
-static const struct serial_uart_config uart_config[PORT_MAX_8250+1] = {
+static const struct serial_uart_config uart_config[] = {
 	{ "unknown",	1,	0 },
 	{ "8250",	1,	0 },
 	{ "16450",	1,	0 },
diff --git a/drivers/tty/synclink.c b/drivers/tty/synclink.c
index 593d40ad0a6b..70e3a525bc82 100644
--- a/drivers/tty/synclink.c
+++ b/drivers/tty/synclink.c
@@ -1359,7 +1359,7 @@ static void mgsl_isr_io_pin( struct mgsl_struct *info )
 			}
 		}
 	
-		if ( (info->port.flags & ASYNC_CTS_FLOW) && 
+		if (tty_port_cts_enabled(&info->port) &&
 		     (status & MISCSTATUS_CTS_LATCHED) ) {
 			if (info->port.tty->hw_stopped) {
 				if (status & MISCSTATUS_CTS) {
@@ -1840,22 +1840,22 @@ static void shutdown(struct mgsl_struct * info)
 	usc_DisableInterrupts(info,RECEIVE_DATA + RECEIVE_STATUS +
 		TRANSMIT_DATA + TRANSMIT_STATUS + IO_PIN + MISC );
 	usc_DisableDmaInterrupts(info,DICR_MASTER + DICR_TRANSMIT + DICR_RECEIVE);
-	
+
 	/* Disable DMAEN (Port 7, Bit 14) */
 	/* This disconnects the DMA request signal from the ISA bus */
 	/* on the ISA adapter. This has no effect for the PCI adapter */
 	usc_OutReg(info, PCR, (u16)((usc_InReg(info, PCR) | BIT15) | BIT14));
-	
+
 	/* Disable INTEN (Port 6, Bit12) */
 	/* This disconnects the IRQ request signal to the ISA bus */
 	/* on the ISA adapter. This has no effect for the PCI adapter */
 	usc_OutReg(info, PCR, (u16)((usc_InReg(info, PCR) | BIT13) | BIT12));
-	
- 	if (!info->port.tty || info->port.tty->termios->c_cflag & HUPCL) {
+
+	if (!info->port.tty || info->port.tty->termios.c_cflag & HUPCL) {
  		info->serial_signals &= ~(SerialSignal_DTR + SerialSignal_RTS);
 		usc_set_serial_signals(info);
 	}
-	
+
 	spin_unlock_irqrestore(&info->irq_spinlock,flags);
 
 	mgsl_release_resources(info);	
@@ -1895,7 +1895,7 @@ static void mgsl_program_hw(struct mgsl_struct *info)
 	usc_EnableInterrupts(info, IO_PIN);
 	usc_get_serial_signals(info);
 		
-	if (info->netcount || info->port.tty->termios->c_cflag & CREAD)
+	if (info->netcount || info->port.tty->termios.c_cflag & CREAD)
 		usc_start_receiver(info);
 		
 	spin_unlock_irqrestore(&info->irq_spinlock,flags);
@@ -1908,14 +1908,14 @@ static void mgsl_change_params(struct mgsl_struct *info)
 	unsigned cflag;
 	int bits_per_char;
 
-	if (!info->port.tty || !info->port.tty->termios)
+	if (!info->port.tty)
 		return;
 		
 	if (debug_level >= DEBUG_LEVEL_INFO)
 		printk("%s(%d):mgsl_change_params(%s)\n",
 			 __FILE__,__LINE__, info->device_name );
 			 
-	cflag = info->port.tty->termios->c_cflag;
+	cflag = info->port.tty->termios.c_cflag;
 
 	/* if B0 rate (hangup) specified then negate DTR and RTS */
 	/* otherwise assert DTR and RTS */
@@ -2367,8 +2367,8 @@ static void mgsl_throttle(struct tty_struct * tty)
 	
 	if (I_IXOFF(tty))
 		mgsl_send_xchar(tty, STOP_CHAR(tty));
- 
- 	if (tty->termios->c_cflag & CRTSCTS) {
+
+	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->irq_spinlock,flags);
 		info->serial_signals &= ~SerialSignal_RTS;
 	 	usc_set_serial_signals(info);
@@ -2401,8 +2401,8 @@ static void mgsl_unthrottle(struct tty_struct * tty)
 		else
 			mgsl_send_xchar(tty, START_CHAR(tty));
 	}
-	
- 	if (tty->termios->c_cflag & CRTSCTS) {
+
+	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->irq_spinlock,flags);
 		info->serial_signals |= SerialSignal_RTS;
 	 	usc_set_serial_signals(info);
@@ -3045,7 +3045,7 @@ static void mgsl_set_termios(struct tty_struct *tty, struct ktermios *old_termio
 
 	/* Handle transition to B0 status */
 	if (old_termios->c_cflag & CBAUD &&
-	    !(tty->termios->c_cflag & CBAUD)) {
+	    !(tty->termios.c_cflag & CBAUD)) {
 		info->serial_signals &= ~(SerialSignal_RTS + SerialSignal_DTR);
 		spin_lock_irqsave(&info->irq_spinlock,flags);
 	 	usc_set_serial_signals(info);
@@ -3054,9 +3054,9 @@ static void mgsl_set_termios(struct tty_struct *tty, struct ktermios *old_termio
 	
 	/* Handle transition away from B0 status */
 	if (!(old_termios->c_cflag & CBAUD) &&
-	    tty->termios->c_cflag & CBAUD) {
+	    tty->termios.c_cflag & CBAUD) {
 		info->serial_signals |= SerialSignal_DTR;
- 		if (!(tty->termios->c_cflag & CRTSCTS) || 
+ 		if (!(tty->termios.c_cflag & CRTSCTS) || 
  		    !test_bit(TTY_THROTTLED, &tty->flags)) {
 			info->serial_signals |= SerialSignal_RTS;
  		}
@@ -3067,7 +3067,7 @@ static void mgsl_set_termios(struct tty_struct *tty, struct ktermios *old_termio
 	
 	/* Handle turning off CRTSCTS */
 	if (old_termios->c_cflag & CRTSCTS &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		mgsl_start(tty);
 	}
@@ -3287,7 +3287,7 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 		return 0;
 	}
 
-	if (tty->termios->c_cflag & CLOCAL)
+	if (tty->termios.c_cflag & CLOCAL)
 		do_clocal = true;
 
 	/* Wait for carrier detect and the line to become
@@ -3313,7 +3313,7 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 	port->blocked_open++;
 	
 	while (1) {
-		if (tty->termios->c_cflag & CBAUD)
+		if (tty->termios.c_cflag & CBAUD)
 			tty_port_raise_dtr_rts(port);
 		
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -3338,9 +3338,9 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 			printk("%s(%d):block_til_ready blocking on %s count=%d\n",
 				 __FILE__,__LINE__, tty->driver->name, port->count );
 				 
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	
 	set_current_state(TASK_RUNNING);
@@ -3362,6 +3362,29 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 	
 }	/* end of block_til_ready() */
 
+static int mgsl_install(struct tty_driver *driver, struct tty_struct *tty)
+{
+	struct mgsl_struct *info;
+	int line = tty->index;
+
+	/* verify range of specified line number */
+	if (line >= mgsl_device_count) {
+		printk("%s(%d):mgsl_open with invalid line #%d.\n",
+			__FILE__, __LINE__, line);
+		return -ENODEV;
+	}
+
+	/* find the info structure for the specified line */
+	info = mgsl_device_list;
+	while (info && info->line != line)
+		info = info->next_device;
+	if (mgsl_paranoia_check(info, tty->name, "mgsl_open"))
+		return -ENODEV;
+	tty->driver_data = info;
+
+	return tty_port_install(&info->port, driver, tty);
+}
+
 /* mgsl_open()
  *
  *	Called when a port is opened.  Init and enable port.
@@ -3374,26 +3397,10 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
  */
 static int mgsl_open(struct tty_struct *tty, struct file * filp)
 {
-	struct mgsl_struct	*info;
-	int 			retval, line;
+	struct mgsl_struct *info = tty->driver_data;
 	unsigned long flags;
+	int retval;
 
-	/* verify range of specified line number */	
-	line = tty->index;
-	if (line >= mgsl_device_count) {
-		printk("%s(%d):mgsl_open with invalid line #%d.\n",
-			__FILE__,__LINE__,line);
-		return -ENODEV;
-	}
-
-	/* find the info structure for the specified line */
-	info = mgsl_device_list;
-	while(info && info->line != line)
-		info = info->next_device;
-	if (mgsl_paranoia_check(info, tty->name, "mgsl_open"))
-		return -ENODEV;
-	
-	tty->driver_data = info;
 	info->port.tty = tty;
 		
 	if (debug_level >= DEBUG_LEVEL_INFO)
@@ -4297,6 +4304,7 @@ static struct mgsl_struct* mgsl_allocate_device(void)
 }	/* end of mgsl_allocate_device()*/
 
 static const struct tty_operations mgsl_ops = {
+	.install = mgsl_install,
 	.open = mgsl_open,
 	.close = mgsl_close,
 	.write = mgsl_write,
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index aa1debf97cc7..b38e954eedd3 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -785,7 +785,7 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle transition to B0 status */
 	if (old_termios->c_cflag & CBAUD &&
-	    !(tty->termios->c_cflag & CBAUD)) {
+	    !(tty->termios.c_cflag & CBAUD)) {
 		info->signals &= ~(SerialSignal_RTS + SerialSignal_DTR);
 		spin_lock_irqsave(&info->lock,flags);
 		set_signals(info);
@@ -794,9 +794,9 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle transition away from B0 status */
 	if (!(old_termios->c_cflag & CBAUD) &&
-	    tty->termios->c_cflag & CBAUD) {
+	    tty->termios.c_cflag & CBAUD) {
 		info->signals |= SerialSignal_DTR;
- 		if (!(tty->termios->c_cflag & CRTSCTS) ||
+ 		if (!(tty->termios.c_cflag & CRTSCTS) ||
  		    !test_bit(TTY_THROTTLED, &tty->flags)) {
 			info->signals |= SerialSignal_RTS;
  		}
@@ -807,7 +807,7 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle turning off CRTSCTS */
 	if (old_termios->c_cflag & CRTSCTS &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		tx_release(tty);
 	}
@@ -1372,7 +1372,7 @@ static void throttle(struct tty_struct * tty)
 	DBGINFO(("%s throttle\n", info->device_name));
 	if (I_IXOFF(tty))
 		send_xchar(tty, STOP_CHAR(tty));
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->signals &= ~SerialSignal_RTS;
 	 	set_signals(info);
@@ -1397,7 +1397,7 @@ static void unthrottle(struct tty_struct * tty)
 		else
 			send_xchar(tty, START_CHAR(tty));
 	}
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->signals |= SerialSignal_RTS;
 	 	set_signals(info);
@@ -2053,7 +2053,7 @@ static void cts_change(struct slgt_info *info, unsigned short status)
 	wake_up_interruptible(&info->event_wait_q);
 	info->pending_bh |= BH_STATUS;
 
-	if (info->port.flags & ASYNC_CTS_FLOW) {
+	if (tty_port_cts_enabled(&info->port)) {
 		if (info->port.tty) {
 			if (info->port.tty->hw_stopped) {
 				if (info->signals & SerialSignal_CTS) {
@@ -2493,7 +2493,7 @@ static void shutdown(struct slgt_info *info)
 
 	slgt_irq_off(info, IRQ_ALL | IRQ_MASTER);
 
- 	if (!info->port.tty || info->port.tty->termios->c_cflag & HUPCL) {
+ 	if (!info->port.tty || info->port.tty->termios.c_cflag & HUPCL) {
  		info->signals &= ~(SerialSignal_DTR + SerialSignal_RTS);
 		set_signals(info);
 	}
@@ -2534,7 +2534,7 @@ static void program_hw(struct slgt_info *info)
 	get_signals(info);
 
 	if (info->netcount ||
-	    (info->port.tty && info->port.tty->termios->c_cflag & CREAD))
+	    (info->port.tty && info->port.tty->termios.c_cflag & CREAD))
 		rx_start(info);
 
 	spin_unlock_irqrestore(&info->lock,flags);
@@ -2548,11 +2548,11 @@ static void change_params(struct slgt_info *info)
 	unsigned cflag;
 	int bits_per_char;
 
-	if (!info->port.tty || !info->port.tty->termios)
+	if (!info->port.tty)
 		return;
 	DBGINFO(("%s change_params\n", info->device_name));
 
-	cflag = info->port.tty->termios->c_cflag;
+	cflag = info->port.tty->termios.c_cflag;
 
 	/* if B0 rate (hangup) specified then negate DTR and RTS */
 	/* otherwise assert DTR and RTS */
@@ -3292,7 +3292,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 		return 0;
 	}
 
-	if (tty->termios->c_cflag & CLOCAL)
+	if (tty->termios.c_cflag & CLOCAL)
 		do_clocal = true;
 
 	/* Wait for carrier detect and the line to become
@@ -3314,7 +3314,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 	port->blocked_open++;
 
 	while (1) {
-		if ((tty->termios->c_cflag & CBAUD))
+		if ((tty->termios.c_cflag & CBAUD))
 			tty_port_raise_dtr_rts(port);
 
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -3336,9 +3336,9 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 		}
 
 		DBGINFO(("%s block_til_ready wait\n", tty->driver->name));
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 
 	set_current_state(TASK_RUNNING);
@@ -3689,8 +3689,11 @@ static void device_init(int adapter_num, struct pci_dev *pdev)
 		}
 	}
 
-	for (i=0; i < port_count; ++i)
-		tty_register_device(serial_driver, port_array[i]->line, &(port_array[i]->pdev->dev));
+	for (i = 0; i < port_count; ++i) {
+		struct slgt_info *info = port_array[i];
+		tty_port_register_device(&info->port, serial_driver, info->line,
+				&info->pdev->dev);
+	}
 }
 
 static int __devinit init_one(struct pci_dev *dev,
diff --git a/drivers/tty/synclinkmp.c b/drivers/tty/synclinkmp.c
index a3dddc12d2fe..f17d9f3d84a2 100644
--- a/drivers/tty/synclinkmp.c
+++ b/drivers/tty/synclinkmp.c
@@ -711,15 +711,11 @@ static void ldisc_receive_buf(struct tty_struct *tty,
 
 /* tty callbacks */
 
-/* Called when a port is opened.  Init and enable port.
- */
-static int open(struct tty_struct *tty, struct file *filp)
+static int install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	SLMP_INFO *info;
-	int retval, line;
-	unsigned long flags;
+	int line = tty->index;
 
-	line = tty->index;
 	if (line >= synclinkmp_device_count) {
 		printk("%s(%d): open with invalid line #%d.\n",
 			__FILE__,__LINE__,line);
@@ -727,17 +723,30 @@ static int open(struct tty_struct *tty, struct file *filp)
 	}
 
 	info = synclinkmp_device_list;
-	while(info && info->line != line)
+	while (info && info->line != line)
 		info = info->next_device;
 	if (sanity_check(info, tty->name, "open"))
 		return -ENODEV;
-	if ( info->init_error ) {
+	if (info->init_error) {
 		printk("%s(%d):%s device is not allocated, init error=%d\n",
-			__FILE__,__LINE__,info->device_name,info->init_error);
+			__FILE__, __LINE__, info->device_name,
+			info->init_error);
 		return -ENODEV;
 	}
 
 	tty->driver_data = info;
+
+	return tty_port_install(&info->port, driver, tty);
+}
+
+/* Called when a port is opened.  Init and enable port.
+ */
+static int open(struct tty_struct *tty, struct file *filp)
+{
+	SLMP_INFO *info = tty->driver_data;
+	unsigned long flags;
+	int retval;
+
 	info->port.tty = tty;
 
 	if (debug_level >= DEBUG_LEVEL_INFO)
@@ -873,7 +882,7 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle transition to B0 status */
 	if (old_termios->c_cflag & CBAUD &&
-	    !(tty->termios->c_cflag & CBAUD)) {
+	    !(tty->termios.c_cflag & CBAUD)) {
 		info->serial_signals &= ~(SerialSignal_RTS + SerialSignal_DTR);
 		spin_lock_irqsave(&info->lock,flags);
 	 	set_signals(info);
@@ -882,9 +891,9 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle transition away from B0 status */
 	if (!(old_termios->c_cflag & CBAUD) &&
-	    tty->termios->c_cflag & CBAUD) {
+	    tty->termios.c_cflag & CBAUD) {
 		info->serial_signals |= SerialSignal_DTR;
- 		if (!(tty->termios->c_cflag & CRTSCTS) ||
+ 		if (!(tty->termios.c_cflag & CRTSCTS) ||
  		    !test_bit(TTY_THROTTLED, &tty->flags)) {
 			info->serial_signals |= SerialSignal_RTS;
  		}
@@ -895,7 +904,7 @@ static void set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 
 	/* Handle turning off CRTSCTS */
 	if (old_termios->c_cflag & CRTSCTS &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
+	    !(tty->termios.c_cflag & CRTSCTS)) {
 		tty->hw_stopped = 0;
 		tx_release(tty);
 	}
@@ -1473,7 +1482,7 @@ static void throttle(struct tty_struct * tty)
 	if (I_IXOFF(tty))
 		send_xchar(tty, STOP_CHAR(tty));
 
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->serial_signals &= ~SerialSignal_RTS;
 	 	set_signals(info);
@@ -1502,7 +1511,7 @@ static void unthrottle(struct tty_struct * tty)
 			send_xchar(tty, START_CHAR(tty));
 	}
 
- 	if (tty->termios->c_cflag & CRTSCTS) {
+ 	if (tty->termios.c_cflag & CRTSCTS) {
 		spin_lock_irqsave(&info->lock,flags);
 		info->serial_signals |= SerialSignal_RTS;
 	 	set_signals(info);
@@ -2491,7 +2500,7 @@ static void isr_io_pin( SLMP_INFO *info, u16 status )
 			}
 		}
 
-		if ( (info->port.flags & ASYNC_CTS_FLOW) &&
+		if (tty_port_cts_enabled(&info->port) &&
 		     (status & MISCSTATUS_CTS_LATCHED) ) {
 			if ( info->port.tty ) {
 				if (info->port.tty->hw_stopped) {
@@ -2708,7 +2717,7 @@ static void shutdown(SLMP_INFO * info)
 
 	reset_port(info);
 
- 	if (!info->port.tty || info->port.tty->termios->c_cflag & HUPCL) {
+ 	if (!info->port.tty || info->port.tty->termios.c_cflag & HUPCL) {
  		info->serial_signals &= ~(SerialSignal_DTR + SerialSignal_RTS);
 		set_signals(info);
 	}
@@ -2749,7 +2758,7 @@ static void program_hw(SLMP_INFO *info)
 
 	get_signals(info);
 
-	if (info->netcount || (info->port.tty && info->port.tty->termios->c_cflag & CREAD) )
+	if (info->netcount || (info->port.tty && info->port.tty->termios.c_cflag & CREAD) )
 		rx_start(info);
 
 	spin_unlock_irqrestore(&info->lock,flags);
@@ -2762,14 +2771,14 @@ static void change_params(SLMP_INFO *info)
 	unsigned cflag;
 	int bits_per_char;
 
-	if (!info->port.tty || !info->port.tty->termios)
+	if (!info->port.tty)
 		return;
 
 	if (debug_level >= DEBUG_LEVEL_INFO)
 		printk("%s(%d):%s change_params()\n",
 			 __FILE__,__LINE__, info->device_name );
 
-	cflag = info->port.tty->termios->c_cflag;
+	cflag = info->port.tty->termios.c_cflag;
 
 	/* if B0 rate (hangup) specified then negate DTR and RTS */
 	/* otherwise assert DTR and RTS */
@@ -3306,7 +3315,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 		return 0;
 	}
 
-	if (tty->termios->c_cflag & CLOCAL)
+	if (tty->termios.c_cflag & CLOCAL)
 		do_clocal = true;
 
 	/* Wait for carrier detect and the line to become
@@ -3332,7 +3341,7 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 	port->blocked_open++;
 
 	while (1) {
-		if (tty->termios->c_cflag & CBAUD)
+		if (tty->termios.c_cflag & CBAUD)
 			tty_port_raise_dtr_rts(port);
 
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -3357,9 +3366,9 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 			printk("%s(%d):%s block_til_ready() count=%d\n",
 				 __FILE__,__LINE__, tty->driver->name, port->count );
 
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 
 	set_current_state(TASK_RUNNING);
@@ -3881,6 +3890,7 @@ static void device_init(int adapter_num, struct pci_dev *pdev)
 }
 
 static const struct tty_operations ops = {
+	.install = install,
 	.open = open,
 	.close = close,
 	.write = write,
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index b425c79675ad..8a5a8b064616 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -181,10 +181,13 @@ struct tty_struct *alloc_tty_struct(void)
 
 void free_tty_struct(struct tty_struct *tty)
 {
+	if (!tty)
+		return;
 	if (tty->dev)
 		put_device(tty->dev);
 	kfree(tty->write_buf);
 	tty_buffer_free_all(tty);
+	tty->magic = 0xDEADDEAD;
 	kfree(tty);
 }
 
@@ -573,7 +576,7 @@ void __tty_hangup(struct tty_struct *tty)
 	}
 	spin_unlock(&redirect_lock);
 
-	tty_lock();
+	tty_lock(tty);
 
 	/* some functions below drop BTM, so we need this bit */
 	set_bit(TTY_HUPPING, &tty->flags);
@@ -666,7 +669,7 @@ void __tty_hangup(struct tty_struct *tty)
 	clear_bit(TTY_HUPPING, &tty->flags);
 	tty_ldisc_enable(tty);
 
-	tty_unlock();
+	tty_unlock(tty);
 
 	if (f)
 		fput(f);
@@ -1103,12 +1106,12 @@ void tty_write_message(struct tty_struct *tty, char *msg)
 {
 	if (tty) {
 		mutex_lock(&tty->atomic_write_lock);
-		tty_lock();
+		tty_lock(tty);
 		if (tty->ops->write && !test_bit(TTY_CLOSING, &tty->flags)) {
-			tty_unlock();
+			tty_unlock(tty);
 			tty->ops->write(tty, msg, strlen(msg));
 		} else
-			tty_unlock();
+			tty_unlock(tty);
 		tty_write_unlock(tty);
 	}
 	return;
@@ -1213,7 +1216,10 @@ static void pty_line_name(struct tty_driver *driver, int index, char *p)
  */
 static void tty_line_name(struct tty_driver *driver, int index, char *p)
 {
-	sprintf(p, "%s%d", driver->name, index + driver->name_base);
+	if (driver->flags & TTY_DRIVER_UNNUMBERED_NODE)
+		strcpy(p, driver->name);
+	else
+		sprintf(p, "%s%d", driver->name, index + driver->name_base);
 }
 
 /**
@@ -1249,21 +1255,19 @@ int tty_init_termios(struct tty_struct *tty)
 	struct ktermios *tp;
 	int idx = tty->index;
 
-	tp = tty->driver->termios[idx];
-	if (tp == NULL) {
-		tp = kzalloc(sizeof(struct ktermios[2]), GFP_KERNEL);
-		if (tp == NULL)
-			return -ENOMEM;
-		memcpy(tp, &tty->driver->init_termios,
-						sizeof(struct ktermios));
-		tty->driver->termios[idx] = tp;
+	if (tty->driver->flags & TTY_DRIVER_RESET_TERMIOS)
+		tty->termios = tty->driver->init_termios;
+	else {
+		/* Check for lazy saved data */
+		tp = tty->driver->termios[idx];
+		if (tp != NULL)
+			tty->termios = *tp;
+		else
+			tty->termios = tty->driver->init_termios;
 	}
-	tty->termios = tp;
-	tty->termios_locked = tp + 1;
-
 	/* Compatibility until drivers always set this */
-	tty->termios->c_ispeed = tty_termios_input_baud_rate(tty->termios);
-	tty->termios->c_ospeed = tty_termios_baud_rate(tty->termios);
+	tty->termios.c_ispeed = tty_termios_input_baud_rate(&tty->termios);
+	tty->termios.c_ospeed = tty_termios_baud_rate(&tty->termios);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(tty_init_termios);
@@ -1403,10 +1407,18 @@ struct tty_struct *tty_init_dev(struct tty_driver *driver, int idx)
 	}
 	initialize_tty_struct(tty, driver, idx);
 
+	tty_lock(tty);
 	retval = tty_driver_install_tty(driver, tty);
 	if (retval < 0)
 		goto err_deinit_tty;
 
+	if (!tty->port)
+		tty->port = driver->ports[idx];
+
+	WARN_RATELIMIT(!tty->port,
+			"%s: %s driver does not set tty->port. This will crash the kernel later. Fix the driver!\n",
+			__func__, tty->driver->name);
+
 	/*
 	 * Structures all installed ... call the ldisc open routines.
 	 * If we fail here just call release_tty to clean up.  No need
@@ -1415,9 +1427,11 @@ struct tty_struct *tty_init_dev(struct tty_driver *driver, int idx)
 	retval = tty_ldisc_setup(tty, tty->link);
 	if (retval)
 		goto err_release_tty;
+	/* Return the tty locked so that it cannot vanish under the caller */
 	return tty;
 
 err_deinit_tty:
+	tty_unlock(tty);
 	deinitialize_tty_struct(tty);
 	free_tty_struct(tty);
 err_module_put:
@@ -1426,6 +1440,7 @@ err_module_put:
 
 	/* call the tty release_tty routine to clean out this slot */
 err_release_tty:
+	tty_unlock(tty);
 	printk_ratelimited(KERN_INFO "tty_init_dev: ldisc open failed, "
 				 "clearing slot %d\n", idx);
 	release_tty(tty, idx);
@@ -1436,22 +1451,25 @@ void tty_free_termios(struct tty_struct *tty)
 {
 	struct ktermios *tp;
 	int idx = tty->index;
-	/* Kill this flag and push into drivers for locking etc */
-	if (tty->driver->flags & TTY_DRIVER_RESET_TERMIOS) {
-		/* FIXME: Locking on ->termios array */
-		tp = tty->termios;
-		tty->driver->termios[idx] = NULL;
-		kfree(tp);
+
+	/* If the port is going to reset then it has no termios to save */
+	if (tty->driver->flags & TTY_DRIVER_RESET_TERMIOS)
+		return;
+
+	/* Stash the termios data */
+	tp = tty->driver->termios[idx];
+	if (tp == NULL) {
+		tp = kmalloc(sizeof(struct ktermios), GFP_KERNEL);
+		if (tp == NULL) {
+			pr_warn("tty: no memory to save termios state.\n");
+			return;
+		}
+		tty->driver->termios[idx] = tp;
 	}
+	*tp = tty->termios;
 }
 EXPORT_SYMBOL(tty_free_termios);
 
-void tty_shutdown(struct tty_struct *tty)
-{
-	tty_driver_remove_tty(tty->driver, tty);
-	tty_free_termios(tty);
-}
-EXPORT_SYMBOL(tty_shutdown);
 
 /**
  *	release_one_tty		-	release tty structure memory
@@ -1462,7 +1480,6 @@ EXPORT_SYMBOL(tty_shutdown);
  *	in use. It also gets called when setup of a device fails.
  *
  *	Locking:
- *		tty_mutex - sometimes only
  *		takes the file list lock internally when working on the list
  *	of ttys that the driver keeps.
  *
@@ -1495,11 +1512,6 @@ static void queue_release_one_tty(struct kref *kref)
 {
 	struct tty_struct *tty = container_of(kref, struct tty_struct, kref);
 
-	if (tty->ops->shutdown)
-		tty->ops->shutdown(tty);
-	else
-		tty_shutdown(tty);
-
 	/* The hangup queue is now free so we can reuse it rather than
 	   waste a chunk of memory for each port */
 	INIT_WORK(&tty->hangup_work, release_one_tty);
@@ -1528,16 +1540,20 @@ EXPORT_SYMBOL(tty_kref_put);
  *	and decrement the refcount of the backing module.
  *
  *	Locking:
- *		tty_mutex - sometimes only
+ *		tty_mutex
  *		takes the file list lock internally when working on the list
  *	of ttys that the driver keeps.
- *		FIXME: should we require tty_mutex is held here ??
  *
  */
 static void release_tty(struct tty_struct *tty, int idx)
 {
 	/* This should always be true but check for the moment */
 	WARN_ON(tty->index != idx);
+	WARN_ON(!mutex_is_locked(&tty_mutex));
+	if (tty->ops->shutdown)
+		tty->ops->shutdown(tty);
+	tty_free_termios(tty);
+	tty_driver_remove_tty(tty->driver, tty);
 
 	if (tty->link)
 		tty_kref_put(tty->link);
@@ -1572,22 +1588,12 @@ static int tty_release_checks(struct tty_struct *tty, struct tty_struct *o_tty,
 				__func__, idx, tty->name);
 		return -1;
 	}
-	if (tty->termios != tty->driver->termios[idx]) {
-		printk(KERN_DEBUG "%s: driver.termios[%d] not termios for (%s)\n",
-				__func__, idx, tty->name);
-		return -1;
-	}
 	if (tty->driver->other) {
 		if (o_tty != tty->driver->other->ttys[idx]) {
 			printk(KERN_DEBUG "%s: other->table[%d] not o_tty for (%s)\n",
 					__func__, idx, tty->name);
 			return -1;
 		}
-		if (o_tty->termios != tty->driver->other->termios[idx]) {
-			printk(KERN_DEBUG "%s: other->termios[%d] not o_termios for (%s)\n",
-					__func__, idx, tty->name);
-			return -1;
-		}
 		if (o_tty->link != tty) {
 			printk(KERN_DEBUG "%s: bad pty pointers\n", __func__);
 			return -1;
@@ -1628,7 +1634,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	if (tty_paranoia_check(tty, inode, __func__))
 		return 0;
 
-	tty_lock();
+	tty_lock(tty);
 	check_tty_count(tty, __func__);
 
 	__tty_fasync(-1, filp, 0);
@@ -1637,10 +1643,11 @@ int tty_release(struct inode *inode, struct file *filp)
 	pty_master = (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
 		      tty->driver->subtype == PTY_TYPE_MASTER);
 	devpts = (tty->driver->flags & TTY_DRIVER_DEVPTS_MEM) != 0;
+	/* Review: parallel close */
 	o_tty = tty->link;
 
 	if (tty_release_checks(tty, o_tty, idx)) {
-		tty_unlock();
+		tty_unlock(tty);
 		return 0;
 	}
 
@@ -1652,7 +1659,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	if (tty->ops->close)
 		tty->ops->close(tty, filp);
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 * Sanity check: if tty->count is going to zero, there shouldn't be
 	 * any waiters on tty->read_wait or tty->write_wait.  We test the
@@ -1675,7 +1682,7 @@ int tty_release(struct inode *inode, struct file *filp)
 		   opens on /dev/tty */
 
 		mutex_lock(&tty_mutex);
-		tty_lock();
+		tty_lock_pair(tty, o_tty);
 		tty_closing = tty->count <= 1;
 		o_tty_closing = o_tty &&
 			(o_tty->count <= (pty_master ? 1 : 0));
@@ -1706,7 +1713,7 @@ int tty_release(struct inode *inode, struct file *filp)
 
 		printk(KERN_WARNING "%s: %s: read/write wait queue active!\n",
 				__func__, tty_name(tty, buf));
-		tty_unlock();
+		tty_unlock_pair(tty, o_tty);
 		mutex_unlock(&tty_mutex);
 		schedule();
 	}
@@ -1715,6 +1722,9 @@ int tty_release(struct inode *inode, struct file *filp)
 	 * The closing flags are now consistent with the open counts on
 	 * both sides, and we've completed the last operation that could
 	 * block, so it's safe to proceed with closing.
+	 *
+	 * We must *not* drop the tty_mutex until we ensure that a further
+	 * entry into tty_open can not pick up this tty.
 	 */
 	if (pty_master) {
 		if (--o_tty->count < 0) {
@@ -1766,12 +1776,13 @@ int tty_release(struct inode *inode, struct file *filp)
 	}
 
 	mutex_unlock(&tty_mutex);
+	tty_unlock_pair(tty, o_tty);
+	/* At this point the TTY_CLOSING flag should ensure a dead tty
+	   cannot be re-opened by a racing opener */
 
 	/* check whether both sides are closing ... */
-	if (!tty_closing || (o_tty && !o_tty_closing)) {
-		tty_unlock();
+	if (!tty_closing || (o_tty && !o_tty_closing))
 		return 0;
-	}
 
 #ifdef TTY_DEBUG_HANGUP
 	printk(KERN_DEBUG "%s: freeing tty structure...\n", __func__);
@@ -1782,14 +1793,17 @@ int tty_release(struct inode *inode, struct file *filp)
 	tty_ldisc_release(tty, o_tty);
 	/*
 	 * The release_tty function takes care of the details of clearing
-	 * the slots and preserving the termios structure.
+	 * the slots and preserving the termios structure. The tty_unlock_pair
+	 * should be safe as we keep a kref while the tty is locked (so the
+	 * unlock never unlocks a freed tty).
 	 */
+	mutex_lock(&tty_mutex);
 	release_tty(tty, idx);
+	mutex_unlock(&tty_mutex);
 
 	/* Make this pty number available for reallocation */
 	if (devpts)
 		devpts_kill_index(inode, idx);
-	tty_unlock();
 	return 0;
 }
 
@@ -1893,6 +1907,9 @@ static struct tty_driver *tty_lookup_driver(dev_t device, struct file *filp,
  *	Locking: tty_mutex protects tty, tty_lookup_driver and tty_init_dev.
  *		 tty->count should protect the rest.
  *		 ->siglock protects ->signal/->sighand
+ *
+ *	Note: the tty_unlock/lock cases without a ref are only safe due to
+ *	tty_mutex
  */
 
 static int tty_open(struct inode *inode, struct file *filp)
@@ -1916,8 +1933,7 @@ retry_open:
 	retval = 0;
 
 	mutex_lock(&tty_mutex);
-	tty_lock();
-
+	/* This is protected by the tty_mutex */
 	tty = tty_open_current_tty(device, filp);
 	if (IS_ERR(tty)) {
 		retval = PTR_ERR(tty);
@@ -1938,17 +1954,19 @@ retry_open:
 	}
 
 	if (tty) {
+		tty_lock(tty);
 		retval = tty_reopen(tty);
-		if (retval)
+		if (retval < 0) {
+			tty_unlock(tty);
 			tty = ERR_PTR(retval);
-	} else
+		}
+	} else	/* Returns with the tty_lock held for now */
 		tty = tty_init_dev(driver, index);
 
 	mutex_unlock(&tty_mutex);
 	if (driver)
 		tty_driver_kref_put(driver);
 	if (IS_ERR(tty)) {
-		tty_unlock();
 		retval = PTR_ERR(tty);
 		goto err_file;
 	}
@@ -1977,7 +1995,7 @@ retry_open:
 		printk(KERN_DEBUG "%s: error %d in opening %s...\n", __func__,
 				retval, tty->name);
 #endif
-		tty_unlock(); /* need to call tty_release without BTM */
+		tty_unlock(tty); /* need to call tty_release without BTM */
 		tty_release(inode, filp);
 		if (retval != -ERESTARTSYS)
 			return retval;
@@ -1989,17 +2007,15 @@ retry_open:
 		/*
 		 * Need to reset f_op in case a hangup happened.
 		 */
-		tty_lock();
 		if (filp->f_op == &hung_up_tty_fops)
 			filp->f_op = &tty_fops;
-		tty_unlock();
 		goto retry_open;
 	}
-	tty_unlock();
+	tty_unlock(tty);
 
 
 	mutex_lock(&tty_mutex);
-	tty_lock();
+	tty_lock(tty);
 	spin_lock_irq(&current->sighand->siglock);
 	if (!noctty &&
 	    current->signal->leader &&
@@ -2007,11 +2023,10 @@ retry_open:
 	    tty->session == NULL)
 		__proc_set_tty(current, tty);
 	spin_unlock_irq(&current->sighand->siglock);
-	tty_unlock();
+	tty_unlock(tty);
 	mutex_unlock(&tty_mutex);
 	return 0;
 err_unlock:
-	tty_unlock();
 	mutex_unlock(&tty_mutex);
 	/* after locks to avoid deadlock */
 	if (!IS_ERR_OR_NULL(driver))
@@ -2094,10 +2109,13 @@ out:
 
 static int tty_fasync(int fd, struct file *filp, int on)
 {
+	struct tty_struct *tty = file_tty(filp);
 	int retval;
-	tty_lock();
+
+	tty_lock(tty);
 	retval = __tty_fasync(fd, filp, on);
-	tty_unlock();
+	tty_unlock(tty);
+
 	return retval;
 }
 
@@ -2756,7 +2774,7 @@ long tty_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	if (ld->ops->ioctl) {
 		retval = ld->ops->ioctl(tty, file, cmd, arg);
 		if (retval == -ENOIOCTLCMD)
-			retval = -EINVAL;
+			retval = -ENOTTY;
 	}
 	tty_ldisc_deref(ld);
 	return retval;
@@ -2934,6 +2952,7 @@ void initialize_tty_struct(struct tty_struct *tty,
 	tty->pgrp = NULL;
 	tty->overrun_time = jiffies;
 	tty_buffer_init(tty);
+	mutex_init(&tty->legacy_mutex);
 	mutex_init(&tty->termios_mutex);
 	mutex_init(&tty->ldisc_mutex);
 	init_waitqueue_head(&tty->write_wait);
@@ -2991,6 +3010,15 @@ EXPORT_SYMBOL_GPL(tty_put_char);
 
 struct class *tty_class;
 
+static int tty_cdev_add(struct tty_driver *driver, dev_t dev,
+		unsigned int index, unsigned int count)
+{
+	/* init here, since reused cdevs cause crashes */
+	cdev_init(&driver->cdevs[index], &tty_fops);
+	driver->cdevs[index].owner = driver->owner;
+	return cdev_add(&driver->cdevs[index], dev, count);
+}
+
 /**
  *	tty_register_device - register a tty device
  *	@driver: the tty driver that describes the tty device
@@ -3013,8 +3041,46 @@ struct class *tty_class;
 struct device *tty_register_device(struct tty_driver *driver, unsigned index,
 				   struct device *device)
 {
+	return tty_register_device_attr(driver, index, device, NULL, NULL);
+}
+EXPORT_SYMBOL(tty_register_device);
+
+static void tty_device_create_release(struct device *dev)
+{
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
+	kfree(dev);
+}
+
+/**
+ *	tty_register_device_attr - register a tty device
+ *	@driver: the tty driver that describes the tty device
+ *	@index: the index in the tty driver for this tty device
+ *	@device: a struct device that is associated with this tty device.
+ *		This field is optional, if there is no known struct device
+ *		for this tty device it can be set to NULL safely.
+ *	@drvdata: Driver data to be set to device.
+ *	@attr_grp: Attribute group to be set on device.
+ *
+ *	Returns a pointer to the struct device for this tty device
+ *	(or ERR_PTR(-EFOO) on error).
+ *
+ *	This call is required to be made to register an individual tty device
+ *	if the tty driver's flags have the TTY_DRIVER_DYNAMIC_DEV bit set.  If
+ *	that bit is not set, this function should not be called by a tty
+ *	driver.
+ *
+ *	Locking: ??
+ */
+struct device *tty_register_device_attr(struct tty_driver *driver,
+				   unsigned index, struct device *device,
+				   void *drvdata,
+				   const struct attribute_group **attr_grp)
+{
 	char name[64];
-	dev_t dev = MKDEV(driver->major, driver->minor_start) + index;
+	dev_t devt = MKDEV(driver->major, driver->minor_start) + index;
+	struct device *dev = NULL;
+	int retval = -ENODEV;
+	bool cdev = false;
 
 	if (index >= driver->num) {
 		printk(KERN_ERR "Attempt to register invalid tty line number "
@@ -3027,9 +3093,40 @@ struct device *tty_register_device(struct tty_driver *driver, unsigned index,
 	else
 		tty_line_name(driver, index, name);
 
-	return device_create(tty_class, device, dev, NULL, name);
+	if (!(driver->flags & TTY_DRIVER_DYNAMIC_ALLOC)) {
+		retval = tty_cdev_add(driver, devt, index, 1);
+		if (retval)
+			goto error;
+		cdev = true;
+	}
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		retval = -ENOMEM;
+		goto error;
+	}
+
+	dev->devt = devt;
+	dev->class = tty_class;
+	dev->parent = device;
+	dev->release = tty_device_create_release;
+	dev_set_name(dev, "%s", name);
+	dev->groups = attr_grp;
+	dev_set_drvdata(dev, drvdata);
+
+	retval = device_register(dev);
+	if (retval)
+		goto error;
+
+	return dev;
+
+error:
+	put_device(dev);
+	if (cdev)
+		cdev_del(&driver->cdevs[index]);
+	return ERR_PTR(retval);
 }
-EXPORT_SYMBOL(tty_register_device);
+EXPORT_SYMBOL_GPL(tty_register_device_attr);
 
 /**
  * 	tty_unregister_device - unregister a tty device
@@ -3046,31 +3143,82 @@ void tty_unregister_device(struct tty_driver *driver, unsigned index)
 {
 	device_destroy(tty_class,
 		MKDEV(driver->major, driver->minor_start) + index);
+	if (!(driver->flags & TTY_DRIVER_DYNAMIC_ALLOC))
+		cdev_del(&driver->cdevs[index]);
 }
 EXPORT_SYMBOL(tty_unregister_device);
 
-struct tty_driver *__alloc_tty_driver(int lines, struct module *owner)
+/**
+ * __tty_alloc_driver -- allocate tty driver
+ * @lines: count of lines this driver can handle at most
+ * @owner: module which is repsonsible for this driver
+ * @flags: some of TTY_DRIVER_* flags, will be set in driver->flags
+ *
+ * This should not be called directly, some of the provided macros should be
+ * used instead. Use IS_ERR and friends on @retval.
+ */
+struct tty_driver *__tty_alloc_driver(unsigned int lines, struct module *owner,
+		unsigned long flags)
 {
 	struct tty_driver *driver;
+	unsigned int cdevs = 1;
+	int err;
+
+	if (!lines || (flags & TTY_DRIVER_UNNUMBERED_NODE && lines > 1))
+		return ERR_PTR(-EINVAL);
 
 	driver = kzalloc(sizeof(struct tty_driver), GFP_KERNEL);
-	if (driver) {
-		kref_init(&driver->kref);
-		driver->magic = TTY_DRIVER_MAGIC;
-		driver->num = lines;
-		driver->owner = owner;
-		/* later we'll move allocation of tables here */
+	if (!driver)
+		return ERR_PTR(-ENOMEM);
+
+	kref_init(&driver->kref);
+	driver->magic = TTY_DRIVER_MAGIC;
+	driver->num = lines;
+	driver->owner = owner;
+	driver->flags = flags;
+
+	if (!(flags & TTY_DRIVER_DEVPTS_MEM)) {
+		driver->ttys = kcalloc(lines, sizeof(*driver->ttys),
+				GFP_KERNEL);
+		driver->termios = kcalloc(lines, sizeof(*driver->termios),
+				GFP_KERNEL);
+		if (!driver->ttys || !driver->termios) {
+			err = -ENOMEM;
+			goto err_free_all;
+		}
 	}
+
+	if (!(flags & TTY_DRIVER_DYNAMIC_ALLOC)) {
+		driver->ports = kcalloc(lines, sizeof(*driver->ports),
+				GFP_KERNEL);
+		if (!driver->ports) {
+			err = -ENOMEM;
+			goto err_free_all;
+		}
+		cdevs = lines;
+	}
+
+	driver->cdevs = kcalloc(cdevs, sizeof(*driver->cdevs), GFP_KERNEL);
+	if (!driver->cdevs) {
+		err = -ENOMEM;
+		goto err_free_all;
+	}
+
 	return driver;
+err_free_all:
+	kfree(driver->ports);
+	kfree(driver->ttys);
+	kfree(driver->termios);
+	kfree(driver);
+	return ERR_PTR(err);
 }
-EXPORT_SYMBOL(__alloc_tty_driver);
+EXPORT_SYMBOL(__tty_alloc_driver);
 
 static void destruct_tty_driver(struct kref *kref)
 {
 	struct tty_driver *driver = container_of(kref, struct tty_driver, kref);
 	int i;
 	struct ktermios *tp;
-	void *p;
 
 	if (driver->flags & TTY_DRIVER_INSTALLED) {
 		/*
@@ -3087,13 +3235,14 @@ static void destruct_tty_driver(struct kref *kref)
 			if (!(driver->flags & TTY_DRIVER_DYNAMIC_DEV))
 				tty_unregister_device(driver, i);
 		}
-		p = driver->ttys;
 		proc_tty_unregister_driver(driver);
-		driver->ttys = NULL;
-		driver->termios = NULL;
-		kfree(p);
-		cdev_del(&driver->cdev);
+		if (driver->flags & TTY_DRIVER_DYNAMIC_ALLOC)
+			cdev_del(&driver->cdevs[0]);
 	}
+	kfree(driver->cdevs);
+	kfree(driver->ports);
+	kfree(driver->termios);
+	kfree(driver->ttys);
 	kfree(driver);
 }
 
@@ -3124,15 +3273,8 @@ int tty_register_driver(struct tty_driver *driver)
 	int error;
 	int i;
 	dev_t dev;
-	void **p = NULL;
 	struct device *d;
 
-	if (!(driver->flags & TTY_DRIVER_DEVPTS_MEM) && driver->num) {
-		p = kzalloc(driver->num * 2 * sizeof(void *), GFP_KERNEL);
-		if (!p)
-			return -ENOMEM;
-	}
-
 	if (!driver->major) {
 		error = alloc_chrdev_region(&dev, driver->minor_start,
 						driver->num, driver->name);
@@ -3144,28 +3286,13 @@ int tty_register_driver(struct tty_driver *driver)
 		dev = MKDEV(driver->major, driver->minor_start);
 		error = register_chrdev_region(dev, driver->num, driver->name);
 	}
-	if (error < 0) {
-		kfree(p);
-		return error;
-	}
+	if (error < 0)
+		goto err;
 
-	if (p) {
-		driver->ttys = (struct tty_struct **)p;
-		driver->termios = (struct ktermios **)(p + driver->num);
-	} else {
-		driver->ttys = NULL;
-		driver->termios = NULL;
-	}
-
-	cdev_init(&driver->cdev, &tty_fops);
-	driver->cdev.owner = driver->owner;
-	error = cdev_add(&driver->cdev, dev, driver->num);
-	if (error) {
-		unregister_chrdev_region(dev, driver->num);
-		driver->ttys = NULL;
-		driver->termios = NULL;
-		kfree(p);
-		return error;
+	if (driver->flags & TTY_DRIVER_DYNAMIC_ALLOC) {
+		error = tty_cdev_add(driver, dev, 0, driver->num);
+		if (error)
+			goto err_unreg_char;
 	}
 
 	mutex_lock(&tty_mutex);
@@ -3177,7 +3304,7 @@ int tty_register_driver(struct tty_driver *driver)
 			d = tty_register_device(driver, i, NULL);
 			if (IS_ERR(d)) {
 				error = PTR_ERR(d);
-				goto err;
+				goto err_unreg_devs;
 			}
 		}
 	}
@@ -3185,7 +3312,7 @@ int tty_register_driver(struct tty_driver *driver)
 	driver->flags |= TTY_DRIVER_INSTALLED;
 	return 0;
 
-err:
+err_unreg_devs:
 	for (i--; i >= 0; i--)
 		tty_unregister_device(driver, i);
 
@@ -3193,13 +3320,11 @@ err:
 	list_del(&driver->tty_drivers);
 	mutex_unlock(&tty_mutex);
 
+err_unreg_char:
 	unregister_chrdev_region(dev, driver->num);
-	driver->ttys = NULL;
-	driver->termios = NULL;
-	kfree(p);
+err:
 	return error;
 }
-
 EXPORT_SYMBOL(tty_register_driver);
 
 /*
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index a1b9a2f68567..12b1fa0f4f86 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -410,7 +410,7 @@ EXPORT_SYMBOL_GPL(tty_termios_encode_baud_rate);
 
 void tty_encode_baud_rate(struct tty_struct *tty, speed_t ibaud, speed_t obaud)
 {
-	tty_termios_encode_baud_rate(tty->termios, ibaud, obaud);
+	tty_termios_encode_baud_rate(&tty->termios, ibaud, obaud);
 }
 EXPORT_SYMBOL_GPL(tty_encode_baud_rate);
 
@@ -427,7 +427,7 @@ EXPORT_SYMBOL_GPL(tty_encode_baud_rate);
 
 speed_t tty_get_baud_rate(struct tty_struct *tty)
 {
-	speed_t baud = tty_termios_baud_rate(tty->termios);
+	speed_t baud = tty_termios_baud_rate(&tty->termios);
 
 	if (baud == 38400 && tty->alt_speed) {
 		if (!tty->warned) {
@@ -509,14 +509,14 @@ int tty_set_termios(struct tty_struct *tty, struct ktermios *new_termios)
 	/* FIXME: we need to decide on some locking/ordering semantics
 	   for the set_termios notification eventually */
 	mutex_lock(&tty->termios_mutex);
-	old_termios = *tty->termios;
-	*tty->termios = *new_termios;
-	unset_locked_termios(tty->termios, &old_termios, tty->termios_locked);
+	old_termios = tty->termios;
+	tty->termios = *new_termios;
+	unset_locked_termios(&tty->termios, &old_termios, &tty->termios_locked);
 
 	/* See if packet mode change of state. */
 	if (tty->link && tty->link->packet) {
 		int extproc = (old_termios.c_lflag & EXTPROC) |
-				(tty->termios->c_lflag & EXTPROC);
+				(tty->termios.c_lflag & EXTPROC);
 		int old_flow = ((old_termios.c_iflag & IXON) &&
 				(old_termios.c_cc[VSTOP] == '\023') &&
 				(old_termios.c_cc[VSTART] == '\021'));
@@ -542,7 +542,7 @@ int tty_set_termios(struct tty_struct *tty, struct ktermios *new_termios)
 	if (tty->ops->set_termios)
 		(*tty->ops->set_termios)(tty, &old_termios);
 	else
-		tty_termios_copy_hw(tty->termios, &old_termios);
+		tty_termios_copy_hw(&tty->termios, &old_termios);
 
 	ld = tty_ldisc_ref(tty);
 	if (ld != NULL) {
@@ -578,7 +578,7 @@ static int set_termios(struct tty_struct *tty, void __user *arg, int opt)
 		return retval;
 
 	mutex_lock(&tty->termios_mutex);
-	memcpy(&tmp_termios, tty->termios, sizeof(struct ktermios));
+	tmp_termios = tty->termios;
 	mutex_unlock(&tty->termios_mutex);
 
 	if (opt & TERMIOS_TERMIO) {
@@ -632,14 +632,14 @@ static int set_termios(struct tty_struct *tty, void __user *arg, int opt)
 static void copy_termios(struct tty_struct *tty, struct ktermios *kterm)
 {
 	mutex_lock(&tty->termios_mutex);
-	memcpy(kterm, tty->termios, sizeof(struct ktermios));
+	*kterm = tty->termios;
 	mutex_unlock(&tty->termios_mutex);
 }
 
 static void copy_termios_locked(struct tty_struct *tty, struct ktermios *kterm)
 {
 	mutex_lock(&tty->termios_mutex);
-	memcpy(kterm, tty->termios_locked, sizeof(struct ktermios));
+	*kterm = tty->termios_locked;
 	mutex_unlock(&tty->termios_mutex);
 }
 
@@ -707,16 +707,16 @@ static int get_sgflags(struct tty_struct *tty)
 {
 	int flags = 0;
 
-	if (!(tty->termios->c_lflag & ICANON)) {
-		if (tty->termios->c_lflag & ISIG)
+	if (!(tty->termios.c_lflag & ICANON)) {
+		if (tty->termios.c_lflag & ISIG)
 			flags |= 0x02;		/* cbreak */
 		else
 			flags |= 0x20;		/* raw */
 	}
-	if (tty->termios->c_lflag & ECHO)
+	if (tty->termios.c_lflag & ECHO)
 		flags |= 0x08;			/* echo */
-	if (tty->termios->c_oflag & OPOST)
-		if (tty->termios->c_oflag & ONLCR)
+	if (tty->termios.c_oflag & OPOST)
+		if (tty->termios.c_oflag & ONLCR)
 			flags |= 0x10;		/* crmod */
 	return flags;
 }
@@ -726,10 +726,10 @@ static int get_sgttyb(struct tty_struct *tty, struct sgttyb __user *sgttyb)
 	struct sgttyb tmp;
 
 	mutex_lock(&tty->termios_mutex);
-	tmp.sg_ispeed = tty->termios->c_ispeed;
-	tmp.sg_ospeed = tty->termios->c_ospeed;
-	tmp.sg_erase = tty->termios->c_cc[VERASE];
-	tmp.sg_kill = tty->termios->c_cc[VKILL];
+	tmp.sg_ispeed = tty->termios.c_ispeed;
+	tmp.sg_ospeed = tty->termios.c_ospeed;
+	tmp.sg_erase = tty->termios.c_cc[VERASE];
+	tmp.sg_kill = tty->termios.c_cc[VKILL];
 	tmp.sg_flags = get_sgflags(tty);
 	mutex_unlock(&tty->termios_mutex);
 
@@ -787,7 +787,7 @@ static int set_sgttyb(struct tty_struct *tty, struct sgttyb __user *sgttyb)
 		return -EFAULT;
 
 	mutex_lock(&tty->termios_mutex);
-	termios = *tty->termios;
+	termios = tty->termios;
 	termios.c_cc[VERASE] = tmp.sg_erase;
 	termios.c_cc[VKILL] = tmp.sg_kill;
 	set_sgflags(&termios, tmp.sg_flags);
@@ -808,12 +808,12 @@ static int get_tchars(struct tty_struct *tty, struct tchars __user *tchars)
 	struct tchars tmp;
 
 	mutex_lock(&tty->termios_mutex);
-	tmp.t_intrc = tty->termios->c_cc[VINTR];
-	tmp.t_quitc = tty->termios->c_cc[VQUIT];
-	tmp.t_startc = tty->termios->c_cc[VSTART];
-	tmp.t_stopc = tty->termios->c_cc[VSTOP];
-	tmp.t_eofc = tty->termios->c_cc[VEOF];
-	tmp.t_brkc = tty->termios->c_cc[VEOL2];	/* what is brkc anyway? */
+	tmp.t_intrc = tty->termios.c_cc[VINTR];
+	tmp.t_quitc = tty->termios.c_cc[VQUIT];
+	tmp.t_startc = tty->termios.c_cc[VSTART];
+	tmp.t_stopc = tty->termios.c_cc[VSTOP];
+	tmp.t_eofc = tty->termios.c_cc[VEOF];
+	tmp.t_brkc = tty->termios.c_cc[VEOL2];	/* what is brkc anyway? */
 	mutex_unlock(&tty->termios_mutex);
 	return copy_to_user(tchars, &tmp, sizeof(tmp)) ? -EFAULT : 0;
 }
@@ -825,12 +825,12 @@ static int set_tchars(struct tty_struct *tty, struct tchars __user *tchars)
 	if (copy_from_user(&tmp, tchars, sizeof(tmp)))
 		return -EFAULT;
 	mutex_lock(&tty->termios_mutex);
-	tty->termios->c_cc[VINTR] = tmp.t_intrc;
-	tty->termios->c_cc[VQUIT] = tmp.t_quitc;
-	tty->termios->c_cc[VSTART] = tmp.t_startc;
-	tty->termios->c_cc[VSTOP] = tmp.t_stopc;
-	tty->termios->c_cc[VEOF] = tmp.t_eofc;
-	tty->termios->c_cc[VEOL2] = tmp.t_brkc;	/* what is brkc anyway? */
+	tty->termios.c_cc[VINTR] = tmp.t_intrc;
+	tty->termios.c_cc[VQUIT] = tmp.t_quitc;
+	tty->termios.c_cc[VSTART] = tmp.t_startc;
+	tty->termios.c_cc[VSTOP] = tmp.t_stopc;
+	tty->termios.c_cc[VEOF] = tmp.t_eofc;
+	tty->termios.c_cc[VEOL2] = tmp.t_brkc;	/* what is brkc anyway? */
 	mutex_unlock(&tty->termios_mutex);
 	return 0;
 }
@@ -842,14 +842,14 @@ static int get_ltchars(struct tty_struct *tty, struct ltchars __user *ltchars)
 	struct ltchars tmp;
 
 	mutex_lock(&tty->termios_mutex);
-	tmp.t_suspc = tty->termios->c_cc[VSUSP];
+	tmp.t_suspc = tty->termios.c_cc[VSUSP];
 	/* what is dsuspc anyway? */
-	tmp.t_dsuspc = tty->termios->c_cc[VSUSP];
-	tmp.t_rprntc = tty->termios->c_cc[VREPRINT];
+	tmp.t_dsuspc = tty->termios.c_cc[VSUSP];
+	tmp.t_rprntc = tty->termios.c_cc[VREPRINT];
 	/* what is flushc anyway? */
-	tmp.t_flushc = tty->termios->c_cc[VEOL2];
-	tmp.t_werasc = tty->termios->c_cc[VWERASE];
-	tmp.t_lnextc = tty->termios->c_cc[VLNEXT];
+	tmp.t_flushc = tty->termios.c_cc[VEOL2];
+	tmp.t_werasc = tty->termios.c_cc[VWERASE];
+	tmp.t_lnextc = tty->termios.c_cc[VLNEXT];
 	mutex_unlock(&tty->termios_mutex);
 	return copy_to_user(ltchars, &tmp, sizeof(tmp)) ? -EFAULT : 0;
 }
@@ -862,14 +862,14 @@ static int set_ltchars(struct tty_struct *tty, struct ltchars __user *ltchars)
 		return -EFAULT;
 
 	mutex_lock(&tty->termios_mutex);
-	tty->termios->c_cc[VSUSP] = tmp.t_suspc;
+	tty->termios.c_cc[VSUSP] = tmp.t_suspc;
 	/* what is dsuspc anyway? */
-	tty->termios->c_cc[VEOL2] = tmp.t_dsuspc;
-	tty->termios->c_cc[VREPRINT] = tmp.t_rprntc;
+	tty->termios.c_cc[VEOL2] = tmp.t_dsuspc;
+	tty->termios.c_cc[VREPRINT] = tmp.t_rprntc;
 	/* what is flushc anyway? */
-	tty->termios->c_cc[VEOL2] = tmp.t_flushc;
-	tty->termios->c_cc[VWERASE] = tmp.t_werasc;
-	tty->termios->c_cc[VLNEXT] = tmp.t_lnextc;
+	tty->termios.c_cc[VEOL2] = tmp.t_flushc;
+	tty->termios.c_cc[VWERASE] = tmp.t_werasc;
+	tty->termios.c_cc[VLNEXT] = tmp.t_lnextc;
 	mutex_unlock(&tty->termios_mutex);
 	return 0;
 }
@@ -920,12 +920,12 @@ static int tty_change_softcar(struct tty_struct *tty, int arg)
 	struct ktermios old;
 
 	mutex_lock(&tty->termios_mutex);
-	old = *tty->termios;
-	tty->termios->c_cflag &= ~CLOCAL;
-	tty->termios->c_cflag |= bit;
+	old = tty->termios;
+	tty->termios.c_cflag &= ~CLOCAL;
+	tty->termios.c_cflag |= bit;
 	if (tty->ops->set_termios)
 		tty->ops->set_termios(tty, &old);
-	if ((tty->termios->c_cflag & CLOCAL) != bit)
+	if ((tty->termios.c_cflag & CLOCAL) != bit)
 		ret = -EINVAL;
 	mutex_unlock(&tty->termios_mutex);
 	return ret;
@@ -1031,7 +1031,7 @@ int tty_mode_ioctl(struct tty_struct *tty, struct file *file,
 					       (struct termios __user *) arg))
 			return -EFAULT;
 		mutex_lock(&real_tty->termios_mutex);
-		memcpy(real_tty->termios_locked, &kterm, sizeof(struct ktermios));
+		real_tty->termios_locked = kterm;
 		mutex_unlock(&real_tty->termios_mutex);
 		return 0;
 #else
@@ -1048,7 +1048,7 @@ int tty_mode_ioctl(struct tty_struct *tty, struct file *file,
 					       (struct termios __user *) arg))
 			return -EFAULT;
 		mutex_lock(&real_tty->termios_mutex);
-		memcpy(real_tty->termios_locked, &kterm, sizeof(struct ktermios));
+		real_tty->termios_locked = kterm;
 		mutex_unlock(&real_tty->termios_mutex);
 		return ret;
 #endif
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 6f99c9959f0c..4d7b56268c79 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -413,7 +413,7 @@ EXPORT_SYMBOL_GPL(tty_ldisc_flush);
 static void tty_set_termios_ldisc(struct tty_struct *tty, int num)
 {
 	mutex_lock(&tty->termios_mutex);
-	tty->termios->c_line = num;
+	tty->termios.c_line = num;
 	mutex_unlock(&tty->termios_mutex);
 }
 
@@ -568,7 +568,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	if (IS_ERR(new_ldisc))
 		return PTR_ERR(new_ldisc);
 
-	tty_lock();
+	tty_lock(tty);
 	/*
 	 *	We need to look at the tty locking here for pty/tty pairs
 	 *	when both sides try to change in parallel.
@@ -582,12 +582,12 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	 */
 
 	if (tty->ldisc->ops->num == ldisc) {
-		tty_unlock();
+		tty_unlock(tty);
 		tty_ldisc_put(new_ldisc);
 		return 0;
 	}
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 *	Problem: What do we do if this blocks ?
 	 *	We could deadlock here
@@ -595,7 +595,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	tty_wait_until_sent(tty, 0);
 
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/*
@@ -605,10 +605,10 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	while (test_bit(TTY_LDISC_CHANGING, &tty->flags)) {
 		mutex_unlock(&tty->ldisc_mutex);
-		tty_unlock();
+		tty_unlock(tty);
 		wait_event(tty_ldisc_wait,
 			test_bit(TTY_LDISC_CHANGING, &tty->flags) == 0);
-		tty_lock();
+		tty_lock(tty);
 		mutex_lock(&tty->ldisc_mutex);
 	}
 
@@ -623,7 +623,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	o_ldisc = tty->ldisc;
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 *	Make sure we don't change while someone holds a
 	 *	reference to the line discipline. The TTY_LDISC bit
@@ -650,7 +650,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	retval = tty_ldisc_wait_idle(tty, 5 * HZ);
 
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/* handle wait idle failure locked */
@@ -665,7 +665,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 		clear_bit(TTY_LDISC_CHANGING, &tty->flags);
 		mutex_unlock(&tty->ldisc_mutex);
 		tty_ldisc_put(new_ldisc);
-		tty_unlock();
+		tty_unlock(tty);
 		return -EIO;
 	}
 
@@ -708,7 +708,7 @@ enable:
 	if (o_work)
 		schedule_work(&o_tty->buf.work);
 	mutex_unlock(&tty->ldisc_mutex);
-	tty_unlock();
+	tty_unlock(tty);
 	return retval;
 }
 
@@ -722,9 +722,9 @@ enable:
 static void tty_reset_termios(struct tty_struct *tty)
 {
 	mutex_lock(&tty->termios_mutex);
-	*tty->termios = tty->driver->init_termios;
-	tty->termios->c_ispeed = tty_termios_input_baud_rate(tty->termios);
-	tty->termios->c_ospeed = tty_termios_baud_rate(tty->termios);
+	tty->termios = tty->driver->init_termios;
+	tty->termios.c_ispeed = tty_termios_input_baud_rate(&tty->termios);
+	tty->termios.c_ospeed = tty_termios_baud_rate(&tty->termios);
 	mutex_unlock(&tty->termios_mutex);
 }
 
@@ -816,11 +816,11 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	 * need to wait for another function taking the BTM
 	 */
 	clear_bit(TTY_LDISC, &tty->flags);
-	tty_unlock();
+	tty_unlock(tty);
 	cancel_work_sync(&tty->buf.work);
 	mutex_unlock(&tty->ldisc_mutex);
 retry:
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/* At this point we have a closed ldisc and we want to
@@ -831,7 +831,7 @@ retry:
 		if (atomic_read(&tty->ldisc->users) != 1) {
 			char cur_n[TASK_COMM_LEN], tty_n[64];
 			long timeout = 3 * HZ;
-			tty_unlock();
+			tty_unlock(tty);
 
 			while (tty_ldisc_wait_idle(tty, timeout) == -EBUSY) {
 				timeout = MAX_SCHEDULE_TIMEOUT;
@@ -846,7 +846,7 @@ retry:
 
 		if (reset == 0) {
 
-			if (!tty_ldisc_reinit(tty, tty->termios->c_line))
+			if (!tty_ldisc_reinit(tty, tty->termios.c_line))
 				err = tty_ldisc_open(tty, tty->ldisc);
 			else
 				err = 1;
@@ -894,6 +894,23 @@ int tty_ldisc_setup(struct tty_struct *tty, struct tty_struct *o_tty)
 	tty_ldisc_enable(tty);
 	return 0;
 }
+
+static void tty_ldisc_kill(struct tty_struct *tty)
+{
+	mutex_lock(&tty->ldisc_mutex);
+	/*
+	 * Now kill off the ldisc
+	 */
+	tty_ldisc_close(tty, tty->ldisc);
+	tty_ldisc_put(tty->ldisc);
+	/* Force an oops if we mess this up */
+	tty->ldisc = NULL;
+
+	/* Ensure the next open requests the N_TTY ldisc */
+	tty_set_termios_ldisc(tty, N_TTY);
+	mutex_unlock(&tty->ldisc_mutex);
+}
+
 /**
  *	tty_ldisc_release		-	release line discipline
  *	@tty: tty being shut down
@@ -912,28 +929,21 @@ void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
 	 * race with the set_ldisc code path.
 	 */
 
-	tty_unlock();
+	tty_lock_pair(tty, o_tty);
 	tty_ldisc_halt(tty);
 	tty_ldisc_flush_works(tty);
-	tty_lock();
-
-	mutex_lock(&tty->ldisc_mutex);
-	/*
-	 * Now kill off the ldisc
-	 */
-	tty_ldisc_close(tty, tty->ldisc);
-	tty_ldisc_put(tty->ldisc);
-	/* Force an oops if we mess this up */
-	tty->ldisc = NULL;
-
-	/* Ensure the next open requests the N_TTY ldisc */
-	tty_set_termios_ldisc(tty, N_TTY);
-	mutex_unlock(&tty->ldisc_mutex);
+	if (o_tty) {
+		tty_ldisc_halt(o_tty);
+		tty_ldisc_flush_works(o_tty);
+	}
 
 	/* This will need doing differently if we need to lock */
+	tty_ldisc_kill(tty);
+
 	if (o_tty)
-		tty_ldisc_release(o_tty, NULL);
+		tty_ldisc_kill(o_tty);
 
+	tty_unlock_pair(tty, o_tty);
 	/* And the memory resources remaining (buffers, termios) will be
 	   disposed of when the kref hits zero */
 }
diff --git a/drivers/tty/tty_mutex.c b/drivers/tty/tty_mutex.c
index 9ff986c32a21..67feac9e6ebb 100644
--- a/drivers/tty/tty_mutex.c
+++ b/drivers/tty/tty_mutex.c
@@ -4,29 +4,70 @@
 #include <linux/semaphore.h>
 #include <linux/sched.h>
 
-/*
- * The 'big tty mutex'
- *
- * This mutex is taken and released by tty_lock() and tty_unlock(),
- * replacing the older big kernel lock.
- * It can no longer be taken recursively, and does not get
- * released implicitly while sleeping.
- *
- * Don't use in new code.
- */
-static DEFINE_MUTEX(big_tty_mutex);
+/* Legacy tty mutex glue */
+
+enum {
+	TTY_MUTEX_NORMAL,
+	TTY_MUTEX_NESTED,
+};
 
 /*
  * Getting the big tty mutex.
  */
-void __lockfunc tty_lock(void)
+
+static void __lockfunc tty_lock_nested(struct tty_struct *tty,
+				       unsigned int subclass)
 {
-	mutex_lock(&big_tty_mutex);
+	if (tty->magic != TTY_MAGIC) {
+		printk(KERN_ERR "L Bad %p\n", tty);
+		WARN_ON(1);
+		return;
+	}
+	tty_kref_get(tty);
+	mutex_lock_nested(&tty->legacy_mutex, subclass);
+}
+
+void __lockfunc tty_lock(struct tty_struct *tty)
+{
+	return tty_lock_nested(tty, TTY_MUTEX_NORMAL);
 }
 EXPORT_SYMBOL(tty_lock);
 
-void __lockfunc tty_unlock(void)
+void __lockfunc tty_unlock(struct tty_struct *tty)
 {
-	mutex_unlock(&big_tty_mutex);
+	if (tty->magic != TTY_MAGIC) {
+		printk(KERN_ERR "U Bad %p\n", tty);
+		WARN_ON(1);
+		return;
+	}
+	mutex_unlock(&tty->legacy_mutex);
+	tty_kref_put(tty);
 }
 EXPORT_SYMBOL(tty_unlock);
+
+/*
+ * Getting the big tty mutex for a pair of ttys with lock ordering
+ * On a non pty/tty pair tty2 can be NULL which is just fine.
+ */
+void __lockfunc tty_lock_pair(struct tty_struct *tty,
+					struct tty_struct *tty2)
+{
+	if (tty < tty2) {
+		tty_lock(tty);
+		tty_lock_nested(tty2, TTY_MUTEX_NESTED);
+	} else {
+		if (tty2 && tty2 != tty)
+			tty_lock(tty2);
+		tty_lock_nested(tty, TTY_MUTEX_NESTED);
+	}
+}
+EXPORT_SYMBOL(tty_lock_pair);
+
+void __lockfunc tty_unlock_pair(struct tty_struct *tty,
+						struct tty_struct *tty2)
+{
+	tty_unlock(tty);
+	if (tty2 && tty2 != tty)
+		tty_unlock(tty2);
+}
+EXPORT_SYMBOL(tty_unlock_pair);
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index bf6e238146ae..d7bdd8d0c23f 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -33,6 +33,70 @@ void tty_port_init(struct tty_port *port)
 }
 EXPORT_SYMBOL(tty_port_init);
 
+/**
+ * tty_port_link_device - link tty and tty_port
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ *
+ * Provide the tty layer wit ha link from a tty (specified by @index) to a
+ * tty_port (@port). Use this only if neither tty_port_register_device nor
+ * tty_port_install is used in the driver. If used, this has to be called before
+ * tty_register_driver.
+ */
+void tty_port_link_device(struct tty_port *port,
+		struct tty_driver *driver, unsigned index)
+{
+	if (WARN_ON(index >= driver->num))
+		return;
+	driver->ports[index] = port;
+}
+EXPORT_SYMBOL_GPL(tty_port_link_device);
+
+/**
+ * tty_port_register_device - register tty device
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ * @device: parent if exists, otherwise NULL
+ *
+ * It is the same as tty_register_device except the provided @port is linked to
+ * a concrete tty specified by @index. Use this or tty_port_install (or both).
+ * Call tty_port_link_device as a last resort.
+ */
+struct device *tty_port_register_device(struct tty_port *port,
+		struct tty_driver *driver, unsigned index,
+		struct device *device)
+{
+	tty_port_link_device(port, driver, index);
+	return tty_register_device(driver, index, device);
+}
+EXPORT_SYMBOL_GPL(tty_port_register_device);
+
+/**
+ * tty_port_register_device_attr - register tty device
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ * @device: parent if exists, otherwise NULL
+ * @drvdata: Driver data to be set to device.
+ * @attr_grp: Attribute group to be set on device.
+ *
+ * It is the same as tty_register_device_attr except the provided @port is
+ * linked to a concrete tty specified by @index. Use this or tty_port_install
+ * (or both). Call tty_port_link_device as a last resort.
+ */
+struct device *tty_port_register_device_attr(struct tty_port *port,
+		struct tty_driver *driver, unsigned index,
+		struct device *device, void *drvdata,
+		const struct attribute_group **attr_grp)
+{
+	tty_port_link_device(port, driver, index);
+	return tty_register_device_attr(driver, index, device, drvdata,
+			attr_grp);
+}
+EXPORT_SYMBOL_GPL(tty_port_register_device_attr);
+
 int tty_port_alloc_xmit_buf(struct tty_port *port)
 {
 	/* We may sleep in get_zeroed_page() */
@@ -230,7 +294,7 @@ int tty_port_block_til_ready(struct tty_port *port,
 
 	/* block if port is in the process of being closed */
 	if (tty_hung_up_p(filp) || port->flags & ASYNC_CLOSING) {
-		wait_event_interruptible_tty(port->close_wait,
+		wait_event_interruptible_tty(tty, port->close_wait,
 				!(port->flags & ASYNC_CLOSING));
 		if (port->flags & ASYNC_HUP_NOTIFY)
 			return -EAGAIN;
@@ -246,7 +310,7 @@ int tty_port_block_til_ready(struct tty_port *port,
 	}
 	if (filp->f_flags & O_NONBLOCK) {
 		/* Indicate we are open */
-		if (tty->termios->c_cflag & CBAUD)
+		if (tty->termios.c_cflag & CBAUD)
 			tty_port_raise_dtr_rts(port);
 		port->flags |= ASYNC_NORMAL_ACTIVE;
 		return 0;
@@ -270,7 +334,7 @@ int tty_port_block_til_ready(struct tty_port *port,
 
 	while (1) {
 		/* Indicate we are open */
-		if (tty->termios->c_cflag & CBAUD)
+		if (tty->termios.c_cflag & CBAUD)
 			tty_port_raise_dtr_rts(port);
 
 		prepare_to_wait(&port->open_wait, &wait, TASK_INTERRUPTIBLE);
@@ -296,9 +360,9 @@ int tty_port_block_til_ready(struct tty_port *port,
 			retval = -ERESTARTSYS;
 			break;
 		}
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	finish_wait(&port->open_wait, &wait);
 
@@ -369,7 +433,7 @@ int tty_port_close_start(struct tty_port *port,
 
 	/* Drop DTR/RTS if HUPCL is set. This causes any attached modem to
 	   hang up the line */
-	if (tty->termios->c_cflag & HUPCL)
+	if (tty->termios.c_cflag & HUPCL)
 		tty_port_lower_dtr_rts(port);
 
 	/* Don't call port->drop for the last reference. Callers will want
@@ -413,6 +477,24 @@ void tty_port_close(struct tty_port *port, struct tty_struct *tty,
 }
 EXPORT_SYMBOL(tty_port_close);
 
+/**
+ * tty_port_install - generic tty->ops->install handler
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @tty: tty to be installed
+ *
+ * It is the same as tty_standard_install except the provided @port is linked
+ * to a concrete tty specified by @tty. Use this or tty_port_register_device
+ * (or both). Call tty_port_link_device as a last resort.
+ */
+int tty_port_install(struct tty_port *port, struct tty_driver *driver,
+		struct tty_struct *tty)
+{
+	tty->port = port;
+	return tty_standard_install(driver, tty);
+}
+EXPORT_SYMBOL_GPL(tty_port_install);
+
 int tty_port_open(struct tty_port *port, struct tty_struct *tty,
 							struct file *filp)
 {
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index 48cc6f25cfd3..681765baef69 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -119,6 +119,7 @@ static const int NR_TYPES = ARRAY_SIZE(max_vals);
 
 static struct input_handler kbd_handler;
 static DEFINE_SPINLOCK(kbd_event_lock);
+static DEFINE_SPINLOCK(led_lock);
 static unsigned long key_down[BITS_TO_LONGS(KEY_CNT)];	/* keyboard key bitmap */
 static unsigned char shift_down[NR_SHIFT];		/* shift state counters.. */
 static bool dead_key_next;
@@ -310,7 +311,7 @@ static void put_queue(struct vc_data *vc, int ch)
 
 	if (tty) {
 		tty_insert_flip_char(tty, ch, 0);
-		con_schedule_flip(tty);
+		tty_schedule_flip(tty);
 	}
 }
 
@@ -325,7 +326,7 @@ static void puts_queue(struct vc_data *vc, char *cp)
 		tty_insert_flip_char(tty, *cp, 0);
 		cp++;
 	}
-	con_schedule_flip(tty);
+	tty_schedule_flip(tty);
 }
 
 static void applkey(struct vc_data *vc, int key, char mode)
@@ -586,7 +587,7 @@ static void fn_send_intr(struct vc_data *vc)
 	if (!tty)
 		return;
 	tty_insert_flip_char(tty, 0, TTY_BREAK);
-	con_schedule_flip(tty);
+	tty_schedule_flip(tty);
 }
 
 static void fn_scroll_forw(struct vc_data *vc)
@@ -984,7 +985,7 @@ static void k_brl(struct vc_data *vc, unsigned char value, char up_flag)
  * or (ii) whatever pattern of lights people want to show using KDSETLED,
  * or (iii) specified bits of specified words in kernel memory.
  */
-unsigned char getledstate(void)
+static unsigned char getledstate(void)
 {
 	return ledstate;
 }
@@ -992,7 +993,7 @@ unsigned char getledstate(void)
 void setledstate(struct kbd_struct *kbd, unsigned int led)
 {
         unsigned long flags;
-        spin_lock_irqsave(&kbd_event_lock, flags);
+        spin_lock_irqsave(&led_lock, flags);
 	if (!(led & ~7)) {
 		ledioctl = led;
 		kbd->ledmode = LED_SHOW_IOCTL;
@@ -1000,7 +1001,7 @@ void setledstate(struct kbd_struct *kbd, unsigned int led)
 		kbd->ledmode = LED_SHOW_FLAGS;
 
 	set_leds();
-	spin_unlock_irqrestore(&kbd_event_lock, flags);
+	spin_unlock_irqrestore(&led_lock, flags);
 }
 
 static inline unsigned char getleds(void)
@@ -1049,13 +1050,13 @@ static int kbd_update_leds_helper(struct input_handle *handle, void *data)
  */
 int vt_get_leds(int console, int flag)
 {
-	unsigned long flags;
 	struct kbd_struct * kbd = kbd_table + console;
 	int ret;
+	unsigned long flags;
 
-	spin_lock_irqsave(&kbd_event_lock, flags);
+	spin_lock_irqsave(&led_lock, flags);
 	ret = vc_kbd_led(kbd, flag);
-	spin_unlock_irqrestore(&kbd_event_lock, flags);
+	spin_unlock_irqrestore(&led_lock, flags);
 
 	return ret;
 }
@@ -1091,11 +1092,11 @@ void vt_set_led_state(int console, int leds)
 void vt_kbd_con_start(int console)
 {
 	struct kbd_struct * kbd = kbd_table + console;
-/*	unsigned long flags; */
-/*	spin_lock_irqsave(&kbd_event_lock, flags); */
+	unsigned long flags;
+	spin_lock_irqsave(&led_lock, flags);
 	clr_vc_kbd_led(kbd, VC_SCROLLOCK);
 	set_leds();
-/*	spin_unlock_irqrestore(&kbd_event_lock, flags); */
+	spin_unlock_irqrestore(&led_lock, flags);
 }
 
 /**
@@ -1104,21 +1105,15 @@ void vt_kbd_con_start(int console)
  *
  *	Handle console stop. This is a wrapper for the VT layer
  *	so that we can keep kbd knowledge internal
- *
- *	FIXME: We eventually need to hold the kbd lock here to protect
- *	the LED updating. We can't do it yet because fn_hold calls stop_tty
- *	and start_tty under the kbd_event_lock, while normal tty paths
- *	don't hold the lock. We probably need to split out an LED lock
- *	but not during an -rc release!
  */
 void vt_kbd_con_stop(int console)
 {
 	struct kbd_struct * kbd = kbd_table + console;
-/*	unsigned long flags; */
-/*	spin_lock_irqsave(&kbd_event_lock, flags); */
+	unsigned long flags;
+	spin_lock_irqsave(&led_lock, flags);
 	set_vc_kbd_led(kbd, VC_SCROLLOCK);
 	set_leds();
-/*	spin_unlock_irqrestore(&kbd_event_lock, flags); */
+	spin_unlock_irqrestore(&led_lock, flags);
 }
 
 /*
@@ -1130,7 +1125,12 @@ void vt_kbd_con_stop(int console)
  */
 static void kbd_bh(unsigned long dummy)
 {
-	unsigned char leds = getleds();
+	unsigned char leds;
+	unsigned long flags;
+	
+	spin_lock_irqsave(&led_lock, flags);
+	leds = getleds();
+	spin_unlock_irqrestore(&led_lock, flags);
 
 	if (leds != ledstate) {
 		input_handler_for_each_handle(&kbd_handler, &leds,
@@ -2035,11 +2035,11 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 			return -EPERM;
 		if (arg & ~0x77)
 			return -EINVAL;
-                spin_lock_irqsave(&kbd_event_lock, flags);
+                spin_lock_irqsave(&led_lock, flags);
 		kbd->ledflagstate = (arg & 7);
 		kbd->default_ledflagstate = ((arg >> 4) & 7);
 		set_leds();
-                spin_unlock_irqrestore(&kbd_event_lock, flags);
+                spin_unlock_irqrestore(&led_lock, flags);
 		return 0;
 
 	/* the ioctls below only set the lights, not the functions */
@@ -2134,8 +2134,10 @@ void vt_reset_keyboard(int console)
 	clr_vc_kbd_mode(kbd, VC_CRLF);
 	kbd->lockstate = 0;
 	kbd->slockstate = 0;
+	spin_lock(&led_lock);
 	kbd->ledmode = LED_SHOW_FLAGS;
 	kbd->ledflagstate = kbd->default_ledflagstate;
+	spin_unlock(&led_lock);
 	/* do not do set_leds here because this causes an endless tasklet loop
 	   when the keyboard hasn't been initialized yet */
 	spin_unlock_irqrestore(&kbd_event_lock, flags);
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 84cbf298c094..999ca63afdef 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -537,45 +537,27 @@ void complement_pos(struct vc_data *vc, int offset)
 
 static void insert_char(struct vc_data *vc, unsigned int nr)
 {
-	unsigned short *p, *q = (unsigned short *)vc->vc_pos;
+	unsigned short *p = (unsigned short *) vc->vc_pos;
 
-	p = q + vc->vc_cols - nr - vc->vc_x;
-	while (--p >= q)
-		scr_writew(scr_readw(p), p + nr);
-	scr_memsetw(q, vc->vc_video_erase_char, nr * 2);
+	scr_memmovew(p + nr, p, vc->vc_cols - vc->vc_x);
+	scr_memsetw(p, vc->vc_video_erase_char, nr * 2);
 	vc->vc_need_wrap = 0;
-	if (DO_UPDATE(vc)) {
-		unsigned short oldattr = vc->vc_attr;
-		vc->vc_sw->con_bmove(vc, vc->vc_y, vc->vc_x, vc->vc_y, vc->vc_x + nr, 1,
-				     vc->vc_cols - vc->vc_x - nr);
-		vc->vc_attr = vc->vc_video_erase_char >> 8;
-		while (nr--)
-			vc->vc_sw->con_putc(vc, vc->vc_video_erase_char, vc->vc_y, vc->vc_x + nr);
-		vc->vc_attr = oldattr;
-	}
+	if (DO_UPDATE(vc))
+		do_update_region(vc, (unsigned long) p,
+			(vc->vc_cols - vc->vc_x) / 2 + 1);
 }
 
 static void delete_char(struct vc_data *vc, unsigned int nr)
 {
-	unsigned int i = vc->vc_x;
-	unsigned short *p = (unsigned short *)vc->vc_pos;
+	unsigned short *p = (unsigned short *) vc->vc_pos;
 
-	while (++i <= vc->vc_cols - nr) {
-		scr_writew(scr_readw(p+nr), p);
-		p++;
-	}
-	scr_memsetw(p, vc->vc_video_erase_char, nr * 2);
+	scr_memcpyw(p, p + nr, vc->vc_cols - vc->vc_x - nr);
+	scr_memsetw(p + vc->vc_cols - vc->vc_x - nr, vc->vc_video_erase_char,
+			nr * 2);
 	vc->vc_need_wrap = 0;
-	if (DO_UPDATE(vc)) {
-		unsigned short oldattr = vc->vc_attr;
-		vc->vc_sw->con_bmove(vc, vc->vc_y, vc->vc_x + nr, vc->vc_y, vc->vc_x, 1,
-				     vc->vc_cols - vc->vc_x - nr);
-		vc->vc_attr = vc->vc_video_erase_char >> 8;
-		while (nr--)
-			vc->vc_sw->con_putc(vc, vc->vc_video_erase_char, vc->vc_y,
-				     vc->vc_cols - 1 - nr);
-		vc->vc_attr = oldattr;
-	}
+	if (DO_UPDATE(vc))
+		do_update_region(vc, (unsigned long) p,
+			(vc->vc_cols - vc->vc_x) / 2);
 }
 
 static int softcursor_original;
@@ -1172,45 +1154,26 @@ static void csi_J(struct vc_data *vc, int vpar)
 		case 0:	/* erase from cursor to end of display */
 			count = (vc->vc_scr_end - vc->vc_pos) >> 1;
 			start = (unsigned short *)vc->vc_pos;
-			if (DO_UPDATE(vc)) {
-				/* do in two stages */
-				vc->vc_sw->con_clear(vc, vc->vc_y, vc->vc_x, 1,
-					      vc->vc_cols - vc->vc_x);
-				vc->vc_sw->con_clear(vc, vc->vc_y + 1, 0,
-					      vc->vc_rows - vc->vc_y - 1,
-					      vc->vc_cols);
-			}
 			break;
 		case 1:	/* erase from start to cursor */
 			count = ((vc->vc_pos - vc->vc_origin) >> 1) + 1;
 			start = (unsigned short *)vc->vc_origin;
-			if (DO_UPDATE(vc)) {
-				/* do in two stages */
-				vc->vc_sw->con_clear(vc, 0, 0, vc->vc_y,
-					      vc->vc_cols);
-				vc->vc_sw->con_clear(vc, vc->vc_y, 0, 1,
-					      vc->vc_x + 1);
-			}
 			break;
 		case 3: /* erase scroll-back buffer (and whole display) */
 			scr_memsetw(vc->vc_screenbuf, vc->vc_video_erase_char,
 				    vc->vc_screenbuf_size >> 1);
 			set_origin(vc);
-			if (CON_IS_VISIBLE(vc))
-				update_screen(vc);
 			/* fall through */
 		case 2: /* erase whole display */
 			count = vc->vc_cols * vc->vc_rows;
 			start = (unsigned short *)vc->vc_origin;
-			if (DO_UPDATE(vc))
-				vc->vc_sw->con_clear(vc, 0, 0,
-					      vc->vc_rows,
-					      vc->vc_cols);
 			break;
 		default:
 			return;
 	}
 	scr_memsetw(start, vc->vc_video_erase_char, 2 * count);
+	if (DO_UPDATE(vc))
+		do_update_region(vc, (unsigned long) start, count);
 	vc->vc_need_wrap = 0;
 }
 
@@ -1223,29 +1186,22 @@ static void csi_K(struct vc_data *vc, int vpar)
 		case 0:	/* erase from cursor to end of line */
 			count = vc->vc_cols - vc->vc_x;
 			start = (unsigned short *)vc->vc_pos;
-			if (DO_UPDATE(vc))
-				vc->vc_sw->con_clear(vc, vc->vc_y, vc->vc_x, 1,
-						     vc->vc_cols - vc->vc_x);
 			break;
 		case 1:	/* erase from start of line to cursor */
 			start = (unsigned short *)(vc->vc_pos - (vc->vc_x << 1));
 			count = vc->vc_x + 1;
-			if (DO_UPDATE(vc))
-				vc->vc_sw->con_clear(vc, vc->vc_y, 0, 1,
-						     vc->vc_x + 1);
 			break;
 		case 2: /* erase whole line */
 			start = (unsigned short *)(vc->vc_pos - (vc->vc_x << 1));
 			count = vc->vc_cols;
-			if (DO_UPDATE(vc))
-				vc->vc_sw->con_clear(vc, vc->vc_y, 0, 1,
-					      vc->vc_cols);
 			break;
 		default:
 			return;
 	}
 	scr_memsetw(start, vc->vc_video_erase_char, 2 * count);
 	vc->vc_need_wrap = 0;
+	if (DO_UPDATE(vc))
+		do_update_region(vc, (unsigned long) start, count);
 }
 
 static void csi_X(struct vc_data *vc, int vpar) /* erase the following vpar positions */
@@ -1380,7 +1336,7 @@ static void respond_string(const char *p, struct tty_struct *tty)
 		tty_insert_flip_char(tty, *p, 0);
 		p++;
 	}
-	con_schedule_flip(tty);
+	tty_schedule_flip(tty);
 }
 
 static void cursor_report(struct vc_data *vc, struct tty_struct *tty)
@@ -2792,41 +2748,52 @@ static void con_flush_chars(struct tty_struct *tty)
 /*
  * Allocate the console screen memory.
  */
-static int con_open(struct tty_struct *tty, struct file *filp)
+static int con_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	unsigned int currcons = tty->index;
-	int ret = 0;
+	struct vc_data *vc;
+	int ret;
 
 	console_lock();
-	if (tty->driver_data == NULL) {
-		ret = vc_allocate(currcons);
-		if (ret == 0) {
-			struct vc_data *vc = vc_cons[currcons].d;
+	ret = vc_allocate(currcons);
+	if (ret)
+		goto unlock;
 
-			/* Still being freed */
-			if (vc->port.tty) {
-				console_unlock();
-				return -ERESTARTSYS;
-			}
-			tty->driver_data = vc;
-			vc->port.tty = tty;
+	vc = vc_cons[currcons].d;
 
-			if (!tty->winsize.ws_row && !tty->winsize.ws_col) {
-				tty->winsize.ws_row = vc_cons[currcons].d->vc_rows;
-				tty->winsize.ws_col = vc_cons[currcons].d->vc_cols;
-			}
-			if (vc->vc_utf)
-				tty->termios->c_iflag |= IUTF8;
-			else
-				tty->termios->c_iflag &= ~IUTF8;
-			console_unlock();
-			return ret;
-		}
+	/* Still being freed */
+	if (vc->port.tty) {
+		ret = -ERESTARTSYS;
+		goto unlock;
 	}
+
+	ret = tty_port_install(&vc->port, driver, tty);
+	if (ret)
+		goto unlock;
+
+	tty->driver_data = vc;
+	vc->port.tty = tty;
+
+	if (!tty->winsize.ws_row && !tty->winsize.ws_col) {
+		tty->winsize.ws_row = vc_cons[currcons].d->vc_rows;
+		tty->winsize.ws_col = vc_cons[currcons].d->vc_cols;
+	}
+	if (vc->vc_utf)
+		tty->termios.c_iflag |= IUTF8;
+	else
+		tty->termios.c_iflag &= ~IUTF8;
+unlock:
 	console_unlock();
 	return ret;
 }
 
+static int con_open(struct tty_struct *tty, struct file *filp)
+{
+	/* everything done in install */
+	return 0;
+}
+
+
 static void con_close(struct tty_struct *tty, struct file *filp)
 {
 	/* Nothing to do - we defer to shutdown */
@@ -2839,7 +2806,6 @@ static void con_shutdown(struct tty_struct *tty)
 	console_lock();
 	vc->port.tty = NULL;
 	console_unlock();
-	tty_shutdown(tty);
 }
 
 static int default_italic_color    = 2; // green (ASCII)
@@ -2947,6 +2913,7 @@ static int __init con_init(void)
 console_initcall(con_init);
 
 static const struct tty_operations con_ops = {
+	.install = con_install,
 	.open = con_open,
 	.close = con_close,
 	.write = con_write,
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 344a51d3c291..36f2be4def2f 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -825,7 +825,7 @@ static void acm_tty_set_termios(struct tty_struct *tty,
 						struct ktermios *termios_old)
 {
 	struct acm *acm = tty->driver_data;
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	struct usb_cdc_line_coding newline;
 	int newctrl = acm->ctrlout;
 
@@ -1298,7 +1298,8 @@ skip_countries:
 	usb_set_intfdata(data_interface, acm);
 
 	usb_get_intf(control_interface);
-	tty_register_device(acm_tty_driver, minor, &control_interface->dev);
+	tty_port_register_device(&acm->port, acm_tty_driver, minor,
+			&control_interface->dev);
 
 	return 0;
 alloc_fail7:
diff --git a/drivers/usb/gadget/u_serial.c b/drivers/usb/gadget/u_serial.c
index da6d479ff9a6..f1739526820f 100644
--- a/drivers/usb/gadget/u_serial.c
+++ b/drivers/usb/gadget/u_serial.c
@@ -1133,7 +1133,8 @@ int gserial_setup(struct usb_gadget *g, unsigned count)
 	for (i = 0; i < count; i++) {
 		struct device	*tty_dev;
 
-		tty_dev = tty_register_device(gs_tty_driver, i, &g->dev);
+		tty_dev = tty_port_register_device(&ports[i].port->port,
+				gs_tty_driver, i, &g->dev);
 		if (IS_ERR(tty_dev))
 			pr_warning("%s: no classdev for port %d, err %ld\n",
 				__func__, i, PTR_ERR(tty_dev));
diff --git a/drivers/usb/serial/ark3116.c b/drivers/usb/serial/ark3116.c
index d154501867a3..cf2522c397d3 100644
--- a/drivers/usb/serial/ark3116.c
+++ b/drivers/usb/serial/ark3116.c
@@ -214,7 +214,7 @@ static void ark3116_release(struct usb_serial *serial)
 
 static void ark3116_init_termios(struct tty_struct *tty)
 {
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	*termios = tty_std_termios;
 	termios->c_cflag = B9600 | CS8
 				      | CREAD | HUPCL | CLOCAL;
@@ -228,7 +228,7 @@ static void ark3116_set_termios(struct tty_struct *tty,
 {
 	struct usb_serial *serial = port->serial;
 	struct ark3116_private *priv = usb_get_serial_port_data(port);
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	unsigned int cflag = termios->c_cflag;
 	int bps = tty_get_baud_rate(tty);
 	int quot;
diff --git a/drivers/usb/serial/belkin_sa.c b/drivers/usb/serial/belkin_sa.c
index 06af24d8744d..99449424193f 100644
--- a/drivers/usb/serial/belkin_sa.c
+++ b/drivers/usb/serial/belkin_sa.c
@@ -304,7 +304,7 @@ static void belkin_sa_set_termios(struct tty_struct *tty,
 	unsigned long control_state;
 	int bad_flow_control;
 	speed_t baud;
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 
 	iflag = termios->c_iflag;
 	cflag = termios->c_cflag;
diff --git a/drivers/usb/serial/console.c b/drivers/usb/serial/console.c
index 6ea18a1ea1a7..5f3bcd31e204 100644
--- a/drivers/usb/serial/console.c
+++ b/drivers/usb/serial/console.c
@@ -162,8 +162,8 @@ static int usb_console_setup(struct console *co, char *options)
 		}
 
 		if (serial->type->set_termios) {
-			tty->termios->c_cflag = cflag;
-			tty_termios_encode_baud_rate(tty->termios, baud, baud);
+			tty->termios.c_cflag = cflag;
+			tty_termios_encode_baud_rate(&tty->termios, baud, baud);
 			memset(&dummy, 0, sizeof(struct ktermios));
 			serial->type->set_termios(tty, port, &dummy);
 
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 697c7d9316ac..28af5acc3360 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -465,7 +465,7 @@ static void cp210x_get_termios(struct tty_struct *tty,
 
 	if (tty) {
 		cp210x_get_termios_port(tty->driver_data,
-			&tty->termios->c_cflag, &baud);
+			&tty->termios.c_cflag, &baud);
 		tty_encode_baud_rate(tty, baud, baud);
 	}
 
@@ -625,7 +625,7 @@ static void cp210x_change_speed(struct tty_struct *tty,
 {
 	u32 baud;
 
-	baud = tty->termios->c_ospeed;
+	baud = tty->termios.c_ospeed;
 
 	/* This maps the requested rate to a rate valid on cp2102 or cp2103,
 	 * or to an arbitrary rate in [1M,2M].
@@ -660,10 +660,10 @@ static void cp210x_set_termios(struct tty_struct *tty,
 	if (!tty)
 		return;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 	old_cflag = old_termios->c_cflag;
 
-	if (tty->termios->c_ospeed != old_termios->c_ospeed)
+	if (tty->termios.c_ospeed != old_termios->c_ospeed)
 		cp210x_change_speed(tty, port, old_termios);
 
 	/* If the number of data bits is to be updated */
diff --git a/drivers/usb/serial/cypress_m8.c b/drivers/usb/serial/cypress_m8.c
index 3f13cdd159d4..1befce21e173 100644
--- a/drivers/usb/serial/cypress_m8.c
+++ b/drivers/usb/serial/cypress_m8.c
@@ -924,38 +924,38 @@ static void cypress_set_termios(struct tty_struct *tty,
 	   early enough */
 	if (!priv->termios_initialized) {
 		if (priv->chiptype == CT_EARTHMATE) {
-			*(tty->termios) = tty_std_termios;
-			tty->termios->c_cflag = B4800 | CS8 | CREAD | HUPCL |
+			tty->termios = tty_std_termios;
+			tty->termios.c_cflag = B4800 | CS8 | CREAD | HUPCL |
 				CLOCAL;
-			tty->termios->c_ispeed = 4800;
-			tty->termios->c_ospeed = 4800;
+			tty->termios.c_ispeed = 4800;
+			tty->termios.c_ospeed = 4800;
 		} else if (priv->chiptype == CT_CYPHIDCOM) {
-			*(tty->termios) = tty_std_termios;
-			tty->termios->c_cflag = B9600 | CS8 | CREAD | HUPCL |
+			tty->termios = tty_std_termios;
+			tty->termios.c_cflag = B9600 | CS8 | CREAD | HUPCL |
 				CLOCAL;
-			tty->termios->c_ispeed = 9600;
-			tty->termios->c_ospeed = 9600;
+			tty->termios.c_ispeed = 9600;
+			tty->termios.c_ospeed = 9600;
 		} else if (priv->chiptype == CT_CA42V2) {
-			*(tty->termios) = tty_std_termios;
-			tty->termios->c_cflag = B9600 | CS8 | CREAD | HUPCL |
+			tty->termios = tty_std_termios;
+			tty->termios.c_cflag = B9600 | CS8 | CREAD | HUPCL |
 				CLOCAL;
-			tty->termios->c_ispeed = 9600;
-			tty->termios->c_ospeed = 9600;
+			tty->termios.c_ispeed = 9600;
+			tty->termios.c_ospeed = 9600;
 		}
 		priv->termios_initialized = 1;
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	/* Unsupported features need clearing */
-	tty->termios->c_cflag &= ~(CMSPAR|CRTSCTS);
+	tty->termios.c_cflag &= ~(CMSPAR|CRTSCTS);
 
-	cflag = tty->termios->c_cflag;
-	iflag = tty->termios->c_iflag;
+	cflag = tty->termios.c_cflag;
+	iflag = tty->termios.c_iflag;
 
 	/* check if there are new settings */
 	if (old_termios) {
 		spin_lock_irqsave(&priv->lock, flags);
-		priv->tmp_termios = *(tty->termios);
+		priv->tmp_termios = tty->termios;
 		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
@@ -1020,7 +1020,7 @@ static void cypress_set_termios(struct tty_struct *tty,
 		dev_dbg(dev, "Using custom termios settings for a baud rate of 4800bps.\n");
 		/* define custom termios settings for NMEA protocol */
 
-		tty->termios->c_iflag /* input modes - */
+		tty->termios.c_iflag /* input modes - */
 			&= ~(IGNBRK  /* disable ignore break */
 			| BRKINT     /* disable break causes interrupt */
 			| PARMRK     /* disable mark parity errors */
@@ -1030,10 +1030,10 @@ static void cypress_set_termios(struct tty_struct *tty,
 			| ICRNL      /* disable translate CR to NL */
 			| IXON);     /* disable enable XON/XOFF flow control */
 
-		tty->termios->c_oflag /* output modes */
+		tty->termios.c_oflag /* output modes */
 			&= ~OPOST;    /* disable postprocess output char */
 
-		tty->termios->c_lflag /* line discipline modes */
+		tty->termios.c_lflag /* line discipline modes */
 			&= ~(ECHO     /* disable echo input characters */
 			| ECHONL      /* disable echo new line */
 			| ICANON      /* disable erase, kill, werase, and rprnt
@@ -1199,7 +1199,7 @@ static void cypress_read_int_callback(struct urb *urb)
 
 	/* hangup, as defined in acm.c... this might be a bad place for it
 	 * though */
-	if (tty && !(tty->termios->c_cflag & CLOCAL) &&
+	if (tty && !(tty->termios.c_cflag & CLOCAL) &&
 			!(priv->current_status & UART_CD)) {
 		dev_dbg(dev, "%s - calling hangup\n", __func__);
 		tty_hangup(tty);
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index d3a8941f66bc..c86f68c6b078 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -686,8 +686,8 @@ static void digi_set_termios(struct tty_struct *tty,
 {
 	struct digi_port *priv = usb_get_serial_port_data(port);
 	struct device *dev = &port->dev;
-	unsigned int iflag = tty->termios->c_iflag;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int iflag = tty->termios.c_iflag;
+	unsigned int cflag = tty->termios.c_cflag;
 	unsigned int old_iflag = old_termios->c_iflag;
 	unsigned int old_cflag = old_termios->c_cflag;
 	unsigned char buf[32];
@@ -710,7 +710,7 @@ static void digi_set_termios(struct tty_struct *tty,
 			/* don't set RTS if using hardware flow control */
 			/* and throttling input */
 			modem_signals = TIOCM_DTR;
-			if (!(tty->termios->c_cflag & CRTSCTS) ||
+			if (!(tty->termios.c_cflag & CRTSCTS) ||
 			    !test_bit(TTY_THROTTLED, &tty->flags))
 				modem_signals |= TIOCM_RTS;
 			digi_set_modem_signals(port, modem_signals, 1);
@@ -749,7 +749,7 @@ static void digi_set_termios(struct tty_struct *tty,
 		}
 	}
 	/* set parity */
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	if ((cflag&(PARENB|PARODD)) != (old_cflag&(PARENB|PARODD))) {
 		if (cflag&PARENB) {
@@ -1127,8 +1127,8 @@ static int digi_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	/* set termios settings */
 	if (tty) {
-		not_termios.c_cflag = ~tty->termios->c_cflag;
-		not_termios.c_iflag = ~tty->termios->c_iflag;
+		not_termios.c_cflag = ~tty->termios.c_cflag;
+		not_termios.c_iflag = ~tty->termios.c_iflag;
 		digi_set_termios(tty, port, &not_termios);
 	}
 	return 0;
@@ -1503,7 +1503,7 @@ static int digi_read_oob_callback(struct urb *urb)
 
 		rts = 0;
 		if (tty)
-			rts = tty->termios->c_cflag & CRTSCTS;
+			rts = tty->termios.c_cflag & CRTSCTS;
 		
 		if (tty && opcode == DIGI_CMD_READ_INPUT_SIGNALS) {
 			spin_lock(&priv->dp_port_lock);
diff --git a/drivers/usb/serial/empeg.c b/drivers/usb/serial/empeg.c
index 4d9935f58ad5..43ede4a1e12c 100644
--- a/drivers/usb/serial/empeg.c
+++ b/drivers/usb/serial/empeg.c
@@ -85,7 +85,7 @@ static int empeg_startup(struct usb_serial *serial)
 
 static void empeg_init_termios(struct tty_struct *tty)
 {
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 
 	/*
 	 * The empeg-car player wants these particular tty settings.
diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index c5710335803d..244477107e2f 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -171,10 +171,11 @@ static void f81232_set_termios(struct tty_struct *tty,
 	/* FIXME - Stubbed out for now */
 
 	/* Don't change anything if nothing has changed */
-	if (!tty_termios_hw_change(tty->termios, old_termios))
+	if (!tty_termios_hw_change(&tty->termios, old_termios))
 		return;
 
 	/* Do the real work here... */
+	tty_termios_copy_hw(&tty->termios, old_termios);
 }
 
 static int f81232_tiocmget(struct tty_struct *tty)
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 9169f51777ef..be845873e23d 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -2099,7 +2099,7 @@ static void ftdi_set_termios(struct tty_struct *tty,
 	struct usb_device *dev = port->serial->dev;
 	struct device *ddev = &port->dev;
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	unsigned int cflag = termios->c_cflag;
 	__u16 urb_value; /* will hold the new flags */
 
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index 75b7ccdd2652..8e6faaf3580c 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -1441,7 +1441,7 @@ static void edge_throttle(struct tty_struct *tty)
 	}
 
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		edge_port->shadowMCR &= ~MCR_RTS;
 		status = send_cmd_write_uart_register(edge_port, MCR,
 							edge_port->shadowMCR);
@@ -1478,7 +1478,7 @@ static void edge_unthrottle(struct tty_struct *tty)
 			return;
 	}
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		edge_port->shadowMCR |= MCR_RTS;
 		send_cmd_write_uart_register(edge_port, MCR,
 						edge_port->shadowMCR);
@@ -1497,8 +1497,8 @@ static void edge_set_termios(struct tty_struct *tty,
 	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
 	unsigned int cflag;
 
-	cflag = tty->termios->c_cflag;
-	dev_dbg(&port->dev, "%s - clfag %08x iflag %08x\n", __func__, tty->termios->c_cflag, tty->termios->c_iflag);
+	cflag = tty->termios.c_cflag;
+	dev_dbg(&port->dev, "%s - clfag %08x iflag %08x\n", __func__, tty->termios.c_cflag, tty->termios.c_iflag);
 	dev_dbg(&port->dev, "%s - old clfag %08x old iflag %08x\n", __func__, old_termios->c_cflag, old_termios->c_iflag);
 
 	if (edge_port == NULL)
@@ -1958,7 +1958,7 @@ static void process_rcvd_status(struct edgeport_serial *edge_serial,
 		tty = tty_port_tty_get(&edge_port->port->port);
 		if (tty) {
 			change_port_settings(tty,
-				edge_port, tty->termios);
+				edge_port, &tty->termios);
 			tty_kref_put(tty);
 		}
 
@@ -2523,7 +2523,7 @@ static void change_port_settings(struct tty_struct *tty,
 		return;
 	}
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	switch (cflag & CSIZE) {
 	case CS5:
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 21c7efa57acf..a2209cd45093 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -1835,7 +1835,7 @@ static int edge_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	/* set up the port settings */
 	if (tty)
-		edge_set_termios(tty, port, tty->termios);
+		edge_set_termios(tty, port, &tty->termios);
 
 	/* open up the port */
 
@@ -2218,12 +2218,12 @@ static void change_port_settings(struct tty_struct *tty,
 
 	config = kmalloc (sizeof (*config), GFP_KERNEL);
 	if (!config) {
-		*tty->termios = *old_termios;
+		tty->termios = *old_termios;
 		dev_err(dev, "%s - out of memory\n", __func__);
 		return;
 	}
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	config->wFlags = 0;
 
@@ -2307,7 +2307,7 @@ static void change_port_settings(struct tty_struct *tty,
 	} else
 		dev_dbg(dev, "%s - OUTBOUND XON/XOFF is disabled\n", __func__);
 
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	/* Round the baud rate */
 	baud = tty_get_baud_rate(tty);
@@ -2352,10 +2352,10 @@ static void edge_set_termios(struct tty_struct *tty,
 	struct edgeport_port *edge_port = usb_get_serial_port_data(port);
 	unsigned int cflag;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	dev_dbg(&port->dev, "%s - clfag %08x iflag %08x\n", __func__,
-		tty->termios->c_cflag, tty->termios->c_iflag);
+		tty->termios.c_cflag, tty->termios.c_iflag);
 	dev_dbg(&port->dev, "%s - old clfag %08x old iflag %08x\n", __func__,
 		old_termios->c_cflag, old_termios->c_iflag);
 	dev_dbg(&port->dev, "%s - port %d\n", __func__, port->number);
diff --git a/drivers/usb/serial/ir-usb.c b/drivers/usb/serial/ir-usb.c
index 4ac3a3a50cfb..e24e2d4f4c1b 100644
--- a/drivers/usb/serial/ir-usb.c
+++ b/drivers/usb/serial/ir-usb.c
@@ -375,7 +375,7 @@ static void ir_set_termios(struct tty_struct *tty,
 		ir_xbof = ir_xbof_change(xbof) ;
 
 	/* Only speed changes are supported */
-	tty_termios_copy_hw(tty->termios, old_termios);
+	tty_termios_copy_hw(&tty->termios, old_termios);
 	tty_encode_baud_rate(tty, baud, baud);
 
 	/*
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index 91e6e372cf17..01da3ea36e89 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -905,7 +905,7 @@ static void iuu_set_termios(struct tty_struct *tty,
 {
 	const u32 supported_mask = CMSPAR|PARENB|PARODD;
 	struct iuu_private *priv = usb_get_serial_port_data(port);
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 	int status;
 	u32 actual;
 	u32 parity;
@@ -914,7 +914,7 @@ static void iuu_set_termios(struct tty_struct *tty,
 	u32 newval = cflag & supported_mask;
 
 	/* Just use the ospeed. ispeed should be the same. */
-	baud = tty->termios->c_ospeed;
+	baud = tty->termios.c_ospeed;
 
 	dev_dbg(&port->dev, "%s - enter c_ospeed or baud=%d\n", __func__, baud);
 
@@ -945,13 +945,13 @@ static void iuu_set_termios(struct tty_struct *tty,
 	 * settings back over and then adjust them
 	 */
 	if (old_termios)
-		tty_termios_copy_hw(tty->termios, old_termios);
+		tty_termios_copy_hw(&tty->termios, old_termios);
 	if (status != 0)	/* Set failed - return old bits */
 		return;
 	/* Re-encode speed, parity and csize */
 	tty_encode_baud_rate(tty, baud, baud);
-	tty->termios->c_cflag &= ~(supported_mask|CSIZE);
-	tty->termios->c_cflag |= newval | csize;
+	tty->termios.c_cflag &= ~(supported_mask|CSIZE);
+	tty->termios.c_cflag |= newval | csize;
 }
 
 static void iuu_close(struct usb_serial_port *port)
@@ -977,14 +977,14 @@ static void iuu_close(struct usb_serial_port *port)
 
 static void iuu_init_termios(struct tty_struct *tty)
 {
-	*(tty->termios) = tty_std_termios;
-	tty->termios->c_cflag = CLOCAL | CREAD | CS8 | B9600
+	tty->termios = tty_std_termios;
+	tty->termios.c_cflag = CLOCAL | CREAD | CS8 | B9600
 				| TIOCM_CTS | CSTOPB | PARENB;
-	tty->termios->c_ispeed = 9600;
-	tty->termios->c_ospeed = 9600;
-	tty->termios->c_lflag = 0;
-	tty->termios->c_oflag = 0;
-	tty->termios->c_iflag = 0;
+	tty->termios.c_ispeed = 9600;
+	tty->termios.c_ospeed = 9600;
+	tty->termios.c_lflag = 0;
+	tty->termios.c_oflag = 0;
+	tty->termios.c_iflag = 0;
 }
 
 static int iuu_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -997,8 +997,8 @@ static int iuu_open(struct tty_struct *tty, struct usb_serial_port *port)
 	u32 actual;
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 
-	baud = tty->termios->c_ospeed;
-	tty->termios->c_ispeed = baud;
+	baud = tty->termios.c_ospeed;
+	tty->termios.c_ispeed = baud;
 	/* Re-encode speed */
 	tty_encode_baud_rate(tty, baud, baud);
 
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index 0acb07131f6e..29c943d737d0 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -155,7 +155,7 @@ static void keyspan_set_termios(struct tty_struct *tty,
 
 	p_priv = usb_get_serial_port_data(port);
 	d_details = p_priv->device_details;
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 	device_port = port->number - port->serial->minor;
 
 	/* Baud rate calculation takes baud rate as an integer
@@ -176,7 +176,7 @@ static void keyspan_set_termios(struct tty_struct *tty,
 	p_priv->flow_control = (cflag & CRTSCTS) ? flow_cts : flow_none;
 
 	/* Mark/Space not supported */
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	keyspan_send_setup(port, 0);
 }
@@ -1089,7 +1089,7 @@ static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	device_port = port->number - port->serial->minor;
 	if (tty) {
-		cflag = tty->termios->c_cflag;
+		cflag = tty->termios.c_cflag;
 		/* Baud rate calculation takes baud rate as an integer
 		   so other rates can be generated if desired. */
 		baud_rate = tty_get_baud_rate(tty);
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index e1cada31356e..ca43ecb4a2bd 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -333,7 +333,7 @@ static void keyspan_pda_set_termios(struct tty_struct *tty,
 	   7[EOMS]1: 10 bit, b0/b7 is parity
 	   7[EOMS]2: 11 bit, b0/b7 is parity, extra bit always (mark?)
 
-	   HW flow control is dictated by the tty->termios->c_cflags & CRTSCTS
+	   HW flow control is dictated by the tty->termios.c_cflags & CRTSCTS
 	   bit.
 
 	   For now, just do baud. */
@@ -348,7 +348,7 @@ static void keyspan_pda_set_termios(struct tty_struct *tty,
 	}
 	/* Only speed can change so copy the old h/w parameters
 	   then encode the new speed */
-	tty_termios_copy_hw(tty->termios, old_termios);
+	tty_termios_copy_hw(&tty->termios, old_termios);
 	tty_encode_baud_rate(tty, speed, speed);
 }
 
diff --git a/drivers/usb/serial/kl5kusb105.c b/drivers/usb/serial/kl5kusb105.c
index e4aa6c0632d7..3f6d7376c02d 100644
--- a/drivers/usb/serial/kl5kusb105.c
+++ b/drivers/usb/serial/kl5kusb105.c
@@ -311,12 +311,12 @@ static int  klsi_105_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	/* set up termios structure */
 	spin_lock_irqsave(&priv->lock, flags);
-	priv->termios.c_iflag = tty->termios->c_iflag;
-	priv->termios.c_oflag = tty->termios->c_oflag;
-	priv->termios.c_cflag = tty->termios->c_cflag;
-	priv->termios.c_lflag = tty->termios->c_lflag;
+	priv->termios.c_iflag = tty->termios.c_iflag;
+	priv->termios.c_oflag = tty->termios.c_oflag;
+	priv->termios.c_cflag = tty->termios.c_cflag;
+	priv->termios.c_lflag = tty->termios.c_lflag;
 	for (i = 0; i < NCCS; i++)
-		priv->termios.c_cc[i] = tty->termios->c_cc[i];
+		priv->termios.c_cc[i] = tty->termios.c_cc[i];
 	priv->cfg.pktlen   = cfg->pktlen;
 	priv->cfg.baudrate = cfg->baudrate;
 	priv->cfg.databits = cfg->databits;
@@ -446,9 +446,9 @@ static void klsi_105_set_termios(struct tty_struct *tty,
 {
 	struct klsi_105_private *priv = usb_get_serial_port_data(port);
 	struct device *dev = &port->dev;
-	unsigned int iflag = tty->termios->c_iflag;
+	unsigned int iflag = tty->termios.c_iflag;
 	unsigned int old_iflag = old_termios->c_iflag;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 	unsigned int old_cflag = old_termios->c_cflag;
 	struct klsi_105_port_settings *cfg;
 	unsigned long flags;
@@ -558,7 +558,7 @@ static void klsi_105_set_termios(struct tty_struct *tty,
 	if ((cflag & (PARENB|PARODD)) != (old_cflag & (PARENB|PARODD))
 	    || (cflag & CSTOPB) != (old_cflag & CSTOPB)) {
 		/* Not currently supported */
-		tty->termios->c_cflag &= ~(PARENB|PARODD|CSTOPB);
+		tty->termios.c_cflag &= ~(PARENB|PARODD|CSTOPB);
 #if 0
 		priv->last_lcr = 0;
 
@@ -585,7 +585,7 @@ static void klsi_105_set_termios(struct tty_struct *tty,
 	    || (iflag & IXON) != (old_iflag & IXON)
 	    ||  (cflag & CRTSCTS) != (old_cflag & CRTSCTS)) {
 		/* Not currently supported */
-		tty->termios->c_cflag &= ~CRTSCTS;
+		tty->termios.c_cflag &= ~CRTSCTS;
 		/* Drop DTR/RTS if no flow control otherwise assert */
 #if 0
 		if ((iflag & IXOFF) || (iflag & IXON) || (cflag & CRTSCTS))
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index ca62fccccf32..5c4d2fbd4e11 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -190,11 +190,11 @@ static void kobil_release(struct usb_serial *serial)
 static void kobil_init_termios(struct tty_struct *tty)
 {
 	/* Default to echo off and other sane device settings */
-	tty->termios->c_lflag = 0;
-	tty->termios->c_lflag &= ~(ISIG | ICANON | ECHO | IEXTEN | XCASE);
-	tty->termios->c_iflag = IGNBRK | IGNPAR | IXOFF;
+	tty->termios.c_lflag = 0;
+	tty->termios.c_iflag &= ~(ISIG | ICANON | ECHO | IEXTEN | XCASE);
+	tty->termios.c_iflag |= IGNBRK | IGNPAR | IXOFF;
 	/* do NOT translate CR to CR-NL (0x0A -> 0x0A 0x0D) */
-	tty->termios->c_oflag &= ~ONLCR;
+	tty->termios.c_oflag &= ~ONLCR;
 }
 
 static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
@@ -565,14 +565,14 @@ static void kobil_set_termios(struct tty_struct *tty,
 	struct kobil_private *priv;
 	int result;
 	unsigned short urb_val = 0;
-	int c_cflag = tty->termios->c_cflag;
+	int c_cflag = tty->termios.c_cflag;
 	speed_t speed;
 
 	priv = usb_get_serial_port_data(port);
 	if (priv->device_type == KOBIL_USBTWIN_PRODUCT_ID ||
 			priv->device_type == KOBIL_KAAN_SIM_PRODUCT_ID) {
 		/* This device doesn't support ioctl calls */
-		*tty->termios = *old;
+		tty_termios_copy_hw(&tty->termios, old);
 		return;
 	}
 
@@ -596,7 +596,7 @@ static void kobil_set_termios(struct tty_struct *tty,
 			urb_val |= SUSBCR_SPASB_EvenParity;
 	} else
 		urb_val |= SUSBCR_SPASB_NoParity;
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 	tty_encode_baud_rate(tty, speed, speed);
 
 	result = usb_control_msg(port->serial->dev,
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index 8042582de0e1..f3947712e137 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -450,7 +450,7 @@ static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 	 * either.
 	 */
 	spin_lock_irqsave(&priv->lock, flags);
-	if (tty && (tty->termios->c_cflag & CBAUD))
+	if (tty && (tty->termios.c_cflag & CBAUD))
 		priv->control_state = TIOCM_DTR | TIOCM_RTS;
 	else
 		priv->control_state = 0;
@@ -623,7 +623,7 @@ static void mct_u232_set_termios(struct tty_struct *tty,
 {
 	struct usb_serial *serial = port->serial;
 	struct mct_u232_private *priv = usb_get_serial_port_data(port);
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	unsigned int cflag = termios->c_cflag;
 	unsigned int old_cflag = old_termios->c_cflag;
 	unsigned long flags;
diff --git a/drivers/usb/serial/metro-usb.c b/drivers/usb/serial/metro-usb.c
index ed61b53df78b..0b257ddffbdb 100644
--- a/drivers/usb/serial/metro-usb.c
+++ b/drivers/usb/serial/metro-usb.c
@@ -127,12 +127,6 @@ static void metrousb_read_int_callback(struct urb *urb)
 
 	/* Set the data read from the usb port into the serial port buffer. */
 	tty = tty_port_tty_get(&port->port);
-	if (!tty) {
-		dev_err(&port->dev, "%s - bad tty pointer - exiting\n",
-			__func__);
-		return;
-	}
-
 	if (tty && urb->actual_length) {
 		/* Loop through the data copying each byte to the tty layer. */
 		tty_insert_flip_string(tty, data, urb->actual_length);
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index af7bc9b6280c..1bf1ad066666 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -1313,7 +1313,7 @@ static void mos7720_throttle(struct tty_struct *tty)
 	}
 
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		mos7720_port->shadowMCR &= ~UART_MCR_RTS;
 		write_mos_reg(port->serial, port->number - port->serial->minor,
 			      MCR, mos7720_port->shadowMCR);
@@ -1345,7 +1345,7 @@ static void mos7720_unthrottle(struct tty_struct *tty)
 	}
 
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		mos7720_port->shadowMCR |= UART_MCR_RTS;
 		write_mos_reg(port->serial, port->number - port->serial->minor,
 			      MCR, mos7720_port->shadowMCR);
@@ -1560,8 +1560,8 @@ static void change_port_settings(struct tty_struct *tty,
 	lStop = 0x00;	/* 1 stop bit */
 	lParity = 0x00;	/* No parity */
 
-	cflag = tty->termios->c_cflag;
-	iflag = tty->termios->c_iflag;
+	cflag = tty->termios.c_cflag;
+	iflag = tty->termios.c_iflag;
 
 	/* Change the number of bits */
 	switch (cflag & CSIZE) {
@@ -1708,10 +1708,10 @@ static void mos7720_set_termios(struct tty_struct *tty,
 
 	dev_dbg(&port->dev, "setting termios - ASPIRE\n");
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	dev_dbg(&port->dev, "%s - cflag %08x iflag %08x\n", __func__,
-		tty->termios->c_cflag, RELEVANT_IFLAG(tty->termios->c_iflag));
+		tty->termios.c_cflag, RELEVANT_IFLAG(tty->termios.c_iflag));
 
 	dev_dbg(&port->dev, "%s - old cflag %08x old iflag %08x\n", __func__,
 		old_termios->c_cflag, RELEVANT_IFLAG(old_termios->c_iflag));
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index a94297d9c7ac..d6d4eeca8c68 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -1591,7 +1591,7 @@ static void mos7840_throttle(struct tty_struct *tty)
 			return;
 	}
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		mos7840_port->shadowMCR &= ~MCR_RTS;
 		status = mos7840_set_uart_reg(port, MODEM_CONTROL_REGISTER,
 					 mos7840_port->shadowMCR);
@@ -1632,7 +1632,7 @@ static void mos7840_unthrottle(struct tty_struct *tty)
 	}
 
 	/* if we are implementing RTS/CTS, toggle that line */
-	if (tty->termios->c_cflag & CRTSCTS) {
+	if (tty->termios.c_cflag & CRTSCTS) {
 		mos7840_port->shadowMCR |= MCR_RTS;
 		status = mos7840_set_uart_reg(port, MODEM_CONTROL_REGISTER,
 					 mos7840_port->shadowMCR);
@@ -1929,8 +1929,8 @@ static void mos7840_change_port_settings(struct tty_struct *tty,
 	lStop = LCR_STOP_1;
 	lParity = LCR_PAR_NONE;
 
-	cflag = tty->termios->c_cflag;
-	iflag = tty->termios->c_iflag;
+	cflag = tty->termios.c_cflag;
+	iflag = tty->termios.c_iflag;
 
 	/* Change the number of bits */
 	if (cflag & CSIZE) {
@@ -2086,10 +2086,10 @@ static void mos7840_set_termios(struct tty_struct *tty,
 
 	dev_dbg(&port->dev, "%s", "setting termios - \n");
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	dev_dbg(&port->dev, "%s - clfag %08x iflag %08x\n", __func__,
-		tty->termios->c_cflag, RELEVANT_IFLAG(tty->termios->c_iflag));
+		tty->termios.c_cflag, RELEVANT_IFLAG(tty->termios.c_iflag));
 	dev_dbg(&port->dev, "%s - old clfag %08x old iflag %08x\n", __func__,
 		old_termios->c_cflag, RELEVANT_IFLAG(old_termios->c_iflag));
 	dev_dbg(&port->dev, "%s - port %d\n", __func__, port->number);
diff --git a/drivers/usb/serial/oti6858.c b/drivers/usb/serial/oti6858.c
index 1498b3ddfead..933241f03fd8 100644
--- a/drivers/usb/serial/oti6858.c
+++ b/drivers/usb/serial/oti6858.c
@@ -402,10 +402,10 @@ static int oti6858_chars_in_buffer(struct tty_struct *tty)
 
 static void oti6858_init_termios(struct tty_struct *tty)
 {
-	*(tty->termios) = tty_std_termios;
-	tty->termios->c_cflag = B38400 | CS8 | CREAD | HUPCL | CLOCAL;
-	tty->termios->c_ispeed = 38400;
-	tty->termios->c_ospeed = 38400;
+	tty->termios = tty_std_termios;
+	tty->termios.c_cflag = B38400 | CS8 | CREAD | HUPCL | CLOCAL;
+	tty->termios.c_ispeed = 38400;
+	tty->termios.c_ospeed = 38400;
 }
 
 static void oti6858_set_termios(struct tty_struct *tty,
@@ -421,7 +421,7 @@ static void oti6858_set_termios(struct tty_struct *tty,
 	if (!tty)
 		return;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	spin_lock_irqsave(&priv->lock, flags);
 	divisor = priv->pending_setup.divisor;
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index ef10a9c6ce46..892ebdc7a364 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -258,16 +258,16 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	   serial settings even to the same values as before. Thus
 	   we actually need to filter in this specific case */
 
-	if (!tty_termios_hw_change(tty->termios, old_termios))
+	if (!tty_termios_hw_change(&tty->termios, old_termios))
 		return;
 
-	cflag = tty->termios->c_cflag;
+	cflag = tty->termios.c_cflag;
 
 	buf = kzalloc(7, GFP_KERNEL);
 	if (!buf) {
 		dev_err(&port->dev, "%s - out of memory.\n", __func__);
 		/* Report back no change occurred */
-		*tty->termios = *old_termios;
+		tty->termios = *old_termios;
 		return;
 	}
 
diff --git a/drivers/usb/serial/quatech2.c b/drivers/usb/serial/quatech2.c
index c1024b168bba..2cdfdcc90b37 100644
--- a/drivers/usb/serial/quatech2.c
+++ b/drivers/usb/serial/quatech2.c
@@ -273,7 +273,7 @@ static void qt2_set_termios(struct tty_struct *tty,
 {
 	struct usb_device *dev = port->serial->dev;
 	struct qt2_port_private *port_priv;
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	u16 baud;
 	unsigned int cflag = termios->c_cflag;
 	u16 new_lcr = 0;
@@ -404,7 +404,7 @@ static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
 	port_priv->device_port = (u8) device_port;
 
 	if (tty)
-		qt2_set_termios(tty, port, tty->termios);
+		qt2_set_termios(tty, port, &tty->termios);
 
 	return 0;
 
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 4ef30ac5e049..01d882cf3775 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -381,7 +381,7 @@ static int sierra_send_setup(struct usb_serial_port *port)
 static void sierra_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
-	tty_termios_copy_hw(tty->termios, old_termios);
+	tty_termios_copy_hw(&tty->termios, old_termios);
 	sierra_send_setup(port);
 }
 
diff --git a/drivers/usb/serial/spcp8x5.c b/drivers/usb/serial/spcp8x5.c
index 1ca86e63b537..9716efe92955 100644
--- a/drivers/usb/serial/spcp8x5.c
+++ b/drivers/usb/serial/spcp8x5.c
@@ -314,10 +314,10 @@ static void spcp8x5_dtr_rts(struct usb_serial_port *port, int on)
 static void spcp8x5_init_termios(struct tty_struct *tty)
 {
 	/* for the 1st time call this function */
-	*(tty->termios) = tty_std_termios;
-	tty->termios->c_cflag = B115200 | CS8 | CREAD | HUPCL | CLOCAL;
-	tty->termios->c_ispeed = 115200;
-	tty->termios->c_ospeed = 115200;
+	tty->termios = tty_std_termios;
+	tty->termios.c_cflag = B115200 | CS8 | CREAD | HUPCL | CLOCAL;
+	tty->termios.c_ispeed = 115200;
+	tty->termios.c_ospeed = 115200;
 }
 
 /* set the serial param for transfer. we should check if we really need to
@@ -328,7 +328,7 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 	struct usb_serial *serial = port->serial;
 	struct spcp8x5_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 	unsigned int old_cflag = old_termios->c_cflag;
 	unsigned short uartdata;
 	unsigned char buf[2] = {0, 0};
@@ -338,7 +338,7 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 
 
 	/* check that they really want us to change something */
-	if (!tty_termios_hw_change(tty->termios, old_termios))
+	if (!tty_termios_hw_change(&tty->termios, old_termios))
 		return;
 
 	/* set DTR/RTS active */
diff --git a/drivers/usb/serial/ssu100.c b/drivers/usb/serial/ssu100.c
index 6635743bd8c2..015810b3785b 100644
--- a/drivers/usb/serial/ssu100.c
+++ b/drivers/usb/serial/ssu100.c
@@ -214,7 +214,7 @@ static void ssu100_set_termios(struct tty_struct *tty,
 			       struct ktermios *old_termios)
 {
 	struct usb_device *dev = port->serial->dev;
-	struct ktermios *termios = tty->termios;
+	struct ktermios *termios = &tty->termios;
 	u16 baud, divisor, remainder;
 	unsigned int cflag = termios->c_cflag;
 	u16 urb_value = 0; /* will hold the new flags */
@@ -320,7 +320,7 @@ static int ssu100_open(struct tty_struct *tty, struct usb_serial_port *port)
 		dev_dbg(&port->dev, "%s - set uart failed\n", __func__);
 
 	if (tty)
-		ssu100_set_termios(tty, port, tty->termios);
+		ssu100_set_termios(tty, port, &tty->termios);
 
 	return usb_serial_generic_open(tty, port);
 }
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index 648249e74125..6f49392cda5b 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -507,7 +507,7 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 	}
 
 	if (tty)
-		ti_set_termios(tty, port, tty->termios);
+		ti_set_termios(tty, port, &tty->termios);
 
 	dev_dbg(&port->dev, "%s - sending TI_OPEN_PORT\n", __func__);
 	status = ti_command_out_sync(tdev, TI_OPEN_PORT,
@@ -549,7 +549,7 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 	usb_clear_halt(dev, port->read_urb->pipe);
 
 	if (tty)
-		ti_set_termios(tty, port, tty->termios);
+		ti_set_termios(tty, port, &tty->termios);
 
 	dev_dbg(&port->dev, "%s - sending TI_OPEN_PORT (2)\n", __func__);
 	status = ti_command_out_sync(tdev, TI_OPEN_PORT,
@@ -817,8 +817,8 @@ static void ti_set_termios(struct tty_struct *tty,
 	int port_number = port->number - port->serial->minor;
 	unsigned int mcr;
 
-	cflag = tty->termios->c_cflag;
-	iflag = tty->termios->c_iflag;
+	cflag = tty->termios.c_cflag;
+	iflag = tty->termios.c_iflag;
 
 	dev_dbg(&port->dev, "%s - cflag %08x, iflag %08x\n", __func__, cflag, iflag);
 	dev_dbg(&port->dev, "%s - old clfag %08x, old iflag %08x\n", __func__,
@@ -857,7 +857,7 @@ static void ti_set_termios(struct tty_struct *tty,
 	}
 
 	/* CMSPAR isn't supported by this driver */
-	tty->termios->c_cflag &= ~CMSPAR;
+	tty->termios.c_cflag &= ~CMSPAR;
 
 	if (cflag & PARENB) {
 		if (cflag & PARODD) {
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 483919bbda6f..73b8e0569164 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -199,7 +199,7 @@ static int serial_install(struct tty_driver *driver, struct tty_struct *tty)
 	if (retval)
 		goto error_get_interface;
 
-	retval = tty_standard_install(driver, tty);
+	retval = tty_port_install(&port->port, driver, tty);
 	if (retval)
 		goto error_init_termios;
 
@@ -299,8 +299,7 @@ static void serial_close(struct tty_struct *tty, struct file *filp)
  * Do the resource freeing and refcount dropping for the port.
  * Avoid freeing the console.
  *
- * Called asynchronously after the last tty kref is dropped,
- * and the tty layer has already done the tty_shutdown(tty);
+ * Called asynchronously after the last tty kref is dropped.
  */
 static void serial_cleanup(struct tty_struct *tty)
 {
@@ -424,7 +423,7 @@ static void serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 	if (port->serial->type->set_termios)
 		port->serial->type->set_termios(tty, port, old);
 	else
-		tty_termios_copy_hw(tty->termios, old);
+		tty_termios_copy_hw(&tty->termios, old);
 }
 
 static int serial_break(struct tty_struct *tty, int break_state)
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index bc58d4a0cee5..e42aa398ed37 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -65,7 +65,7 @@ void usb_wwan_set_termios(struct tty_struct *tty,
 	struct usb_wwan_intf_private *intfdata = port->serial->private;
 
 	/* Doesn't support option setting */
-	tty_termios_copy_hw(tty->termios, old_termios);
+	tty_termios_copy_hw(&tty->termios, old_termios);
 
 	if (intfdata->send_setup)
 		intfdata->send_setup(port);
diff --git a/drivers/usb/serial/whiteheat.c b/drivers/usb/serial/whiteheat.c
index efa32bf5f758..346c7efc20b0 100644
--- a/drivers/usb/serial/whiteheat.c
+++ b/drivers/usb/serial/whiteheat.c
@@ -653,7 +653,7 @@ static void firm_setup_port(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct device *dev = &port->dev;
 	struct whiteheat_port_settings port_settings;
-	unsigned int cflag = tty->termios->c_cflag;
+	unsigned int cflag = tty->termios.c_cflag;
 
 	port_settings.port = port->number + 1;
 
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index 211a4920b88a..d8dedc7d3910 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -76,9 +76,24 @@ static int virqfd_wakeup(wait_queue_t *wait, unsigned mode, int sync, void *key)
 			schedule_work(&virqfd->inject);
 	}
 
-	if (flags & POLLHUP)
-		/* The eventfd is closing, detach from VFIO */
-		virqfd_deactivate(virqfd);
+	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;
 }
@@ -93,7 +108,6 @@ static void virqfd_ptable_queue_proc(struct file *file,
 static void virqfd_shutdown(struct work_struct *work)
 {
 	struct virqfd *virqfd = container_of(work, struct virqfd, shutdown);
-	struct virqfd **pvirqfd = virqfd->pvirqfd;
 	u64 cnt;
 
 	eventfd_ctx_remove_wait_queue(virqfd->eventfd, &virqfd->wait, &cnt);
@@ -101,7 +115,6 @@ static void virqfd_shutdown(struct work_struct *work)
 	eventfd_ctx_put(virqfd->eventfd);
 
 	kfree(virqfd);
-	*pvirqfd = NULL;
 }
 
 static void virqfd_inject(struct work_struct *work)
@@ -122,15 +135,11 @@ static int virqfd_enable(struct vfio_pci_device *vdev,
 	int ret = 0;
 	unsigned int events;
 
-	if (*pvirqfd)
-		return -EBUSY;
-
 	virqfd = kzalloc(sizeof(*virqfd), GFP_KERNEL);
 	if (!virqfd)
 		return -ENOMEM;
 
 	virqfd->pvirqfd = pvirqfd;
-	*pvirqfd = virqfd;
 	virqfd->vdev = vdev;
 	virqfd->handler = handler;
 	virqfd->thread = thread;
@@ -154,6 +163,23 @@ static int virqfd_enable(struct vfio_pci_device *vdev,
 	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 fail;
+	}
+	*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.
 	 */
@@ -187,19 +213,29 @@ fail:
 		fput(file);
 
 	kfree(virqfd);
-	*pvirqfd = NULL;
 
 	return ret;
 }
 
-static void virqfd_disable(struct virqfd *virqfd)
+static void virqfd_disable(struct vfio_pci_device *vdev,
+			   struct virqfd **pvirqfd)
 {
-	if (!virqfd)
-		return;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vdev->irqlock, flags);
+
+	if (*pvirqfd) {
+		virqfd_deactivate(*pvirqfd);
+		*pvirqfd = NULL;
+	}
 
-	virqfd_deactivate(virqfd);
+	spin_unlock_irqrestore(&vdev->irqlock, flags);
 
-	/* Block until we know all outstanding shutdown jobs have completed. */
+	/*
+	 * 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);
 }
 
@@ -392,8 +428,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->ctx[0].unmask);
-	virqfd_disable(vdev->ctx[0].mask);
+	virqfd_disable(vdev, &vdev->ctx[0].unmask);
+	virqfd_disable(vdev, &vdev->ctx[0].mask);
 	vdev->irq_type = VFIO_PCI_NUM_IRQS;
 	vdev->num_ctx = 0;
 	kfree(vdev->ctx);
@@ -539,8 +575,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->ctx[i].unmask);
-		virqfd_disable(vdev->ctx[i].mask);
+		virqfd_disable(vdev, &vdev->ctx[i].unmask);
+		virqfd_disable(vdev, &vdev->ctx[i].mask);
 	}
 
 	if (msix) {
@@ -577,7 +613,7 @@ static int vfio_pci_set_intx_unmask(struct vfio_pci_device *vdev,
 					     vfio_send_intx_eventfd, NULL,
 					     &vdev->ctx[0].unmask, fd);
 
-		virqfd_disable(vdev->ctx[0].unmask);
+		virqfd_disable(vdev, &vdev->ctx[0].unmask);
 	}
 
 	return 0;
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index f75da8758adc..f49181c73113 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -228,7 +228,6 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 	data->port = pdata->flags;
 	if (data->port < 0) {
 		dev_err(&pdev->dev, "wrong platform data is assigned");
-		kfree(data);
 		return -EINVAL;
 	}
 
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index b4a632ada401..932abaa58a89 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -553,7 +553,9 @@ static int __init efifb_init(void)
 	int ret;
 	char *option = NULL;
 
-	dmi_check_system(dmi_system_table);
+	if (screen_info.orig_video_isVGA != VIDEO_TYPE_EFI ||
+	    !(screen_info.capabilities & VIDEO_CAPABILITY_SKIP_QUIRKS))
+		dmi_check_system(dmi_system_table);
 
 	if (screen_info.orig_video_isVGA != VIDEO_TYPE_EFI)
 		return -ENODEV;
diff --git a/drivers/video/exynos/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index 4bc2b8a5dd8b..663c308d0e73 100644
--- a/drivers/video/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -461,7 +461,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
 done:
 	platform_set_drvdata(pdev, dsim);
 
-	dev_dbg(&pdev->dev, "%s() completed sucessfuly (%s mode)\n", __func__,
+	dev_dbg(&pdev->dev, "%s() completed successfully (%s mode)\n", __func__,
 		dsim_config->e_interface == DSIM_COMMAND ? "CPU" : "RGB");
 
 	return 0;
diff --git a/drivers/video/tmiofb.c b/drivers/video/tmiofb.c
index 8e4a446b5ed1..b244f060f151 100644
--- a/drivers/video/tmiofb.c
+++ b/drivers/video/tmiofb.c
@@ -694,6 +694,10 @@ static int __devinit tmiofb_probe(struct platform_device *dev)
 		dev_err(&dev->dev, "NULL platform data!\n");
 		return -EINVAL;
 	}
+	if (ccr == NULL || lcr == NULL || vram == NULL || irq < 0) {
+		dev_err(&dev->dev, "missing resources\n");
+		return -EINVAL;
+	}
 
 	info = framebuffer_alloc(sizeof(struct tmiofb_par), &dev->dev);
 
diff --git a/drivers/vme/bridges/vme_ca91cx42.c b/drivers/vme/bridges/vme_ca91cx42.c
index e0df92ec44bd..1425d22cf956 100644
--- a/drivers/vme/bridges/vme_ca91cx42.c
+++ b/drivers/vme/bridges/vme_ca91cx42.c
@@ -1603,7 +1603,7 @@ static int ca91cx42_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	int retval, i;
 	u32 data;
-	struct list_head *pos = NULL;
+	struct list_head *pos = NULL, *n;
 	struct vme_bridge *ca91cx42_bridge;
 	struct ca91cx42_driver *ca91cx42_device;
 	struct vme_master_resource *master_image;
@@ -1821,28 +1821,28 @@ err_reg:
 	ca91cx42_crcsr_exit(ca91cx42_bridge, pdev);
 err_lm:
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->lm_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->lm_resources) {
 		lm = list_entry(pos, struct vme_lm_resource, list);
 		list_del(pos);
 		kfree(lm);
 	}
 err_dma:
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->dma_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->dma_resources) {
 		dma_ctrlr = list_entry(pos, struct vme_dma_resource, list);
 		list_del(pos);
 		kfree(dma_ctrlr);
 	}
 err_slave:
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->slave_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->slave_resources) {
 		slave_image = list_entry(pos, struct vme_slave_resource, list);
 		list_del(pos);
 		kfree(slave_image);
 	}
 err_master:
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->master_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->master_resources) {
 		master_image = list_entry(pos, struct vme_master_resource,
 			list);
 		list_del(pos);
@@ -1868,7 +1868,7 @@ err_struct:
 
 static void ca91cx42_remove(struct pci_dev *pdev)
 {
-	struct list_head *pos = NULL;
+	struct list_head *pos = NULL, *n;
 	struct vme_master_resource *master_image;
 	struct vme_slave_resource *slave_image;
 	struct vme_dma_resource *dma_ctrlr;
@@ -1905,28 +1905,28 @@ static void ca91cx42_remove(struct pci_dev *pdev)
 	ca91cx42_crcsr_exit(ca91cx42_bridge, pdev);
 
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->lm_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->lm_resources) {
 		lm = list_entry(pos, struct vme_lm_resource, list);
 		list_del(pos);
 		kfree(lm);
 	}
 
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->dma_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->dma_resources) {
 		dma_ctrlr = list_entry(pos, struct vme_dma_resource, list);
 		list_del(pos);
 		kfree(dma_ctrlr);
 	}
 
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->slave_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->slave_resources) {
 		slave_image = list_entry(pos, struct vme_slave_resource, list);
 		list_del(pos);
 		kfree(slave_image);
 	}
 
 	/* resources are stored in link list */
-	list_for_each(pos, &ca91cx42_bridge->master_resources) {
+	list_for_each_safe(pos, n, &ca91cx42_bridge->master_resources) {
 		master_image = list_entry(pos, struct vme_master_resource,
 			list);
 		list_del(pos);
diff --git a/drivers/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index 880d9242e349..5fbd08ffb9c2 100644
--- a/drivers/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -2350,7 +2350,7 @@ static int tsi148_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	int retval, i, master_num;
 	u32 data;
-	struct list_head *pos = NULL;
+	struct list_head *pos = NULL, *n;
 	struct vme_bridge *tsi148_bridge;
 	struct tsi148_driver *tsi148_device;
 	struct vme_master_resource *master_image;
@@ -2615,28 +2615,28 @@ err_reg:
 err_crcsr:
 err_lm:
 	/* resources are stored in link list */
-	list_for_each(pos, &tsi148_bridge->lm_resources) {
+	list_for_each_safe(pos, n, &tsi148_bridge->lm_resources) {
 		lm = list_entry(pos, struct vme_lm_resource, list);
 		list_del(pos);
 		kfree(lm);
 	}
 err_dma:
 	/* resources are stored in link list */
-	list_for_each(pos, &tsi148_bridge->dma_resources) {
+	list_for_each_safe(pos, n, &tsi148_bridge->dma_resources) {
 		dma_ctrlr = list_entry(pos, struct vme_dma_resource, list);
 		list_del(pos);
 		kfree(dma_ctrlr);
 	}
 err_slave:
 	/* resources are stored in link list */
-	list_for_each(pos, &tsi148_bridge->slave_resources) {
+	list_for_each_safe(pos, n, &tsi148_bridge->slave_resources) {
 		slave_image = list_entry(pos, struct vme_slave_resource, list);
 		list_del(pos);
 		kfree(slave_image);
 	}
 err_master:
 	/* resources are stored in link list */
-	list_for_each(pos, &tsi148_bridge->master_resources) {
+	list_for_each_safe(pos, n, &tsi148_bridge->master_resources) {
 		master_image = list_entry(pos, struct vme_master_resource,
 			list);
 		list_del(pos);
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index 5ceb1cd50195..7e984034a11b 100644
--- a/drivers/w1/masters/Kconfig
+++ b/drivers/w1/masters/Kconfig
@@ -60,7 +60,6 @@ config W1_MASTER_GPIO
 
 config HDQ_MASTER_OMAP
 	tristate "OMAP HDQ driver"
-	depends on ARCH_OMAP2PLUS
 	help
 	  Say Y here if you want support for the 1-wire or HDQ Interface
 	  on an OMAP processor.
diff --git a/drivers/w1/masters/ds1wm.c b/drivers/w1/masters/ds1wm.c
index 530a2d309063..7c294f4dc0ed 100644
--- a/drivers/w1/masters/ds1wm.c
+++ b/drivers/w1/masters/ds1wm.c
@@ -349,7 +349,7 @@ static void ds1wm_search(void *data, struct w1_master *master_dev,
 			"pass: %d entering ASM\n", pass);
 		ds1wm_write_register(ds1wm_data, DS1WM_CMD, DS1WM_CMD_SRA);
 		dev_dbg(&ds1wm_data->pdev->dev,
-			"pass: %d begining nibble loop\n", pass);
+			"pass: %d beginning nibble loop\n", pass);
 
 		r_prime = 0;
 		d = 0;
diff --git a/drivers/w1/masters/omap_hdq.c b/drivers/w1/masters/omap_hdq.c
index 4b0fcf3c2d03..ca8e60bb2f9c 100644
--- a/drivers/w1/masters/omap_hdq.c
+++ b/drivers/w1/masters/omap_hdq.c
@@ -18,9 +18,6 @@
 #include <linux/sched.h>
 #include <linux/pm_runtime.h>
 
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
 #include "../w1.h"
 #include "../w1_int.h"
 
@@ -73,11 +70,11 @@ struct hdq_data {
 };
 
 static int __devinit omap_hdq_probe(struct platform_device *pdev);
-static int omap_hdq_remove(struct platform_device *pdev);
+static int __devexit omap_hdq_remove(struct platform_device *pdev);
 
 static struct platform_driver omap_hdq_driver = {
 	.probe =	omap_hdq_probe,
-	.remove =	omap_hdq_remove,
+	.remove =	__devexit_p(omap_hdq_remove),
 	.driver =	{
 		.name =	"omap_hdq",
 	},
@@ -538,39 +535,35 @@ static void omap_w1_write_byte(void *_hdq, u8 byte)
 		hdq_data->init_trans = 0;
 		mutex_unlock(&hdq_data->hdq_mutex);
 	}
-
-	return;
 }
 
 static int __devinit omap_hdq_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct hdq_data *hdq_data;
 	struct resource *res;
 	int ret, irq;
 	u8 rev;
 
-	hdq_data = kmalloc(sizeof(*hdq_data), GFP_KERNEL);
+	hdq_data = devm_kzalloc(dev, sizeof(*hdq_data), GFP_KERNEL);
 	if (!hdq_data) {
 		dev_dbg(&pdev->dev, "unable to allocate memory\n");
-		ret = -ENOMEM;
-		goto err_kmalloc;
+		return -ENOMEM;
 	}
 
-	hdq_data->dev = &pdev->dev;
+	hdq_data->dev = dev;
 	platform_set_drvdata(pdev, hdq_data);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_dbg(&pdev->dev, "unable to get resource\n");
-		ret = -ENXIO;
-		goto err_resource;
+		return -ENXIO;
 	}
 
-	hdq_data->hdq_base = ioremap(res->start, SZ_4K);
+	hdq_data->hdq_base = devm_request_and_ioremap(dev, res);
 	if (!hdq_data->hdq_base) {
 		dev_dbg(&pdev->dev, "ioremap failed\n");
-		ret = -EINVAL;
-		goto err_ioremap;
+		return -ENOMEM;
 	}
 
 	hdq_data->hdq_usecount = 0;
@@ -591,7 +584,8 @@ static int __devinit omap_hdq_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
-	ret = request_irq(irq, hdq_isr, IRQF_DISABLED, "omap_hdq", hdq_data);
+	ret = devm_request_irq(dev, irq, hdq_isr, IRQF_DISABLED,
+			"omap_hdq", hdq_data);
 	if (ret < 0) {
 		dev_dbg(&pdev->dev, "could not request irq\n");
 		goto err_irq;
@@ -616,19 +610,10 @@ err_irq:
 err_w1:
 	pm_runtime_disable(&pdev->dev);
 
-	iounmap(hdq_data->hdq_base);
-
-err_ioremap:
-err_resource:
-	platform_set_drvdata(pdev, NULL);
-	kfree(hdq_data);
-
-err_kmalloc:
 	return ret;
-
 }
 
-static int omap_hdq_remove(struct platform_device *pdev)
+static int __devexit omap_hdq_remove(struct platform_device *pdev)
 {
 	struct hdq_data *hdq_data = platform_get_drvdata(pdev);
 
@@ -644,27 +629,11 @@ static int omap_hdq_remove(struct platform_device *pdev)
 
 	/* remove module dependency */
 	pm_runtime_disable(&pdev->dev);
-	free_irq(INT_24XX_HDQ_IRQ, hdq_data);
-	platform_set_drvdata(pdev, NULL);
-	iounmap(hdq_data->hdq_base);
-	kfree(hdq_data);
 
 	return 0;
 }
 
-static int __init
-omap_hdq_init(void)
-{
-	return platform_driver_register(&omap_hdq_driver);
-}
-module_init(omap_hdq_init);
-
-static void __exit
-omap_hdq_exit(void)
-{
-	platform_driver_unregister(&omap_hdq_driver);
-}
-module_exit(omap_hdq_exit);
+module_platform_driver(omap_hdq_driver);
 
 module_param(w1_id, int, S_IRUSR);
 MODULE_PARM_DESC(w1_id, "1-wire id for the slave detection");
diff --git a/drivers/w1/masters/w1-gpio.c b/drivers/w1/masters/w1-gpio.c
index df600d14974d..6012c4ea3206 100644
--- a/drivers/w1/masters/w1-gpio.c
+++ b/drivers/w1/masters/w1-gpio.c
@@ -14,6 +14,8 @@
 #include <linux/slab.h>
 #include <linux/w1-gpio.h>
 #include <linux/gpio.h>
+#include <linux/of_platform.h>
+#include <linux/of_gpio.h>
 
 #include "../w1.h"
 #include "../w1_int.h"
@@ -42,12 +44,55 @@ static u8 w1_gpio_read_bit(void *data)
 	return gpio_get_value(pdata->pin) ? 1 : 0;
 }
 
+#ifdef CONFIG_OF
+static struct of_device_id w1_gpio_dt_ids[] = {
+	{ .compatible = "w1-gpio" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, w1_gpio_dt_ids);
+
+static int w1_gpio_probe_dt(struct platform_device *pdev)
+{
+	struct w1_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *of_id =
+			of_match_device(w1_gpio_dt_ids, &pdev->dev);
+
+	if (!of_id)
+		return 0;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	if (of_get_property(np, "linux,open-drain", NULL))
+		pdata->is_open_drain = 1;
+
+	pdata->pin = of_get_gpio(np, 0);
+	pdata->ext_pullup_enable_pin = of_get_gpio(np, 1);
+	pdev->dev.platform_data = pdata;
+
+	return 0;
+}
+#else
+static int w1_gpio_probe_dt(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+
 static int __init w1_gpio_probe(struct platform_device *pdev)
 {
 	struct w1_bus_master *master;
-	struct w1_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct w1_gpio_platform_data *pdata;
 	int err;
 
+	err = w1_gpio_probe_dt(pdev);
+	if (err < 0)
+		return err;
+
+	pdata = pdev->dev.platform_data;
+
 	if (!pdata)
 		return -ENXIO;
 
@@ -59,6 +104,13 @@ static int __init w1_gpio_probe(struct platform_device *pdev)
 	if (err)
 		goto free_master;
 
+	if (gpio_is_valid(pdata->ext_pullup_enable_pin)) {
+		err = gpio_request_one(pdata->ext_pullup_enable_pin,
+				       GPIOF_INIT_LOW, "w1 pullup");
+		if (err < 0)
+			goto free_gpio;
+	}
+
 	master->data = pdata;
 	master->read_bit = w1_gpio_read_bit;
 
@@ -72,15 +124,21 @@ static int __init w1_gpio_probe(struct platform_device *pdev)
 
 	err = w1_add_master_device(master);
 	if (err)
-		goto free_gpio;
+		goto free_gpio_ext_pu;
 
 	if (pdata->enable_external_pullup)
 		pdata->enable_external_pullup(1);
 
+	if (gpio_is_valid(pdata->ext_pullup_enable_pin))
+		gpio_set_value(pdata->ext_pullup_enable_pin, 1);
+
 	platform_set_drvdata(pdev, master);
 
 	return 0;
 
+ free_gpio_ext_pu:
+	if (gpio_is_valid(pdata->ext_pullup_enable_pin))
+		gpio_free(pdata->ext_pullup_enable_pin);
  free_gpio:
 	gpio_free(pdata->pin);
  free_master:
@@ -97,6 +155,9 @@ static int __exit w1_gpio_remove(struct platform_device *pdev)
 	if (pdata->enable_external_pullup)
 		pdata->enable_external_pullup(0);
 
+	if (gpio_is_valid(pdata->ext_pullup_enable_pin))
+		gpio_set_value(pdata->ext_pullup_enable_pin, 0);
+
 	w1_remove_master_device(master);
 	gpio_free(pdata->pin);
 	kfree(master);
@@ -135,6 +196,7 @@ static struct platform_driver w1_gpio_driver = {
 	.driver = {
 		.name	= "w1-gpio",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(w1_gpio_dt_ids),
 	},
 	.remove	= __exit_p(w1_gpio_remove),
 	.suspend = w1_gpio_suspend,
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index 1eff743ec497..ae60406ea8a1 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -814,6 +814,9 @@ static int __devinit hpwdt_init_one(struct pci_dev *dev,
 	hpwdt_timer_reg = pci_mem_addr + 0x70;
 	hpwdt_timer_con = pci_mem_addr + 0x72;
 
+	/* Make sure that timer is disabled until /dev/watchdog is opened */
+	hpwdt_stop();
+
 	/* Make sure that we have a valid soft_margin */
 	if (hpwdt_change_timer(soft_margin))
 		hpwdt_change_timer(DEFAULT_MARGIN);
diff --git a/drivers/watchdog/watchdog_core.c b/drivers/watchdog/watchdog_core.c
index 6aa46a90ff02..3796434991fa 100644
--- a/drivers/watchdog/watchdog_core.c
+++ b/drivers/watchdog/watchdog_core.c
@@ -128,11 +128,12 @@ EXPORT_SYMBOL_GPL(watchdog_register_device);
 void watchdog_unregister_device(struct watchdog_device *wdd)
 {
 	int ret;
-	int devno = wdd->cdev.dev;
+	int devno;
 
 	if (wdd == NULL)
 		return;
 
+	devno = wdd->cdev.dev;
 	ret = watchdog_dev_unregister(wdd);
 	if (ret)
 		pr_err("error unregistering /dev/watchdog (err=%d)\n", ret);
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 1ffd03bf8e10..7f1241608489 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -314,8 +314,9 @@ static int __unmap_grant_pages(struct grant_map *map, int offset, int pages)
 		}
 	}
 
-	err = gnttab_unmap_refs(map->unmap_ops + offset, map->pages + offset,
-				pages, true);
+	err = gnttab_unmap_refs(map->unmap_ops + offset,
+			use_ptemod ? map->kmap_ops + offset : NULL, map->pages + offset,
+			pages);
 	if (err)
 		return err;
 
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index 0bfc1ef11259..006726688baf 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -870,7 +870,8 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 EXPORT_SYMBOL_GPL(gnttab_map_refs);
 
 int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
-		      struct page **pages, unsigned int count, bool clear_pte)
+		      struct gnttab_map_grant_ref *kmap_ops,
+		      struct page **pages, unsigned int count)
 {
 	int i, ret;
 	bool lazy = false;
@@ -888,7 +889,8 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 	}
 
 	for (i = 0; i < count; i++) {
-		ret = m2p_remove_override(pages[i], clear_pte);
+		ret = m2p_remove_override(pages[i], kmap_ops ?
+				       &kmap_ops[i] : NULL);
 		if (ret)
 			return ret;
 	}
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 03342728bf23..92ff01dbeb10 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -871,7 +871,7 @@ end:
 }
 
 /*add xen_pcibk AER handling*/
-static struct pci_error_handlers xen_pcibk_error_handler = {
+static const struct pci_error_handlers xen_pcibk_error_handler = {
 	.error_detected = xen_pcibk_error_detected,
 	.mmio_enabled = xen_pcibk_mmio_enabled,
 	.slot_reset = xen_pcibk_slot_reset,